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
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

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

@@ -1,19 +1,24 @@
#include <Arduino.h>
#include <WiFi.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 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");
}

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

@@ -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);
}