Smikkelbakje/Drone-FabAcademy-2025
1
0
Fork 0
mirror of https://gitlab.waag.org/make/fablab/interns/2025/sam.git synced 2025-08-03 11:54:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Killswitch drone

Browse Source
This commit is contained in:
Sam Hos
2025-05-26 14:50:47 +02:00
parent 27935b52fb
commit a1d488be28
3 changed files with 145 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/controller/drone controller/platformio.ini
View File

@@ -12,10 +12,14 @@
platform = espressif32 platform = espressif32
board = lolin_c3_mini board = lolin_c3_mini
framework = arduino 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] [env:Xiao_c3]
board = seeed_xiao_esp32c3 board = seeed_xiao_esp32c3
framework = arduino framework = arduino
platform = espressif32 platform = espressif32
lib_deps = adafruit/Adafruit SSD1306@^2.5.13 lib_deps =
adafruit/Adafruit SSD1306@^2.5.13
olikraus/U8g2@^2.36.5

90
src/controller/drone controller/src/main.cpp
View File

@@ -1,11 +1,15 @@
#include <Arduino.h> #include <Arduino.h>
#include <WiFi.h> #include <WiFi.h>
#include <esp_now.h> #include <esp_now.h>
#include <U8g2lib.h>
// Oled Screen stuff
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);
const int MAXPWMVALUE = 1000; const int MAXPWMVALUE = 1000;
const int MINPWMVALUE = 2000; const int MINPWMVALUE = 2000;
const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8B, 0x18}; 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 typedef struct struct_message
@@ -14,6 +18,7 @@ typedef struct struct_message
int PWMCH2; int PWMCH2;
int PWMCH3; int PWMCH3;
int PWMCH4; int PWMCH4;
int killSwitch = 1; // 1 = throttle cut, 0 = normal operation
} struct_message; } struct_message;
struct_message JoystickData; // declare the struct as JoystickData struct_message JoystickData; // declare the struct as JoystickData
@@ -39,44 +44,43 @@ void printPWMValues();
int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin); int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
hardJoystickValues GUIParser(); hardJoystickValues GUIParser();
void MUXSetup(); void MUXSetup();
void killSwitch();
void OLEDSetup();
void setup() void setup()
{ {
OLEDSetup();
espNow(); espNow();
MUXSetup(); // Setup the multiplexer MUXSetup(); // Setup the multiplexer
Serial.begin(9600); Serial.begin(9600);
} }
void loop() void loop()
{ {
//Debugging // Debugging
// readAllMultiPlexer(); // readAllMultiPlexer();
// printPWMValues(); // printPWMValues();
// Set values to send // Set values to send
JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0)); //Right joystick Y 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.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0)); // Right joystick X
JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0)); // left joystick Y 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.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0)); // left joystick X
bool buttonRight = abs(analogReadMultiPlexer(0, 0, 1, 0, A0)/4095); // right button bool buttonRight = abs(analogReadMultiPlexer(0, 0, 1, 0, A0) / 4095); // right button
bool buttonLeft = abs(analogReadMultiPlexer(1, 0, 1, 0, A0)/4095); // left button bool buttonLeft = abs(analogReadMultiPlexer(1, 0, 1, 0, A0) / 4095); // left button
killSwitch();
GUIParser(); GUIParser();
espNowLoop(); espNowLoop();
delay(10); // delay to avoid hammering the radio and to save power/heat delay(10); // delay to avoid hammering the radio and to save power/heat
} }
int mapPot(int normalizedValue) int mapPot(int normalizedValue)
{ {
return map(normalizedValue, 400, 2500, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range 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 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); int pot = analogRead(pin);
@@ -100,7 +104,7 @@ int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD
digitalWrite(D8, addressD); digitalWrite(D8, addressD);
return analogRead(pin); 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) int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin)
{ {
digitalWrite(D3, LOW); digitalWrite(D3, LOW);
@@ -132,7 +136,8 @@ void espNow()
} }
} }
void espNowLoop(){ void espNowLoop()
{
// Send message via ESP-NOW // 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 *)&JoystickData, sizeof(JoystickData));
if (result == ESP_OK) if (result == ESP_OK)
@@ -155,10 +160,11 @@ void MUXSetup()
pinMode(A0, INPUT); // MUX input pinMode(A0, INPUT); // MUX input
} }
// Function to parse joystick data to hard values // Function to parse joystick data to hard values
//If joystick is Up then the value is 1 of var A // 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 Down then the value is 1 of var B
//If joystick is in the middle A and B are 0 // If joystick is in the middle A and B are 0
hardJoystickValues GUIParser(){ hardJoystickValues GUIParser()
{
// Define joystick offsets (calibrated resting values) // Define joystick offsets (calibrated resting values)
const int offsetPWMCH1 = 1090; // Resting value for PWMCH1 (right joystick Y-axis) const int offsetPWMCH1 = 1090; // Resting value for PWMCH1 (right joystick Y-axis)
@@ -176,17 +182,24 @@ hardJoystickValues GUIParser(){
int adjustedPWMCH4 = JoystickData.PWMCH4 - offsetPWMCH4; int adjustedPWMCH4 = JoystickData.PWMCH4 - offsetPWMCH4;
// Apply deadzone // Apply deadzone
if (abs(adjustedPWMCH1) < deadzone) adjustedPWMCH1 = 0; //abs to avoid negatives if (abs(adjustedPWMCH1) < deadzone)
if (abs(adjustedPWMCH2) < deadzone) adjustedPWMCH2 = 0; adjustedPWMCH1 = 0; // abs to avoid negatives
if (abs(adjustedPWMCH3) < deadzone) adjustedPWMCH3 = 0; if (abs(adjustedPWMCH2) < deadzone)
if (abs(adjustedPWMCH4) < deadzone) adjustedPWMCH4 = 0; adjustedPWMCH2 = 0;
if (abs(adjustedPWMCH3) < deadzone)
adjustedPWMCH3 = 0;
if (abs(adjustedPWMCH4) < deadzone)
adjustedPWMCH4 = 0;
// Map joystick values to hard values // Map joystick values to hard values
int LXU = 0; // Left joystick X axis up int LXU = 0; // Left joystick X axis up
int LXD = 0; // Left joystick X axis down int LXD = 0; // Left joystick X axis down
if (adjustedPWMCH1 > 0) { if (adjustedPWMCH1 > 0)
{
LXU = 1; // Joystick is up LXU = 1; // Joystick is up
} else if (adjustedPWMCH1 < 0) { }
else if (adjustedPWMCH1 < 0)
{
LXD = 1; // Joystick is down LXD = 1; // Joystick is down
} }
return {LXU, LXD, 0, 0}; // Return the values as a struct return {LXU, LXD, 0, 0}; // Return the values as a struct
@@ -196,7 +209,6 @@ hardJoystickValues GUIParser(){
// Debugging Functions // // Debugging Functions //
///////////////////////////////////////////// /////////////////////////////////////////////
void readAllMultiPlexer() void readAllMultiPlexer()
{ {
// we counting in binary // we counting in binary
@@ -234,7 +246,6 @@ void readAllMultiPlexer()
Serial.println(analogReadMultiPlexer(1, 1, 1, 1, A0)); Serial.println(analogReadMultiPlexer(1, 1, 1, 1, A0));
} }
void printPWMValues() void printPWMValues()
{ {
Serial.print("PWMCH1: "); Serial.print("PWMCH1: ");
@@ -246,3 +257,32 @@ void printPWMValues()
Serial.print(" PWMCH4: "); Serial.print(" PWMCH4: ");
Serial.println(JoystickData.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");
}

53
src/drone/src/main.cpp
View File

@@ -276,8 +276,9 @@ typedef struct struct_message
int PWMCH2 = 1500; int PWMCH2 = 1500;
int PWMCH3 = 1500; int PWMCH3 = 1500;
int PWMCH4 = 1500; int PWMCH4 = 1500;
int killSwitch = 1; // 1 = throttle cut, 0 = normal operation
} struct_message; } struct_message;
struct_message myData; //Initialise struct as myData struct_message ControllerData; //Initialise struct as ControllerData
#endif #endif
// IMU: // IMU:
@@ -584,12 +585,12 @@ void IMUinit()
if (myIMU.begin() == false) // from sparkfun example if (myIMU.begin() == false) // from sparkfun example
{ {
Serial.println("BNO080 not detected at default I2C address. Check your jumpers and the hookup guide. Freezing..."); 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 Wire.setClock(400000); // Increase I2C data rate to 400kHz
Serial.println("IMU initialized"); 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); delay(100);
myIMU.enableAccelerometer(50); myIMU.enableAccelerometer(50);
delay(100); delay(100);
@@ -1351,10 +1352,10 @@ void getCommands()
} }
#elif defined USE_ESPNow #elif defined USE_ESPNow
channel_1_pwm = myData.PWMCH1; channel_1_pwm = ControllerData.PWMCH1;
channel_2_pwm = myData.PWMCH2; channel_2_pwm = ControllerData.PWMCH2;
channel_3_pwm = myData.PWMCH3; channel_3_pwm = ControllerData.PWMCH3;
channel_4_pwm = myData.PWMCH4; channel_4_pwm = ControllerData.PWMCH4;
channel_5_pwm = 1000; // Temporary always armed channel_5_pwm = 1000; // Temporary always armed
// channel_6_pwm = getRadioPWM(6); // channel_6_pwm = getRadioPWM(6);
@@ -1929,5 +1930,39 @@ void ESPNowSetup()
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) 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);
} }