fix controller so middle of joystick is actually 1500 now

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 MAXPWMVALUE = 2000;
-const int MINPWMVALUE = 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();
+  // readAllMultiPlexer();
-  //  printPWMValues();
+  // printFilteredPWMValues();
+  // printUnfilteredPWMValues();
+  // printOffsets();
+  debugAllChannels();
 
   // Set values to send
-  JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0));  // Right joystick Y
+  JoystickData.PWMCH1 = analogReadMultiPlexer(0, 0, 0, 0, A0); // Right joystick Y
-  JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0));  // Right joystick X
+  JoystickData.PWMCH2 = analogReadMultiPlexer(1, 0, 0, 0, A0); // Right joystick X
-  JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0));  // left joystick Y
+  JoystickData.PWMCH3 = analogReadMultiPlexer(0, 0, 0, 1, A0); // left joystick Y
-  JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0));  // left joystick X
+  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
+  Serial.print("offsetPWMCH1: ");
-  if (abs(analogReadMultiPlexer(0, 0, 1, 0, A0) / 4095) == 1) // Right button
+  Serial.print(offsetPWMCH1);
-  {
+  Serial.print(" offsetPWMCH2: ");
-    JoystickData.killSwitch = 1; // Activate kill switch
+  Serial.print(offsetPWMCH2);
-    Serial.println("Kill switch activated, motors stopped.");
+  Serial.print(" offsetPWMCH3: ");
-    u8g2.clearBuffer();
+  Serial.print(offsetPWMCH3);
-    u8g2.drawStr(25, 15, "Kill Switch: ON");
+  Serial.print(" offsetPWMCH4: ");
-  }
+  Serial.println(offsetPWMCH4);
-  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(){
+void debugAllChannels() {
-  u8g2.begin();
+  // Raw values
-  u8g2.clearBuffer();
+  Serial.print("Raw: [");
-  u8g2.sendBuffer();
+  Serial.print(JoystickData.PWMCH1); Serial.print(",");
-  u8g2.setFont(u8g2_font_6x10_tf); // Use a different, simpler font
+  Serial.print(JoystickData.PWMCH2); Serial.print(",");
-  u8g2.clearBuffer();
+  Serial.print(JoystickData.PWMCH3); Serial.print(",");
-  u8g2.drawStr(25, 15, "Kill Switch: ON");
+  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("]");
 }