mirror of
https://gitlab.waag.org/make/fablab/interns/2025/sam.git
synced 2025-08-03 11:54:58 +00:00
fix controller so middle of joystick is actually 1500 now
This commit is contained in:
@@ -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();
|
||||
// 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("]");
|
||||
}
|
||||
Reference in New Issue
Block a user