diff --git a/src/controller/drone controller/src/main.cpp b/src/controller/drone controller/src/main.cpp index 3020d11..9686b8a 100644 --- a/src/controller/drone controller/src/main.cpp +++ b/src/controller/drone controller/src/main.cpp @@ -6,12 +6,22 @@ // 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 int MAXPWMVALUE = 2000; +const int MINPWMVALUE = 1000; const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8B, 0x18}; // 8c:bf:ea:cc:8b:18 //=====================================================================================// // Struct declarations +typedef struct filteredJoystickData +{ + int PWMCH1; + int PWMCH2; + int PWMCH3; + int PWMCH4; + int killSwitch = 1; // 1 = throttle cut, 0 = normal operation +} filteredJoystickData; +filteredJoystickData filteredJoystick; // declare the struct as JoystickData + typedef struct struct_message { int PWMCH1; @@ -32,6 +42,16 @@ struct hardJoystickValues int RXD; // Right joystick X axis down }; +// uncalibrated offsets these are calibrated in setup() function +int offsetPWMCH1 = 0; // Resting value for PWMCH1 (right joystick Y-axis) +int offsetPWMCH2 = 0; // Resting value for PWMCH2 (right joystick X-axis) +int offsetPWMCH3 = 0; // Resting value for PWMCH3 (left joystick X-axis) +int offsetPWMCH4 = 0; // Resting value for PWMCH4 (left joystick Y-axis) +// Define deadzone threshold +const int deadzone = 200; +const int pwmDeadzone = 50; // Much smaller deadzone in PWM space + + //=====================================================================================// // declarations int normalizePot(int pin, int minValue); @@ -40,37 +60,51 @@ int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD void espNow(); void readAllMultiPlexer(); void espNowLoop(); -void printPWMValues(); +void printFilteredPWMValues(); +void printUnfilteredPWMValues(); int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin); hardJoystickValues GUIParser(); void MUXSetup(); void killSwitch(); void OLEDSetup(); +void deadZonedJoysticks(); // Apply deadzone to joystick values +void calibrateJoystickOffsets(); +void printOffsets(); +void debugPWM1(); +void debugAllChannels(); + +//===========================================================================// void setup() { + Serial.begin(9600); OLEDSetup(); espNow(); MUXSetup(); // Setup the multiplexer - Serial.begin(9600); + calibrateJoystickOffsets(); } void loop() { // Debugging - // readAllMultiPlexer(); - // printPWMValues(); + // readAllMultiPlexer(); + // printFilteredPWMValues(); + // printUnfilteredPWMValues(); + // printOffsets(); + debugAllChannels(); // 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 + JoystickData.PWMCH1 = analogReadMultiPlexer(0, 0, 0, 0, A0); // Right joystick Y + JoystickData.PWMCH2 = analogReadMultiPlexer(1, 0, 0, 0, A0); // Right joystick X + JoystickData.PWMCH3 = analogReadMultiPlexer(0, 0, 0, 1, A0); // left joystick Y + JoystickData.PWMCH4 = analogReadMultiPlexer(1, 0, 0, 1, A0); // left joystick X + + deadZonedJoysticks(); // Apply deadzone to joystick values + 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 } @@ -139,7 +173,7 @@ void espNow() void espNowLoop() { // Send message via ESP-NOW - esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&JoystickData, sizeof(JoystickData)); + esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&filteredJoystick, sizeof(filteredJoystick)); if (result == ESP_OK) { // Serial.println("Sent with success"); @@ -166,15 +200,6 @@ void MUXSetup() 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; @@ -205,6 +230,108 @@ hardJoystickValues GUIParser() return {LXU, LXD, 0, 0}; // Return the values as a struct } +void deadZonedJoysticks() +{ + // Convert raw values to PWM first, then check deadzone + int pwmCH1 = mapPot(JoystickData.PWMCH1); + int pwmCH2 = mapPot(JoystickData.PWMCH2); + int pwmCH3 = mapPot(JoystickData.PWMCH3); + int pwmCH4 = mapPot(JoystickData.PWMCH4); + + // Convert offsets to PWM too + int pwmOffsetCH1 = mapPot(offsetPWMCH1); + int pwmOffsetCH2 = mapPot(offsetPWMCH2); + int pwmOffsetCH3 = mapPot(offsetPWMCH3); + int pwmOffsetCH4 = mapPot(offsetPWMCH4); + + // Calculate adjusted values in PWM space + int adjustedPWMCH1 = pwmCH1 - pwmOffsetCH1; + int adjustedPWMCH2 = pwmCH2 - pwmOffsetCH2; + int adjustedPWMCH3 = pwmCH3 - pwmOffsetCH3; + int adjustedPWMCH4 = pwmCH4 - pwmOffsetCH4; + + // // Apply deadzone + // if (abs(adjustedPWMCH1) < deadzone) + // filteredJoystick.PWMCH1 = 1500; // abs to avoid negatives + // else + // filteredJoystick.PWMCH1 = JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0)); // Right joystick Y + // if (abs(adjustedPWMCH2) < deadzone) + // filteredJoystick.PWMCH2 = 1500; + // else + // filteredJoystick.PWMCH2 = JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0)); // Right joystick X + // if (abs(adjustedPWMCH3) < deadzone) + // filteredJoystick.PWMCH3 = 1500; + // else + // filteredJoystick.PWMCH3 = JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0)); // Left joystick Y + // if (abs(adjustedPWMCH4) < deadzone) + // filteredJoystick.PWMCH4 = 1500; + // else + // filteredJoystick.PWMCH4 = JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0)); // Left joystick X + + // Apply deadzone and set filtered values + // This is the same as above but more compact + + filteredJoystick.PWMCH1 = (abs(adjustedPWMCH1) < pwmDeadzone) ? 1500 : pwmCH1; + filteredJoystick.PWMCH2 = (abs(adjustedPWMCH2) < pwmDeadzone) ? 1500 : pwmCH2; + filteredJoystick.PWMCH3 = (abs(adjustedPWMCH3) < pwmDeadzone) ? 1500 : pwmCH3; + filteredJoystick.PWMCH4 = (abs(adjustedPWMCH4) < pwmDeadzone) ? 1500 : pwmCH4; + + // Copy kill switch state + filteredJoystick.killSwitch = JoystickData.killSwitch; +} + +void calibrateJoystickOffsets() +{ + delay(5000); // Wait for everything to stabilize + + long sum[4] = {0, 0, 0, 0}; + const int samples = 500; + + for (int i = 0; i < samples; i++) + { + sum[0] += analogReadMultiPlexer(0, 0, 0, 0, A0); + sum[1] += analogReadMultiPlexer(1, 0, 0, 0, A0); + sum[2] += analogReadMultiPlexer(0, 0, 0, 1, A0); + sum[3] += analogReadMultiPlexer(1, 0, 0, 1, A0); + delay(10); // Longer delay for stability + } + + offsetPWMCH1 = sum[0] / samples; + offsetPWMCH2 = sum[1] / samples; + offsetPWMCH3 = sum[2] / samples; + offsetPWMCH4 = sum[3] / samples + 1000; // Hardware quirk of the controller +} + +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"); +} + ///////////////////////////////////////////// // Debugging Functions // ///////////////////////////////////////////// @@ -246,7 +373,19 @@ void readAllMultiPlexer() Serial.println(analogReadMultiPlexer(1, 1, 1, 1, A0)); } -void printPWMValues() +void printFilteredPWMValues() +{ + Serial.print("PWMCH1: "); + Serial.print(filteredJoystick.PWMCH1); + Serial.print(" PWMCH2: "); + Serial.print(filteredJoystick.PWMCH2); + Serial.print(" PWMCH3: "); + Serial.print(filteredJoystick.PWMCH3); + Serial.print(" PWMCH4: "); + Serial.println(filteredJoystick.PWMCH4); +} + +void printUnfilteredPWMValues() { Serial.print("PWMCH1: "); Serial.print(JoystickData.PWMCH1); @@ -258,31 +397,73 @@ void printPWMValues() Serial.println(JoystickData.PWMCH4); } -void killSwitch() +void printOffsets() { - // 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(); + Serial.print("offsetPWMCH1: "); + Serial.print(offsetPWMCH1); + Serial.print(" offsetPWMCH2: "); + Serial.print(offsetPWMCH2); + Serial.print(" offsetPWMCH3: "); + Serial.print(offsetPWMCH3); + Serial.print(" offsetPWMCH4: "); + Serial.println(offsetPWMCH4); } -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"); +void debugAllChannels() { + // Raw values + Serial.print("Raw: ["); + Serial.print(JoystickData.PWMCH1); Serial.print(","); + Serial.print(JoystickData.PWMCH2); Serial.print(","); + Serial.print(JoystickData.PWMCH3); Serial.print(","); + Serial.print(JoystickData.PWMCH4); Serial.print("] "); + + // Offsets + Serial.print("Offsets: ["); + Serial.print(offsetPWMCH1); Serial.print(","); + Serial.print(offsetPWMCH2); Serial.print(","); + Serial.print(offsetPWMCH3); Serial.print(","); + Serial.print(offsetPWMCH4); Serial.print("] "); + + // PWM converted values + int pwmCH1 = mapPot(JoystickData.PWMCH1); + int pwmCH2 = mapPot(JoystickData.PWMCH2); + int pwmCH3 = mapPot(JoystickData.PWMCH3); + int pwmCH4 = mapPot(JoystickData.PWMCH4); + + int pwmOffsetCH1 = mapPot(offsetPWMCH1); + int pwmOffsetCH2 = mapPot(offsetPWMCH2); + int pwmOffsetCH3 = mapPot(offsetPWMCH3); + int pwmOffsetCH4 = mapPot(offsetPWMCH4); + + Serial.print("PWM: ["); + Serial.print(pwmCH1); Serial.print(","); + Serial.print(pwmCH2); Serial.print(","); + Serial.print(pwmCH3); Serial.print(","); + Serial.print(pwmCH4); Serial.print("] "); + + // Adjusted values + int adjustedPWMCH1 = pwmCH1 - pwmOffsetCH1; + int adjustedPWMCH2 = pwmCH2 - pwmOffsetCH2; + int adjustedPWMCH3 = pwmCH3 - pwmOffsetCH3; + int adjustedPWMCH4 = pwmCH4 - pwmOffsetCH4; + + Serial.print("Adj: ["); + Serial.print(adjustedPWMCH1); Serial.print(","); + Serial.print(adjustedPWMCH2); Serial.print(","); + Serial.print(adjustedPWMCH3); Serial.print(","); + Serial.print(adjustedPWMCH4); Serial.print("] "); + + // Deadzone status + Serial.print("InDZ: ["); + Serial.print(abs(adjustedPWMCH1) < pwmDeadzone ? "Y" : "N"); Serial.print(","); + Serial.print(abs(adjustedPWMCH2) < pwmDeadzone ? "Y" : "N"); Serial.print(","); + Serial.print(abs(adjustedPWMCH3) < pwmDeadzone ? "Y" : "N"); Serial.print(","); + Serial.print(abs(adjustedPWMCH4) < pwmDeadzone ? "Y" : "N"); Serial.print("] "); + + // Final filtered values + Serial.print("Final: ["); + Serial.print(filteredJoystick.PWMCH1); Serial.print(","); + Serial.print(filteredJoystick.PWMCH2); Serial.print(","); + Serial.print(filteredJoystick.PWMCH3); Serial.print(","); + Serial.print(filteredJoystick.PWMCH4); Serial.println("]"); } \ No newline at end of file