added cablights

RC excavator/FW/src/controller.cpp

@@ -7,6 +7,7 @@ uint16_t joymax = 0;
 uint16_t trgmax = 0;
 #define deadband 20000
 #define trgdeadband 200
+bool cablights = false;
 
 direction getdirectionJoy(int setpoint)
 {
@@ -58,6 +59,22 @@ void setup_controller()
   Serial.printf("Waiting for controller");
 }
 
+void handleCabLights(uint8_t button)
+{
+  if(!button)  return;
+  
+  if (cablights)
+  {
+    digitalWrite(cabLights, HIGH);
+    cablights = true;
+  }
+  else
+  {
+    digitalWrite(cabLights, LOW);
+    cablights = false;
+  }
+}
+
   void loop_controller()
   {
     xboxController.onLoop();
@@ -93,8 +110,7 @@ void loop_controller()
 
         controlMotor("TrackLeft", getTrackDirection(xboxController.xboxNotif.trigLT, xboxController.xboxNotif.btnLB));
         controlMotor("TrackRight", getTrackDirection(xboxController.xboxNotif.trigRT, xboxController.xboxNotif.btnRB));
-
-
+        handleCabLights(xboxController.xboxNotif.btnShare);
       }
     }
     else
@@ -103,6 +119,7 @@ void loop_controller()
       {
         log_w("not connected");
         lastcontrollernotify = millis();
+        handleCabLights(1);
       }
       if (xboxController.getCountFailedConnection() > 10)
       {