diff --git a/src/controller/drone controller/platformio.ini b/src/controller/drone controller/platformio.ini index 0adf078..4581943 100644 --- a/src/controller/drone controller/platformio.ini +++ b/src/controller/drone controller/platformio.ini @@ -12,10 +12,14 @@ platform = espressif32 board = lolin_c3_mini framework = arduino -lib_deps = adafruit/Adafruit SSD1306@^2.5.13 +lib_deps = + adafruit/Adafruit SSD1306@^2.5.13 + olikraus/U8g2@^2.36.5 [env:Xiao_c3] board = seeed_xiao_esp32c3 framework = arduino platform = espressif32 -lib_deps = adafruit/Adafruit SSD1306@^2.5.13 +lib_deps = + adafruit/Adafruit SSD1306@^2.5.13 + olikraus/U8g2@^2.36.5 diff --git a/src/controller/drone controller/src/main.cpp b/src/controller/drone controller/src/main.cpp index 81b9ca5..a1bc0a2 100644 --- a/src/controller/drone controller/src/main.cpp +++ b/src/controller/drone controller/src/main.cpp @@ -1,19 +1,24 @@ #include #include #include +#include + +// Oled Screen stuff +U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE); const int MAXPWMVALUE = 1000; const int MINPWMVALUE = 2000; const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8B, 0x18}; -//8c:bf:ea:cc:8b:18 +// 8c:bf:ea:cc:8b:18 //=====================================================================================// -// Struct declarations +// Struct declarations typedef struct struct_message { int PWMCH1; int PWMCH2; int PWMCH3; int PWMCH4; + int killSwitch = 1; // 1 = throttle cut, 0 = normal operation } struct_message; struct_message JoystickData; // declare the struct as JoystickData @@ -25,7 +30,7 @@ struct hardJoystickValues int LXD; // Left joystick X axis down int RXU; // Right joystick X axis up int RXD; // Right joystick X axis down -}; +}; //=====================================================================================// // declarations @@ -39,44 +44,43 @@ void printPWMValues(); int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin); hardJoystickValues GUIParser(); void MUXSetup(); - - +void killSwitch(); +void OLEDSetup(); void setup() { + OLEDSetup(); espNow(); MUXSetup(); // Setup the multiplexer Serial.begin(9600); - } void loop() { - //Debugging - // readAllMultiPlexer(); - // printPWMValues(); - + // Debugging + // readAllMultiPlexer(); + // printPWMValues(); // Set values to send - JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0)); //Right joystick Y - JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0)); // Right joystick X - JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0)); // left joystick Y - JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0)); // left joystick X - bool buttonRight = abs(analogReadMultiPlexer(0, 0, 1, 0, A0)/4095); // right button - bool buttonLeft = abs(analogReadMultiPlexer(1, 0, 1, 0, A0)/4095); // left button + JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0)); // Right joystick Y + JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0)); // Right joystick X + JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0)); // left joystick Y + JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0)); // left joystick X + bool buttonRight = abs(analogReadMultiPlexer(0, 0, 1, 0, A0) / 4095); // right button + bool buttonLeft = abs(analogReadMultiPlexer(1, 0, 1, 0, A0) / 4095); // left button + killSwitch(); GUIParser(); espNowLoop(); delay(10); // delay to avoid hammering the radio and to save power/heat } - int mapPot(int normalizedValue) { return map(normalizedValue, 400, 2500, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range } -//legacy stuff for original potsliders +// legacy stuff for original potsliders int normalizePot(int pin, int minValue) // normalize the pot value to a range of 80 to 4095 instead of 0 to 4095 because the potmeter is at lower values not accurate { int pot = analogRead(pin); @@ -100,7 +104,7 @@ int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD digitalWrite(D8, addressD); return analogRead(pin); } -//For some reason the digitalRead function does not work with the multiplexer. Maybe because the Resistance is too high. Even though analogRead returns 4095 +// For some reason the digitalRead function does not work with the multiplexer. Maybe because the Resistance is too high. Even though analogRead returns 4095 int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin) { digitalWrite(D3, LOW); @@ -132,7 +136,8 @@ void espNow() } } -void espNowLoop(){ +void espNowLoop() +{ // Send message via ESP-NOW esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&JoystickData, sizeof(JoystickData)); if (result == ESP_OK) @@ -155,48 +160,55 @@ void MUXSetup() pinMode(A0, INPUT); // MUX input } // Function to parse joystick data to hard values -//If joystick is Up then the value is 1 of var A -//If joystick is Down then the value is 1 of var B -//If joystick is in the middle A and B are 0 -hardJoystickValues GUIParser(){ +// If joystick is Up then the value is 1 of var A +// If joystick is Down then the value is 1 of var B +// If joystick is in the middle A and B are 0 +hardJoystickValues GUIParser() +{ - // Define joystick offsets (calibrated resting values) - const int offsetPWMCH1 = 1090; // Resting value for PWMCH1 (right joystick Y-axis) - const int offsetPWMCH2 = 1072; // Resting value for PWMCH2 (right joystick X-axis) - const int offsetPWMCH3 = 1043; // Resting value for PWMCH3 (left joystick X-axis) - const int offsetPWMCH4 = 1476; // Resting value for PWMCH4 (left joystick Y-axis) - - // Define deadzone threshold - const int deadzone = 120; - - // Adjust joystick values by subtracting offsets - int adjustedPWMCH1 = JoystickData.PWMCH1 - offsetPWMCH1; - int adjustedPWMCH2 = JoystickData.PWMCH2 - offsetPWMCH2; - int adjustedPWMCH3 = JoystickData.PWMCH3 - offsetPWMCH3; - int adjustedPWMCH4 = JoystickData.PWMCH4 - offsetPWMCH4; - - // Apply deadzone - if (abs(adjustedPWMCH1) < deadzone) adjustedPWMCH1 = 0; //abs to avoid negatives - if (abs(adjustedPWMCH2) < deadzone) adjustedPWMCH2 = 0; - if (abs(adjustedPWMCH3) < deadzone) adjustedPWMCH3 = 0; - if (abs(adjustedPWMCH4) < deadzone) adjustedPWMCH4 = 0; + // Define joystick offsets (calibrated resting values) + const int offsetPWMCH1 = 1090; // Resting value for PWMCH1 (right joystick Y-axis) + const int offsetPWMCH2 = 1072; // Resting value for PWMCH2 (right joystick X-axis) + const int offsetPWMCH3 = 1043; // Resting value for PWMCH3 (left joystick X-axis) + const int offsetPWMCH4 = 1476; // Resting value for PWMCH4 (left joystick Y-axis) - // Map joystick values to hard values - int LXU = 0; // Left joystick X axis up - int LXD = 0; // Left joystick X axis down - if (adjustedPWMCH1 > 0) { - LXU = 1; // Joystick is up - } else if (adjustedPWMCH1 < 0) { - LXD = 1; // Joystick is down - } - return {LXU, LXD, 0, 0}; // Return the values as a struct + // Define deadzone threshold + const int deadzone = 120; + + // Adjust joystick values by subtracting offsets + int adjustedPWMCH1 = JoystickData.PWMCH1 - offsetPWMCH1; + int adjustedPWMCH2 = JoystickData.PWMCH2 - offsetPWMCH2; + int adjustedPWMCH3 = JoystickData.PWMCH3 - offsetPWMCH3; + int adjustedPWMCH4 = JoystickData.PWMCH4 - offsetPWMCH4; + + // Apply deadzone + if (abs(adjustedPWMCH1) < deadzone) + adjustedPWMCH1 = 0; // abs to avoid negatives + if (abs(adjustedPWMCH2) < deadzone) + adjustedPWMCH2 = 0; + if (abs(adjustedPWMCH3) < deadzone) + adjustedPWMCH3 = 0; + if (abs(adjustedPWMCH4) < deadzone) + adjustedPWMCH4 = 0; + + // Map joystick values to hard values + int LXU = 0; // Left joystick X axis up + int LXD = 0; // Left joystick X axis down + if (adjustedPWMCH1 > 0) + { + LXU = 1; // Joystick is up + } + else if (adjustedPWMCH1 < 0) + { + LXD = 1; // Joystick is down + } + return {LXU, LXD, 0, 0}; // Return the values as a struct } ///////////////////////////////////////////// // Debugging Functions // ///////////////////////////////////////////// - void readAllMultiPlexer() { // we counting in binary @@ -234,7 +246,6 @@ void readAllMultiPlexer() Serial.println(analogReadMultiPlexer(1, 1, 1, 1, A0)); } - void printPWMValues() { Serial.print("PWMCH1: "); @@ -245,4 +256,33 @@ void printPWMValues() Serial.print(JoystickData.PWMCH3); Serial.print(" PWMCH4: "); Serial.println(JoystickData.PWMCH4); +} + +void killSwitch() +{ + // Set the kill switch to 1 to stop the motors + if (abs(analogReadMultiPlexer(0, 0, 1, 0, A0) / 4095) == 1) // Right button + { + JoystickData.killSwitch = 1; // Activate kill switch + Serial.println("Kill switch activated, motors stopped."); + u8g2.clearBuffer(); + u8g2.drawStr(25, 15, "Kill Switch: ON"); + } + else if (abs(analogReadMultiPlexer(1, 0, 1, 0, A0) / 4095) == 1) // Left button + { + JoystickData.killSwitch = 0; // Deactivate kill switch + Serial.println("Kill switch deactivated, motors can run."); + u8g2.clearBuffer(); + u8g2.drawStr(25, 15, "Kill Switch: OFF"); + } + u8g2.sendBuffer(); +} + +void OLEDSetup(){ + u8g2.begin(); + u8g2.clearBuffer(); + u8g2.sendBuffer(); + u8g2.setFont(u8g2_font_6x10_tf); // Use a different, simpler font + u8g2.clearBuffer(); + u8g2.drawStr(25, 15, "Kill Switch: ON"); } \ No newline at end of file diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index 60c04ae..b0a1ab8 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -276,8 +276,9 @@ typedef struct struct_message int PWMCH2 = 1500; int PWMCH3 = 1500; int PWMCH4 = 1500; + int killSwitch = 1; // 1 = throttle cut, 0 = normal operation } struct_message; -struct_message myData; //Initialise struct as myData +struct_message ControllerData; //Initialise struct as ControllerData #endif // IMU: @@ -584,12 +585,12 @@ void IMUinit() if (myIMU.begin() == false) // from sparkfun example { Serial.println("BNO080 not detected at default I2C address. Check your jumpers and the hookup guide. Freezing..."); - // while (1) - // ; + while (1) + ; } Wire.setClock(400000); // Increase I2C data rate to 400kHz Serial.println("IMU initialized"); - myIMU.enableGyro(50); + myIMU.enableGyro(50); //for some reason when enabling things too fast the BNO085 will not respond delay(100); myIMU.enableAccelerometer(50); delay(100); @@ -1351,10 +1352,10 @@ void getCommands() } #elif defined USE_ESPNow - channel_1_pwm = myData.PWMCH1; - channel_2_pwm = myData.PWMCH2; - channel_3_pwm = myData.PWMCH3; - channel_4_pwm = myData.PWMCH4; + channel_1_pwm = ControllerData.PWMCH1; + channel_2_pwm = ControllerData.PWMCH2; + channel_3_pwm = ControllerData.PWMCH3; + channel_4_pwm = ControllerData.PWMCH4; channel_5_pwm = 1000; // Temporary always armed // channel_6_pwm = getRadioPWM(6); @@ -1929,5 +1930,39 @@ void ESPNowSetup() void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { - memcpy(&myData, incomingData, sizeof(myData)); + // Create a temporary struct to check the data first + struct_message tempData; + memcpy(&tempData, incomingData, sizeof(tempData)); + + // Always update the kill switch state first + ControllerData.killSwitch = tempData.killSwitch; + + if (tempData.killSwitch == 1) + { + // Keep channel values at safe defaults when kill switch is active + ControllerData.PWMCH1 = 1000; // Min throttle + ControllerData.PWMCH2 = 1500; // Center stick + ControllerData.PWMCH3 = 1500; // Center stick + ControllerData.PWMCH4 = 1500; // Center stick + + // Immediately disable motors + ledcWrite(m1Pin, pulseWidthToDutyCycle(1000)); // Minimum pulse width (1000μs) + ledcWrite(m2Pin, pulseWidthToDutyCycle(1000)); + ledcWrite(m3Pin, pulseWidthToDutyCycle(1000)); + ledcWrite(m4Pin, pulseWidthToDutyCycle(1000)); + + Serial.println("Kill switch activated. Motors disabled."); + } + else + { + // Only copy control channel data if kill switch is not active + ControllerData.PWMCH1 = tempData.PWMCH1; + ControllerData.PWMCH2 = tempData.PWMCH2; + ControllerData.PWMCH3 = tempData.PWMCH3; + ControllerData.PWMCH4 = tempData.PWMCH4; + } + + // For debugging - uncomment if needed + // Serial.print("KillSwitch: "); + // Serial.println(ControllerData.killSwitch); } \ No newline at end of file