added code to control brakelight

src/main.cpp

@@ -1,9 +1,11 @@
 #include <Arduino.h>
 #include <FlexCAN_T4.h>
 
-//Function Declaration
+/* Brakelight config */
-void sendEnable(); // function to turn the inverter on DON'T FORGET TO EDIT THE ID OF THE DRIVE
+#define PIN_BRAKELIGHT  6    /* Digital output used to enable the brakelight */
-void sendERPM(); // function to change the speed DON'T FORGET TO EDIT THE ID OF THE DRIVE
+#define BRAKELIGHT_ON   0b1
+#define BRAKELIGHT_OFF  0b0
+#define BPPS_THRESHOLD  10   /* Threshold that BPPS needs to have to be considered as braking */
 
 /* Invertor config */
 #define MC_L_ID 0x4F        /* ID of the left invertor, to be used in CAN communication */
@@ -391,9 +393,31 @@ void update_drives() {
 	can_mc_send_erpm(erpm, erpm);
 }
 
+/* BRAKELIGHT */
+void setup_brakelight() {
+	/* Init IO */
+	pinMode(PIN_BRAKELIGHT, OUTPUT);
+	digitalWrite(PIN_BRAKELIGHT, BRAKELIGHT_ON);
+	delay(1000);  /* To allow visual check that brakelight is working */
+}
+
+void update_brakelight() {
+	/* Update IO */
+	char output = (bpps_value > BPPS_THRESHOLD) ? BRAKELIGHT_ON : BRAKELIGHT_OFF;
+
+	/* When fault: blink brakelight */
+	if (tc_state != TS_OK) {
+		output = (millis() & 0xFF < 50) ? BRAKELIGHT_ON : BRAKELIGHT_OFF;
+	}
+
+	digitalWrite(PIN_BRAKELIGHT, output);
+}
+
+/* MAIN */
 void setup() {
-	/* Init CAN */
+	/* Setup all required stuff */
-	can_setup();
+	can_setup();         /* Init CAN */
+	setup_brakelight();  /* Setup brakelight obviously */
 }
 
 void loop() {
@@ -403,7 +427,5 @@ void loop() {
 	check_heartbeat();    /* Do heartbeat stuffz */
 	analyse_drive_data(); /* Analyse the data from the drives duh */
 	update_drives();      /* Do motorcontroller shit */
-
-	/* TODO: check drive faults */
 }