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

Fix motors not getting armed

Browse Source
This commit is contained in:
Sam Hos
2025-03-31 13:10:27 +02:00
parent c491110afe
commit 5c10f8fbfc
2 changed files with 74 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,22 +5,23 @@
// declarations
int normalizePot(int pin, int minValue);
int mapPot(int normalizedValue);
int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
// constants
const int POTPIN1 = 2;
const int MAXPWMVALUE = 1400;
const int MAXPWMVALUE = 1000;
const int MINPWMVALUE = 2000;
const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8E, 0x5C};
//Define the struct that will be sent
typedef struct struct_message {
// Define the struct that will be sent
typedef struct struct_message
{
int PWMCH1;
int PWMCH2;
int PWMCH3;
int PWMCH4;
} struct_message;
struct_message myData; //declare the struct as myData
struct_message myData; // declare the struct as myData
esp_now_peer_info_t peerInfo; //create a class object of the ESPNow class
esp_now_peer_info_t peerInfo; // create a class object of the ESPNow class
void setup()
{
@@ -28,52 +29,64 @@ void setup()
WiFi.mode(WIFI_STA);
// Init ESP-NOW
if (esp_now_init() != ESP_OK) {
if (esp_now_init() != ESP_OK)
{
Serial.println("Error initializing ESP-NOW");
return;
}
// Register peer
memcpy(peerInfo.peer_addr, broadcastAddress, 6);
peerInfo.channel = 0;
peerInfo.channel = 0;
peerInfo.encrypt = false;
// Add peer
if (esp_now_add_peer(&peerInfo) != ESP_OK){
// Add peer
if (esp_now_add_peer(&peerInfo) != ESP_OK)
{
Serial.println("Failed to add peer");
return;
}
Serial.begin(9600);
pinMode(POTPIN1, INPUT);
pinMode(D3, OUTPUT);
pinMode(D6, OUTPUT);
pinMode(D7, OUTPUT);
pinMode(D8, OUTPUT);
pinMode(D9, OUTPUT);
pinMode(D0, INPUT);
}
void loop()
{
Serial.println(mapPot(normalizePot(POTPIN1, 80))); //call normalizePot and put the output into mapPot then print it
Serial.println(analogReadMultiPlexer(0, 0, 0, 0, A0)); // call normalizePot and put the output into mapPot then print it
// Set values to send
myData.PWMCH1 = mapPot(normalizePot(POTPIN1, 80));
myData.PWMCH2 = 1000; //test values
myData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0));
myData.PWMCH2 = 1000; // test values
myData.PWMCH3 = 1000;
myData.PWMCH4 = 1000;
// Send message via ESP-NOW
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData));
if (result == ESP_OK) {
Serial.println("Sent with success");
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&myData, sizeof(myData));
if (result == ESP_OK)
{
// Serial.println("Sent with success");
}
else {
Serial.println("Error sending the data");
else
{
// Serial.println("Error sending the data");
}
delay(10);
}
int mapPot(int normalizedValue){
return map(normalizedValue, 80, 4095, MINPWMVALUE, MAXPWMVALUE); //map the normalized value to the PWM range
int mapPot(int normalizedValue)
{
return map(normalizedValue, 400, 2500, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range
}
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);
@@ -85,4 +98,14 @@ int normalizePot(int pin, int minValue) //normalize the pot value to a range of
{
return pot;
}
}
int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin)
{
digitalWrite(D3, LOW);
digitalWrite(D6, LOW);
digitalWrite(D7, LOW);
digitalWrite(D9, LOW);
digitalWrite(D8, LOW);
return analogRead(pin);
}

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

@@ -367,6 +367,7 @@ float invSqrt(float x);
void controlMixer();
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len);
void ESPNowSetup();
void printDebugInfo();
//========================================================================================================================//
// VOID SETUP //
@@ -476,14 +477,15 @@ void loop()
// Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE:
// printRadioData(); //Prints radio pwm values (expected: 1000 to 2000)
// printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle)
// printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest)
// printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest)
printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
// printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300)
printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level)
// printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
// printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250)
// printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180)
// printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations)
// printDebugInfo(); //PWM 1 and 5 + Armed condition
// Get arming status
armedStatus(); // Check if the throttle cut is off and throttle is low.
@@ -886,16 +888,7 @@ void calculate_IMU_error()
void calibrateAttitude()
{
// DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators
// Assuming vehicle is powered up on level surface!
/*
* This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds
* to boot.
*/
// Warm up IMU and madgwick filter in simulated main loop
for (int i = 0; i <= 10000; i++)
{
prev_time = current_time;
current_time = micros();
// Assuming vehicle is powered up on level surface!throttle
dt = (current_time - prev_time) / 1000000.0;
getIMUdata();
Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt);
@@ -1434,7 +1427,7 @@ void getCommands()
channel_2_pwm = myData.PWMCH2;
channel_3_pwm = myData.PWMCH3;
channel_4_pwm = myData.PWMCH4;
// channel_5_pwm = getRadioPWM(5); //commented out because ESPnow only sends 4 channels
channel_5_pwm = 1000; // Temporary always armed
// channel_6_pwm = getRadioPWM(6);
#endif
@@ -2005,6 +1998,24 @@ void printLoopRate()
}
}
void printDebugInfo() {
if (current_time - print_counter > 10000) {
print_counter = micros();
Serial.print("CH1(thro):");
Serial.print(channel_1_pwm);
Serial.print(" CH5:");
Serial.print(channel_5_pwm);
Serial.print(" Armed:");
Serial.print(armedFly);
Serial.print(" thro_des:");
Serial.print(thro_des);
Serial.print(" m1_scaled:");
Serial.print(m1_command_scaled);
Serial.print(" m1_PWM:");
Serial.println(m1_command_PWM);
}
}
//=========================================================================================//
// HELPER FUNCTIONS