diff --git a/RC excavator/FW/src/controller.cpp b/RC excavator/FW/src/controller.cpp index b3141a3..cae850c 100644 --- a/RC excavator/FW/src/controller.cpp +++ b/RC excavator/FW/src/controller.cpp @@ -7,42 +7,43 @@ uint16_t joymax = 0; uint16_t trgmax = 0; #define deadband 20000 #define trgdeadband 200 +bool cablights = false; direction getdirectionJoy(int setpoint) { - if(setpoint > (joymax/2 +deadband)) + if (setpoint > (joymax / 2 + deadband)) { return direction::right; } - else if(setpoint < (joymax/2 -deadband)) + else if (setpoint < (joymax / 2 - deadband)) { return direction::left; } - //else + // else return direction::stop; } direction getdirecion2Button(uint8_t button0, uint8_t button1) { - if(button0 & !button1) + if (button0 & !button1) { return direction::left; } - if(!button0 & button1) + if (!button0 & button1) { return direction::right; } - //else + // else return direction::stop; } direction getTrackDirection(int trigger, uint8_t shoulder) { - if(trigger > (trgmax/2 + trgdeadband) & !shoulder) + if (trigger > (trgmax / 2 + trgdeadband) & !shoulder) { return direction::right; } - if((trigger == 0) & shoulder) + if ((trigger == 0) & shoulder) { return direction::left; } @@ -58,8 +59,24 @@ void setup_controller() Serial.printf("Waiting for controller"); } -void loop_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(); if (xboxController.isConnected()) { @@ -81,7 +98,7 @@ void loop_controller() // log_i("joyRHori rate: %u", xboxController.xboxNotif.trigLT); // log_i("joyRVert rate: %u", xboxController.xboxNotif.trigRT); log_i("battery %d %", xboxController.battery); - //log_v("joymax:%d",joymax ); + // log_v("joymax:%d",joymax ); lastcontrollernotify = millis(); } controlMotor("Pivot", getdirectionJoy(xboxController.xboxNotif.joyLHori)); @@ -93,21 +110,21 @@ 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 + else { - if (millis() - lastcontrollernotify > CONTROLLERNOTIFY) - { - log_w("not connected"); - lastcontrollernotify = millis(); - } + if (millis() - lastcontrollernotify > CONTROLLERNOTIFY) + { + log_w("not connected"); + lastcontrollernotify = millis(); + handleCabLights(1); + } if (xboxController.getCountFailedConnection() > 10) { log_e("restarting"); ESP.restart(); } } -} + }