diff --git a/docs/final_project/PXL_20250528_093723437(1)(1).mp4 b/docs/final_project/PXL_20250528_093723437(1)(1).mp4 new file mode 100644 index 0000000..22f6134 Binary files /dev/null and b/docs/final_project/PXL_20250528_093723437(1)(1).mp4 differ diff --git a/docs/final_project/final_project.md b/docs/final_project/final_project.md index 435ef09..697e856 100644 --- a/docs/final_project/final_project.md +++ b/docs/final_project/final_project.md @@ -346,6 +346,7 @@ From this tab I can edit the body with all the references in place. So I could u ## Burnout! +### Part 1 One of my motors during testing burned out completely when it was attached to the printed arm. I think the cause for this is overheating due to a propellor that's too big. ![alt text](image-27.jpg) @@ -359,6 +360,12 @@ Another thing that also could be the issue is that my ESC was surging above 40 A I think the main reason was heat and the coating on the motors evaporating causing a chain reaction. +### Part 2 +This time it was completely my fault for not paying attention to the wires when connecting them to the right terminals. I had all other motors taped off like this but I forgot to do that for one motor I was testing. + +![alt text](imag1.jpg) + + ## Cables The drone needs lots of cables to get power to the places where they need to be. I first started out by laying out everything that needed to be connected. diff --git a/docs/final_project/imag1.jpg b/docs/final_project/imag1.jpg new file mode 100644 index 0000000..7d8cea0 Binary files /dev/null and b/docs/final_project/imag1.jpg differ diff --git a/src/controller/drone controller/src/main.cpp b/src/controller/drone controller/src/main.cpp index a1bc0a2..3020d11 100644 --- a/src/controller/drone controller/src/main.cpp +++ b/src/controller/drone controller/src/main.cpp @@ -77,7 +77,7 @@ void loop() int mapPot(int normalizedValue) { - return map(normalizedValue, 400, 2500, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range + return map(normalizedValue, 400, 4095, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range } // legacy stuff for original potsliders diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index 7de97e4..a5d7446 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -175,9 +175,9 @@ unsigned long channel_5_fs = 2000; // gear, greater than 1500 = throttle cut unsigned long channel_6_fs = 2000; // aux1 // Filter parameters - Defaults tuned for 2kHz loop rate; Do not touch unless you know what you are doing: -float B_madgwick = 0.04; // Madgwick filter parameter -float B_accel = 0.14; // Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) -float B_gyro = 0.1; // Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) +float B_madgwick = 0.00001; // Madgwick filter parameter +float B_accel = 0.00001; // Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) +float B_gyro = 0.0001; // Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) float B_mag = 1.0; // Magnetometer LP filter parameter // Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these @@ -189,33 +189,33 @@ float MagScaleY = 1.0; float MagScaleZ = 1.0; // IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() -float AccErrorX = 0.0; -float AccErrorY = 0.0; -float AccErrorZ = 0.0; -float GyroErrorX = 0.0; -float GyroErrorY = 0.0; -float GyroErrorZ = 0.0; +float AccErrorX = 0.05; +float AccErrorY = 0.01; +float AccErrorZ = -0.01; +float GyroErrorX = 0.00; +float GyroErrorY = 0.00; +float GyroErrorZ = 0.00; // Controller parameters (take note of defaults before modifying!): float i_limit = 25.0; // Integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 30.0; // Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode -float maxPitch = 30.0; // Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxRoll = 15.0; // Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxPitch = 15.0; // Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode float maxYaw = 160.0; // Max yaw rate in deg/sec -float Kp_roll_angle = 0.2; // Roll P-gain - angle mode -float Ki_roll_angle = 0.3; // Roll I-gain - angle mode -float Kd_roll_angle = 0.05; // Roll D-gain - angle mode (has no effect on controlANGLE2) +float Kp_roll_angle = 2.0; // Roll P-gain - angle mode +float Ki_roll_angle = 0.5; // Roll I-gain - angle mode +float Kd_roll_angle = 0.1; // Roll D-gain - angle mode (has no effect on controlANGLE2) float B_loop_roll = 0.9; // Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) -float Kp_pitch_angle = 0.2; // Pitch P-gain - angle mode -float Ki_pitch_angle = 0.3; // Pitch I-gain - angle mode -float Kd_pitch_angle = 0.05; // Pitch D-gain - angle mode (has no effect on controlANGLE2) +float Kp_pitch_angle = 2.0; // Pitch P-gain - angle mode +float Ki_pitch_angle = 0.5; // Pitch I-gain - angle mode +float Kd_pitch_angle = 0.1; // Pitch D-gain - angle mode (has no effect on controlANGLE2) float B_loop_pitch = 0.9; // Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) -float Kp_roll_rate = 0.15; // Roll P-gain - rate mode -float Ki_roll_rate = 0.2; // Roll I-gain - rate mode +float Kp_roll_rate = 0.015; // Roll P-gain - rate mode +float Ki_roll_rate = 0.02; // Roll I-gain - rate mode float Kd_roll_rate = 0.0002; // Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) -float Kp_pitch_rate = 0.15; // Pitch P-gain - rate mode -float Ki_pitch_rate = 0.2; // Pitch I-gain - rate mode +float Kp_pitch_rate = 0.015; // Pitch P-gain - rate mode +float Ki_pitch_rate = 0.02; // Pitch I-gain - rate mode float Kd_pitch_rate = 0.0002; // Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) float Kp_yaw = 0.3; // Yaw P-gain @@ -358,6 +358,7 @@ void printDebugInfo(); void setupMotorPWM(); int pulseWidthToDutyCycle(int pulseWidth); void setupMatrixSerial(); +void printPitchInfo(); //========================================================================================================================// // VOID SETUP // //========================================================================================================================// @@ -445,15 +446,16 @@ 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) + // 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) // 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) - // printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) + printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) // printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) - // printDebugInfo(); //PWM 1 and 5 + Armed condition + // printDebugInfo(); //PWM 1 and 5 + Armed conditio + // printPitchInfo(); // Get arming status armedStatus(); // Check if the throttle cut is off and throttle is low. @@ -507,15 +509,14 @@ void controlMixer() *channel_6_pwm - free auxillary channel, can be used to toggle things with an 'if' statement */ - // Quad mixing - EXAMPLE - m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; // Front Left (CW) Check - m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; // Front Right (CCW) check - m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; // Back Right (CW) check - m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; // Back Left (CCW) + // Quad mixer with pitch bias because the sensor isnt flat on the board + m1_command_scaled = thro_des - pitch_PID + roll_PID - yaw_PID; // Front Left CCW + m2_command_scaled = thro_des - pitch_PID - roll_PID + yaw_PID; // Front Right CW + m3_command_scaled = thro_des + pitch_PID - roll_PID - yaw_PID; // Back Right CCW + m4_command_scaled = thro_des + pitch_PID + roll_PID + yaw_PID; // Back Left CW m5_command_scaled = 0; m6_command_scaled = 0; - - // 0.5 is centered servo, 0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle + s1_command_scaled = 0; s2_command_scaled = 0; s3_command_scaled = 0; @@ -743,45 +744,54 @@ void calculate_IMU_error() * measurement. */ int16_t AcX, AcY, AcZ, GyX, GyY, GyZ, MgX, MgY, MgZ; - AccErrorX = 0.0; - AccErrorY = 0.0; - AccErrorZ = 0.0; - GyroErrorX = 0.0; - GyroErrorY = 0.0; - GyroErrorZ = 0.0; + // DESCRIPTION: Computes IMU accelerometer and gyro error on startup + AccErrorX = 0.0; + AccErrorY = 0.0; + AccErrorZ = 0.0; + GyroErrorX = 0.0; + GyroErrorY = 0.0; + GyroErrorZ = 0.0; - // Read IMU values 12000 times - int c = 0; - while (c < 12000) - { -#if defined USE_MPU6050_I2C + Serial.println("Starting IMU calibration..."); + + // Read IMU values 2000 times (reduced from 12000 for faster calibration) + int c = 0; + while (c < 2000) + { +#if defined USE_BNO085_I2C + unsigned long startTime = millis(); + while (!myIMU.dataAvailable()) + { + delay(1); + if (millis() - startTime > 100) // Reduced timeout + { + Serial.print("BNO085 timeout at iteration: "); + Serial.println(c); + return; // Exit if timeout + } + } + + // Get raw sensor data - BNO085 already gives calibrated values + float tempGyroX = myIMU.getGyroX() * 57.29577951f; // Convert rad/s to deg/s + float tempGyroY = myIMU.getGyroY() * 57.29577951f; + float tempGyroZ = myIMU.getGyroZ() * 57.29577951f; + + float tempAccX = myIMU.getAccelX() / 9.80665f; // Convert m/s² to g + float tempAccY = myIMU.getAccelY() / 9.80665f; + float tempAccZ = myIMU.getAccelZ() / 9.80665f; + + // Sum all readings + AccErrorX += tempAccX; + AccErrorY += tempAccY; + AccErrorZ += tempAccZ; + GyroErrorX += tempGyroX; + GyroErrorY += tempGyroY; + GyroErrorZ += tempGyroZ; + +#elif defined USE_MPU6050_I2C mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); #elif defined USE_MPU9250_SPI mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); -#elif defined USE_BNO085_I2C - - while (!myIMU.dataAvailable()) - { - delay(1); // Small delay to avoid hammering the I2C bus too hard - } - - // bno magnetometer - MgX = myIMU.getMagX(); - MgY = myIMU.getMagY(); - MgZ = myIMU.getMagZ(); - - // convert radians to degrees - GyX = myIMU.getGyroX() * 180 / PI; - GyY = myIMU.getGyroY() * 180 / PI; - GyZ = myIMU.getGyroZ() * 180 / PI; - - // Acelleration (dont know if I need linear or normal. One way to find out) - AcX = myIMU.getAccelX() / 9.80665; // m/s² to g - AcY = myIMU.getAccelY() / 9.80665; // m/s² to g - AcZ = myIMU.getAccelZ() / 9.80665; // m/s² to g - - - #endif AccX = AcX / ACCEL_SCALE_FACTOR; @@ -1365,7 +1375,7 @@ void getCommands() #endif // Low-pass the critical commands and update previous values - float b = 0.7; // Lower=slower, higher=noiser + float b = 0.5; // Lower=slower, higher=noiser channel_1_pwm = (1.0 - b) * channel_1_pwm_prev + b * channel_1_pwm; channel_2_pwm = (1.0 - b) * channel_2_pwm_prev + b * channel_2_pwm; channel_3_pwm = (1.0 - b) * channel_3_pwm_prev + b * channel_3_pwm; @@ -1891,6 +1901,20 @@ void printDebugInfo() { } } +void printPitchInfo() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print("pitch_IMU:"); + Serial.print(pitch_IMU); + Serial.print(" pitch_PID:"); + Serial.print(pitch_PID); + Serial.print(" pitch_des:"); + Serial.print(pitch_des); + Serial.print(" error_pitch:"); + Serial.println(error_pitch); + } +} + //=========================================================================================// // HELPER FUNCTIONS