Glen Chadburn / Mbed 2 deprecated 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

main.cpp@0:7f6319a73219, 2022-04-28 (annotated)

Committer:
Forest1213
Date:
Thu Apr 28 23:16:23 2022 +0000
Revision:
0:7f6319a73219
Child:
1:0acb5ca94031
V1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Forest1213 0:7f6319a73219 1 // Calibrate and arm ESC and then sweep through motor speed range
Forest1213 0:7f6319a73219 2 // To run: Hold down reset on mbed and power up ESC motor supply
Forest1213 0:7f6319a73219 3 // Wait for three power up beeps from ESC, then release reset (or apply power) on mbed
Forest1213 0:7f6319a73219 4 // See https://github.com/bitdump/BLHeli/blob/master/BLHeli_S%20SiLabs/BLHeli_S%20manual%20SiLabs%20Rev16.x.pdf
Forest1213 0:7f6319a73219 5 // for info on beep codes and calibration
Forest1213 0:7f6319a73219 6 #include "mbed.h"
Forest1213 0:7f6319a73219 7 #include "Servo.h"
Forest1213 0:7f6319a73219 8 #include "MPU6050.h"
Forest1213 0:7f6319a73219 9 #include "ledControl.h"
Forest1213 0:7f6319a73219 10 #include "PID.h"
Forest1213 0:7f6319a73219 11
Forest1213 0:7f6319a73219 12 PwmOut ledf(LED1); //throttle up test led with PWM dimming
Forest1213 0:7f6319a73219 13 PwmOut ledr(LED2); //throttle down test led with PWM dimming
Forest1213 0:7f6319a73219 14
Forest1213 0:7f6319a73219 15 Servo ESC0(p21); //Back Left
Forest1213 0:7f6319a73219 16 Servo ESC1(p22); //Front Left
Forest1213 0:7f6319a73219 17 Servo ESC2(p24); //Front Right
Forest1213 0:7f6319a73219 18 Servo ESC3(p23); //Back Right
Forest1213 0:7f6319a73219 19
Forest1213 0:7f6319a73219 20 Serial pc(USBTX, USBRX); //Initalise PC serial comms
Forest1213 0:7f6319a73219 21 Serial Receiver(p13, p14); //Initialize Receiver Serial comms
Forest1213 0:7f6319a73219 22 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
Forest1213 0:7f6319a73219 23 Ticker toggler1;
Forest1213 0:7f6319a73219 24 Ticker filter;
Forest1213 0:7f6319a73219 25
Forest1213 0:7f6319a73219 26 //using namespace std::chrono;
Forest1213 0:7f6319a73219 27
Forest1213 0:7f6319a73219 28 void toggle_led1();
Forest1213 0:7f6319a73219 29 void toggle_led2();
Forest1213 0:7f6319a73219 30 void compFilter();
Forest1213 0:7f6319a73219 31
Forest1213 0:7f6319a73219 32 float pitchAngle = 0;
Forest1213 0:7f6319a73219 33 float rollAngle = 0;
Forest1213 0:7f6319a73219 34 float yawAngle = 0;
Forest1213 0:7f6319a73219 35
Forest1213 0:7f6319a73219 36 float pitchTarget = 0;
Forest1213 0:7f6319a73219 37 float rollTarget = 0;
Forest1213 0:7f6319a73219 38 float yawTarget = 0;
Forest1213 0:7f6319a73219 39
Forest1213 0:7f6319a73219 40 PID PitchPID(0.5,1,0.01);
Forest1213 0:7f6319a73219 41 PID RollPID(0.5,1,0.01);
Forest1213 0:7f6319a73219 42 PID YawPID(0.5,1,0.01);
Forest1213 0:7f6319a73219 43
Forest1213 0:7f6319a73219 44 float ThrustValue = 0;
Forest1213 0:7f6319a73219 45
Forest1213 0:7f6319a73219 46 // Receiver variables
Forest1213 0:7f6319a73219 47 uint16_t channel[14];
Forest1213 0:7f6319a73219 48
Forest1213 0:7f6319a73219 49 Timer t;
Forest1213 0:7f6319a73219 50
Forest1213 0:7f6319a73219 51 int main()
Forest1213 0:7f6319a73219 52 {
Forest1213 0:7f6319a73219 53 ESC0 = ESC1 = ESC2 = ESC3 = 0;
Forest1213 0:7f6319a73219 54 ledf = ledr = 0;
Forest1213 0:7f6319a73219 55 ESC0 = ESC1 = ESC2 = ESC3 = 0;
Forest1213 0:7f6319a73219 56 wait(0.5);
Forest1213 0:7f6319a73219 57 ESC0 = ESC1 = ESC2 = ESC3 = 0.2;
Forest1213 0:7f6319a73219 58 wait(0.5);
Forest1213 0:7f6319a73219 59 ESC0 = ESC1 = ESC2 = ESC3 = 0;
Forest1213 0:7f6319a73219 60 wait(0.5);
Forest1213 0:7f6319a73219 61
Forest1213 0:7f6319a73219 62 uint8_t byte1, byte2;
Forest1213 0:7f6319a73219 63 //bool ready_to_arm = false;
Forest1213 0:7f6319a73219 64 pc.baud (115200);
Forest1213 0:7f6319a73219 65 Receiver.baud(115200);
Forest1213 0:7f6319a73219 66
Forest1213 0:7f6319a73219 67 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
Forest1213 0:7f6319a73219 68 wait(1);
Forest1213 0:7f6319a73219 69 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
Forest1213 0:7f6319a73219 70 pc.printf("Calibration is completed. \r\n");
Forest1213 0:7f6319a73219 71 wait(0.5);
Forest1213 0:7f6319a73219 72 mpu6050.init(); // Initialize the sensor
Forest1213 0:7f6319a73219 73 wait(1);
Forest1213 0:7f6319a73219 74 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
Forest1213 0:7f6319a73219 75 wait_ms(500);
Forest1213 0:7f6319a73219 76 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
Forest1213 0:7f6319a73219 77 rollTarget = rollAngle;
Forest1213 0:7f6319a73219 78 pitchTarget = pitchAngle;
Forest1213 0:7f6319a73219 79 yawTarget = yawAnglw;
Forest1213 0:7f6319a73219 80
Forest1213 0:7f6319a73219 81 PitchPID.setLimit(-0.25,0.25);
Forest1213 0:7f6319a73219 82 RollPID.setLimit(-0.25,0.25);
Forest1213 0:7f6319a73219 83 YawPID.setLimit(-0.25,0.25);
Forest1213 0:7f6319a73219 84
Forest1213 0:7f6319a73219 85 pc.printf("Ready to go...\n\r");
Forest1213 0:7f6319a73219 86
Forest1213 0:7f6319a73219 87 float rollAdjustment = 0;
Forest1213 0:7f6319a73219 88 float pitchAdjustment = 0;
Forest1213 0:7f6319a73219 89 float yawAdjustment = 0;
Forest1213 0:7f6319a73219 90
Forest1213 0:7f6319a73219 91 while (1) {
Forest1213 0:7f6319a73219 92 t.start();
Forest1213 0:7f6319a73219 93
Forest1213 0:7f6319a73219 94 //PitchPID.setProcessValue(pitchAngle);
Forest1213 0:7f6319a73219 95 //pc.printf("| Pitch: %.5f | Output:%.5f |\r\n",pitchAngle,PitchPID.compute());
Forest1213 0:7f6319a73219 96
Forest1213 0:7f6319a73219 97 //PitchPID.setProcessValue(rollAngle);
Forest1213 0:7f6319a73219 98 //pc.printf("| Roll: %.5f | Output:%.5f |\r\n\n",rollAngle,PitchPID.compute());
Forest1213 0:7f6319a73219 99
Forest1213 0:7f6319a73219 100 //float imuRoll = 0;
Forest1213 0:7f6319a73219 101 //float imuPitch = 0;
Forest1213 0:7f6319a73219 102
Forest1213 0:7f6319a73219 103 if (Receiver.readable()) {
Forest1213 0:7f6319a73219 104 if(Receiver.getc() == 0x20) {
Forest1213 0:7f6319a73219 105 if(Receiver.getc() == 0x40) {
Forest1213 0:7f6319a73219 106 //printf("Message Recieved! \n");
Forest1213 0:7f6319a73219 107 for (int i = 0; i<3; i++) {
Forest1213 0:7f6319a73219 108 byte1 = Receiver.getc();
Forest1213 0:7f6319a73219 109 //byte2 = Receiver.getc();
Forest1213 0:7f6319a73219 110 //if (i == 2)
Forest1213 0:7f6319a73219 111 // printf("Throttle: %02X %02X Sum: %d \n\r", (byte1&0x000000FF), byte2, channel[i]);
Forest1213 0:7f6319a73219 112 channel[i] = (Receiver.getc() << 8);
Forest1213 0:7f6319a73219 113 channel[i] += (byte1&0x000000FF);
Forest1213 0:7f6319a73219 114 }
Forest1213 0:7f6319a73219 115
Forest1213 0:7f6319a73219 116 //channel[2] -= 1000; //Motor
Forest1213 0:7f6319a73219 117 ThrustValue = (channel[2]-1000)*0.001;
Forest1213 0:7f6319a73219 118
Forest1213 0:7f6319a73219 119 rollAdjustment = RollPID(rollTarget,rollAngle);
Forest1213 0:7f6319a73219 120 // Limit roll adjustment if needed
Forest1213 0:7f6319a73219 121
Forest1213 0:7f6319a73219 122 pitchAdjustment = PitchPID(pitchTarget,pitchAngle);
Forest1213 0:7f6319a73219 123 // Limit pitch adjustment if needed
Forest1213 0:7f6319a73219 124
Forest1213 0:7f6319a73219 125 yawAdjustment = YawPID(yawTarget,yawAngle);
Forest1213 0:7f6319a73219 126 // Limit pitch adjustment if needed
Forest1213 0:7f6319a73219 127 }
Forest1213 0:7f6319a73219 128 }
Forest1213 0:7f6319a73219 129 t.stop();
Forest1213 0:7f6319a73219 130 pc.printf("The time taken was %.5f seconds\n\r", t.read());
Forest1213 0:7f6319a73219 131 UpdateMotors(ThrustValue, yawAngle, pitchAngle, rollAngle);
Forest1213 0:7f6319a73219 132 }
Forest1213 0:7f6319a73219 133 }
Forest1213 0:7f6319a73219 134 }
Forest1213 0:7f6319a73219 135
Forest1213 0:7f6319a73219 136 void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
Forest1213 0:7f6319a73219 137 {
Forest1213 0:7f6319a73219 138 //Back Left
Forest1213 0:7f6319a73219 139 ESC0 = Thrust + Yaw - Pitch - Roll;
Forest1213 0:7f6319a73219 140
Forest1213 0:7f6319a73219 141 //Front Left
Forest1213 0:7f6319a73219 142 ESC1 = Thrust - Yaw + Pitch - Roll;
Forest1213 0:7f6319a73219 143
Forest1213 0:7f6319a73219 144 //Front Right
Forest1213 0:7f6319a73219 145 ESC2 = Thrust + Yaw + Pitch + Roll;
Forest1213 0:7f6319a73219 146
Forest1213 0:7f6319a73219 147 //Back Right
Forest1213 0:7f6319a73219 148 ESC3 = Thrust + Yaw - Pitch + Roll;
Forest1213 0:7f6319a73219 149 }
Forest1213 0:7f6319a73219 150
Forest1213 0:7f6319a73219 151 //float MotorCheck(float Motor){
Forest1213 0:7f6319a73219 152
Forest1213 0:7f6319a73219 153 //}
Forest1213 0:7f6319a73219 154
Forest1213 0:7f6319a73219 155 void toggle_led1()
Forest1213 0:7f6319a73219 156 {
Forest1213 0:7f6319a73219 157 ledToggle(1);
Forest1213 0:7f6319a73219 158 }
Forest1213 0:7f6319a73219 159 void toggle_led2()
Forest1213 0:7f6319a73219 160 {
Forest1213 0:7f6319a73219 161 ledToggle(2);
Forest1213 0:7f6319a73219 162 }
Forest1213 0:7f6319a73219 163
Forest1213 0:7f6319a73219 164 /* This function is created to avoid address error that caused from Ticker.attach func */
Forest1213 0:7f6319a73219 165 void compFilter()
Forest1213 0:7f6319a73219 166 {
Forest1213 0:7f6319a73219 167 mpu6050.complementaryFilter(&pitchAngle, &rollAngle, &yawAngle);
Forest1213 0:7f6319a73219 168
Forest1213 0:7f6319a73219 169 }