Glen Chadburn
/
Mbed-Drone
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed Servo PID MPU6050
Revision 3:1e25f6df1a0d, committed 2022-05-10
- Comitter:
- Forest1213
- Date:
- Tue May 10 22:32:12 2022 +0000
- Parent:
- 2:91f2479501a0
- Commit message:
- Commented
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun May 08 16:32:55 2022 +0000
+++ b/main.cpp Tue May 10 22:32:12 2022 +0000
@@ -16,28 +16,23 @@
Servo ESC2(p24); //Front Right
Servo ESC3(p23); //Back Right
-uint16_t motor_min = 1100;
+uint16_t motor_min = 1100; //Minimum motor speed
DigitalOut led1(LED1);
-void Rx_interrupt();
+void Rx_interrupt(); //Establish Reciever interupt
Serial pc(USBTX, USBRX); //Initalise PC serial comms
Serial Receiver(p13, p14); //Initialize Receiver Serial comms
MPU6050 mpu6050; // class: MPU6050, object: mpu6050
Ticker toggler1;
-//using namespace std::chrono;
-
float pitchAngle = 0;
float rollAngle = 0;
float yawAngle = 0;
-
int arm = 0;
-
uint16_t pitchTarget = 0;
uint16_t rollTarget = 0;
//float yawTarget = 0;
-
float pitchOffset = 0;
float rollOffset = 0;
@@ -62,10 +57,8 @@
int16_t grC[3];
int16_t accC[3];
-
-
Timer t;
-Timer loop_time;
+Timer loop_time; //Loop timer initialisation
Timer re;
void Calculate_set_points(){
@@ -117,8 +110,6 @@
if(Receiver.getc() == 0x40){
for (int i = 0; i<7; i++) {
byte1 = Receiver.getc();
- //byte2 = Receiver.getc();
- //if (i == 2)
// printf("Throttle: %02X %02X Sum: %d \n\r", (byte1&0x000000FF), byte2, channel[i]);
channel[i] = (Receiver.getc() << 8);
channel[i] += (byte1&0x000000FF);
@@ -185,7 +176,6 @@
float NSC3 = (SC3-1003)/1003;
ESC3 = NSC3;
-
//pc.printf("BL:%.3f, FL:%.3f, FR:%.3f, BR:%.3f ", SC0, SC1, SC2, SC3);
//pc.printf("NBL:%.3f, NFL:%.3f, NFR:%.3f, NBR:%.3f ", NSC0, NSC1, NSC2, NSC3);
}
@@ -193,14 +183,15 @@
int main()
{
// Setup a serial interrupt function to receive data
- led1 = 1;
+ led1 = 1; //Turn on LED during calibration
Receiver.baud(115200);
pc.baud (115200);
- Receiver.attach(&Rx_interrupt);
+ Receiver.attach(&Rx_interrupt); //Set up reciever serial bus interupt
for (int c = 0; c<14; c++) {
- channel[c] = 0;
+ channel[c] = 0; //Reset all channels to 0 at initialisation
}
+ //Arm motors
ESC0 = ESC1 = ESC2 = ESC3 = 0;
wait(0.5);
ESC0 = ESC1 = ESC2 = ESC3 = 1100;
@@ -208,35 +199,32 @@
ESC0 = ESC1 = ESC2 = ESC3 = 0;
wait(0.5);
-
- //
- mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
+ // Check and calibrate IMU
+ mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
wait(1);
mpu6050.init();
wait(0.5); // Initialize the sensor
mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
- pc.printf("Calibration is completed. \r\n");
+ //pc.printf("Calibration is completed. \r\n");
wait(1);
- pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+ //pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
wait_ms(500);
-
-
-
//pc.printf("%f ,%f",Roll Angle, pitchAngle)
+ // Set PID limits
PitchPID.setLimit(-250,250);
RollPID.setLimit(-250,250);
YawPID.setLimit(-250,250);
//YawPID.setLimit(-0.25,0.25);
- pc.printf("Ready to go...\n\r");
- led1 = 0;
+ //pc.printf("Ready to go...\n\r");
+ led1 = 0; //Turn LED off after calibration
while (1) {
- loop_time.start();
+ loop_time.start(); //Start loop timer
t.start();
// TO ADD
@@ -247,8 +235,10 @@
//pc.printf("Pitch: %.3f, Roll: %.3f, Yaw: %.3f \n\r", pitchAngle, rollAngle, yawAngle);
// RATE MODE
+ //Read IMU data
mpu6050.readAccelData(acc);
mpu6050.readGyroData(gr);
+ //Apply bias'
accC[0] = ((acc[0]-accelBias[0])/16384);
grC[0] = ((gr[0]-gyroBias[0])/65.536);
@@ -258,16 +248,14 @@
accC[2] = ((acc[2]-accelBias[2])/16384);
grC[2] = ((gr[2]-gyroBias[2])/65.536)*-1;
+ //Use gyro to calculate rate of change in angles
pitchAngle = (pitchAngle*0.7) + (grC[0]*0.3);
rollAngle = (rollAngle*0.7) + (grC[1]*0.3);
yawAngle = (yawAngle*0.7) + (grC[2]*0.3);
//pc.printf("ac_x: %d ac_y %d ac_z %d gy_x %d gy_y %d gy_z %d \r\n", accC[0], accC[1], accC[2], grC[0], grC[1], grC[2]);
- Calculate_set_points();
-
- // Set Reciver channels
-
+ Calculate_set_points();//Calculate values to input to PID
//pc.printf("T:%.1f R:%.1f P:%.1f ARM: %u \n\r",Throttle, pid_setpoint_roll, pid_setpoint_pitch, channel[6]);
@@ -276,7 +264,7 @@
pid_output_pitch = PitchPID.computeOutput(pid_setpoint_pitch,pitchAngle);
pid_output_yaw = YawPID.computeOutput(pid_setpoint_yaw,yawAngle);
//pc.printf("Radj: %.1f, Padj %.1f ",pid_output_roll, pid_output_pitch);
- // Yaw angle is currently very broken and constantly rises (Check MPU6050 complimentry filter function as it looks
+ // Yaw angle currently constantly rises (Check MPU6050 complimentry filter function as it looks
// like it doesnt apply the z axis config offset values)
//yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
@@ -289,42 +277,19 @@
ESC2 = 0;
ESC3 = 0;
}
-
-
-
-
-
-
-
-
-
-
-
- //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
- //yawAdjustment*=Ykc;
- //printf("Adj: %f, Targ: %f, Ang: %f\n\r",yawAdjustment,yawTarget,yawAngle);
- //printf("%f\n\r",yawAdjustment);
- // Limit pitch adjustment if needed
-
- //if (arm) {}
-
- t.stop();
+ t.stop(); //End loop timer
//pc.printf("Loop time %f \n\r", t.read());
- if(t.read()*1000 > 3) {
- pc.printf("LOOP TIME TOO LONG!");
+ if(t.read()*1000 > 3) { //If loop time is too long warn user
+ //pc.printf("LOOP TIME TOO LONG!");
led1 = 1;
- } else {
+ } else { //Otherwise ensure loop is consistantly timed
wait_ms(3.04-t.read()*1000);
}
- t.reset();
+ t.reset(); //Reset the loop timer
loop_time.stop();
//pc.printf("Loop time %f \n\r", loop_time.read());
loop_time.reset();
}
}
-//float MotorCheck(float Motor){
-
-//}
-