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@2:91f2479501a0, 2022-05-08 (annotated)

Committer:
ahdyer
Date:
Sun May 08 16:32:55 2022 +0000
Revision:
2:91f2479501a0
Parent:
1:0acb5ca94031
Child:
3:1e25f6df1a0d
Working Alex

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 "PID.h"
Forest1213 0:7f6319a73219 10
Forest1213 0:7f6319a73219 11 PwmOut ledf(LED1); //throttle up test led with PWM dimming
Forest1213 0:7f6319a73219 12 PwmOut ledr(LED2); //throttle down test led with PWM dimming
Forest1213 0:7f6319a73219 13
Forest1213 0:7f6319a73219 14 Servo ESC0(p21); //Back Left
Forest1213 0:7f6319a73219 15 Servo ESC1(p22); //Front Left
Forest1213 0:7f6319a73219 16 Servo ESC2(p24); //Front Right
Forest1213 0:7f6319a73219 17 Servo ESC3(p23); //Back Right
Forest1213 0:7f6319a73219 18
ahdyer 2:91f2479501a0 19 uint16_t motor_min = 1100;
ahdyer 2:91f2479501a0 20
ahdyer 2:91f2479501a0 21 DigitalOut led1(LED1);
ahdyer 2:91f2479501a0 22 void Rx_interrupt();
ahdyer 2:91f2479501a0 23
Forest1213 0:7f6319a73219 24 Serial pc(USBTX, USBRX); //Initalise PC serial comms
Forest1213 0:7f6319a73219 25 Serial Receiver(p13, p14); //Initialize Receiver Serial comms
Forest1213 0:7f6319a73219 26 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
Forest1213 0:7f6319a73219 27 Ticker toggler1;
Forest1213 0:7f6319a73219 28
Forest1213 0:7f6319a73219 29 //using namespace std::chrono;
Forest1213 0:7f6319a73219 30
Forest1213 0:7f6319a73219 31 float pitchAngle = 0;
Forest1213 0:7f6319a73219 32 float rollAngle = 0;
Forest1213 0:7f6319a73219 33 float yawAngle = 0;
Forest1213 0:7f6319a73219 34
ahdyer 2:91f2479501a0 35 int arm = 0;
ahdyer 2:91f2479501a0 36
ahdyer 2:91f2479501a0 37 uint16_t pitchTarget = 0;
ahdyer 2:91f2479501a0 38 uint16_t rollTarget = 0;
Forest1213 1:0acb5ca94031 39 //float yawTarget = 0;
Forest1213 1:0acb5ca94031 40
Forest1213 1:0acb5ca94031 41 float pitchOffset = 0;
Forest1213 1:0acb5ca94031 42 float rollOffset = 0;
Forest1213 1:0acb5ca94031 43
ahdyer 2:91f2479501a0 44 PID PitchPID(0.5,0,0.05);
ahdyer 2:91f2479501a0 45 PID RollPID(0.5,0,0.05);
ahdyer 2:91f2479501a0 46 PID YawPID(2,0.01,0);
ahdyer 2:91f2479501a0 47
ahdyer 2:91f2479501a0 48 float Throttle = 0;
ahdyer 2:91f2479501a0 49
ahdyer 2:91f2479501a0 50 // Receiver variables
ahdyer 2:91f2479501a0 51 int16_t channel[14];
Forest1213 0:7f6319a73219 52
ahdyer 2:91f2479501a0 53 float pid_setpoint_roll = 0;
ahdyer 2:91f2479501a0 54 float pid_setpoint_pitch = 0;
ahdyer 2:91f2479501a0 55 float pid_setpoint_yaw = 0;
ahdyer 2:91f2479501a0 56 float pid_output_roll = 0;
ahdyer 2:91f2479501a0 57 float pid_output_pitch = 0;
ahdyer 2:91f2479501a0 58 float pid_output_yaw = 0;
ahdyer 2:91f2479501a0 59
ahdyer 2:91f2479501a0 60 int16_t gr[3];
ahdyer 2:91f2479501a0 61 int16_t acc[3];
ahdyer 2:91f2479501a0 62 int16_t grC[3];
ahdyer 2:91f2479501a0 63 int16_t accC[3];
Forest1213 1:0acb5ca94031 64
Forest1213 1:0acb5ca94031 65
Forest1213 0:7f6319a73219 66
Forest1213 0:7f6319a73219 67 Timer t;
ahdyer 2:91f2479501a0 68 Timer loop_time;
ahdyer 2:91f2479501a0 69 Timer re;
Forest1213 0:7f6319a73219 70
ahdyer 2:91f2479501a0 71 void Calculate_set_points(){
ahdyer 2:91f2479501a0 72
ahdyer 2:91f2479501a0 73 // Throttle- CHannel[2]
ahdyer 2:91f2479501a0 74 if (channel[2] > 1800){Throttle = 1800;}
ahdyer 2:91f2479501a0 75 else{Throttle = channel[2];}
ahdyer 2:91f2479501a0 76 // Roll - Y - Channel[0]
ahdyer 2:91f2479501a0 77 if (channel[0] != 0){
ahdyer 2:91f2479501a0 78 if (channel[0] > 1505){pid_setpoint_roll = channel[0] - 1505;} // max 495
ahdyer 2:91f2479501a0 79 if (channel[0] < 1495){pid_setpoint_roll = channel[0] - 1495;} // max -495
ahdyer 2:91f2479501a0 80 pid_setpoint_roll /= 1; // Max angular rate of 247 deg/s
ahdyer 2:91f2479501a0 81 }
ahdyer 2:91f2479501a0 82 else {
ahdyer 2:91f2479501a0 83 pid_setpoint_roll = 0;
ahdyer 2:91f2479501a0 84 }
Forest1213 1:0acb5ca94031 85
ahdyer 2:91f2479501a0 86 // Pitch - X - Channel[1]
ahdyer 2:91f2479501a0 87 if (channel[1] != 0){
ahdyer 2:91f2479501a0 88 if (channel[1] > 1505){pid_setpoint_pitch = 1505 - channel[1];} // max 495
ahdyer 2:91f2479501a0 89 if (channel[1] < 1495){pid_setpoint_pitch = 1495 - channel[1];} // max -495
ahdyer 2:91f2479501a0 90 pid_setpoint_pitch /= 1; // Max angular rate of 247 deg/s
ahdyer 2:91f2479501a0 91 }
ahdyer 2:91f2479501a0 92 else {
ahdyer 2:91f2479501a0 93 pid_setpoint_roll = 0;
ahdyer 2:91f2479501a0 94 }
Forest1213 1:0acb5ca94031 95
ahdyer 2:91f2479501a0 96 //Yaw - Z - Channel[3]
ahdyer 2:91f2479501a0 97 if (channel[3] != 0){
ahdyer 2:91f2479501a0 98 if (channel[3] > 1505){pid_setpoint_yaw = 1505 - channel[3];} // max 495
ahdyer 2:91f2479501a0 99 if (channel[3] < 1495){pid_setpoint_yaw = 1495 - channel[3];} // max -495
ahdyer 2:91f2479501a0 100 pid_setpoint_roll /= 1; // Max angular rate of 247 deg/s
ahdyer 2:91f2479501a0 101 }
ahdyer 2:91f2479501a0 102 else{
ahdyer 2:91f2479501a0 103 pid_setpoint_yaw = 0;
ahdyer 2:91f2479501a0 104 }
ahdyer 2:91f2479501a0 105
ahdyer 2:91f2479501a0 106 //pc.printf("Pset: %.1f, Rset %.1f, Yset %.1f ",pid_setpoint_roll, pid_setpoint_roll, pid_setpoint_roll);
Forest1213 1:0acb5ca94031 107 }
Forest1213 1:0acb5ca94031 108
ahdyer 2:91f2479501a0 109 // Interupt Routine to read in data from serial port
ahdyer 2:91f2479501a0 110 void Rx_interrupt(){
ahdyer 2:91f2479501a0 111 uint8_t byte1;
ahdyer 2:91f2479501a0 112 uint8_t high_byte;
ahdyer 2:91f2479501a0 113 uint8_t chksum_low_byte;
ahdyer 2:91f2479501a0 114 uint8_t chksum_high_byte;
ahdyer 2:91f2479501a0 115 if (Receiver.readable()) {
Forest1213 0:7f6319a73219 116 if(Receiver.getc() == 0x20) {
ahdyer 2:91f2479501a0 117 if(Receiver.getc() == 0x40){
Forest1213 1:0acb5ca94031 118 for (int i = 0; i<7; i++) {
ahdyer 2:91f2479501a0 119 byte1 = Receiver.getc();
Forest1213 0:7f6319a73219 120 //byte2 = Receiver.getc();
Forest1213 0:7f6319a73219 121 //if (i == 2)
Forest1213 0:7f6319a73219 122 // printf("Throttle: %02X %02X Sum: %d \n\r", (byte1&0x000000FF), byte2, channel[i]);
Forest1213 0:7f6319a73219 123 channel[i] = (Receiver.getc() << 8);
Forest1213 0:7f6319a73219 124 channel[i] += (byte1&0x000000FF);
ahdyer 2:91f2479501a0 125 }
ahdyer 2:91f2479501a0 126 chksum_low_byte = Receiver.getc();
ahdyer 2:91f2479501a0 127 chksum_high_byte = Receiver.getc();
ahdyer 2:91f2479501a0 128 //pc.printf("CLB %u,CHB %u \n\r", chksum_low_byte, chksum_high_byte);
ahdyer 2:91f2479501a0 129 //pc.printf("CN1: %u, CN2: %u, CN3: %u \n\r", channel[1], channel[2], channel[3]);
ahdyer 2:91f2479501a0 130 }
Forest1213 1:0acb5ca94031 131 }
Forest1213 1:0acb5ca94031 132 }
ahdyer 2:91f2479501a0 133 }
ahdyer 2:91f2479501a0 134
ahdyer 2:91f2479501a0 135 void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
ahdyer 2:91f2479501a0 136 {
ahdyer 2:91f2479501a0 137 // ESC0 | Back Left | CCW
ahdyer 2:91f2479501a0 138 // ESC1 | Front Left | CW
ahdyer 2:91f2479501a0 139 // ESC2 | Front Right | CCW
ahdyer 2:91f2479501a0 140 // ESC3 | Back Right | CW
ahdyer 2:91f2479501a0 141
ahdyer 2:91f2479501a0 142
ahdyer 2:91f2479501a0 143 //Back Left
ahdyer 2:91f2479501a0 144 float SC0 = 0;
ahdyer 2:91f2479501a0 145 SC0 = Thrust - Yaw - Pitch + Roll;
ahdyer 2:91f2479501a0 146 if(SC0 < motor_min) {
ahdyer 2:91f2479501a0 147 SC0 = motor_min;
ahdyer 2:91f2479501a0 148 }
ahdyer 2:91f2479501a0 149 if(SC0 > 2000) {
ahdyer 2:91f2479501a0 150 SC0 = 2000;
ahdyer 2:91f2479501a0 151 }
ahdyer 2:91f2479501a0 152 float NSC0 = (SC0-1003)/1003;
ahdyer 2:91f2479501a0 153 ESC0 = NSC0;
ahdyer 2:91f2479501a0 154
ahdyer 2:91f2479501a0 155 //Front Left
ahdyer 2:91f2479501a0 156 float SC1 = Thrust + Yaw + Pitch + Roll;
ahdyer 2:91f2479501a0 157 if(SC1 < motor_min) {
ahdyer 2:91f2479501a0 158 SC1 = motor_min;
ahdyer 2:91f2479501a0 159 }
ahdyer 2:91f2479501a0 160 if(SC1 > 2000) {
ahdyer 2:91f2479501a0 161 SC1 = 2000;
ahdyer 2:91f2479501a0 162 }
ahdyer 2:91f2479501a0 163 float NSC1 = (SC1-1003)/1003;
ahdyer 2:91f2479501a0 164 ESC1 = NSC1;
ahdyer 2:91f2479501a0 165
ahdyer 2:91f2479501a0 166 //Front Right
ahdyer 2:91f2479501a0 167 float SC2 = Thrust - Yaw + Pitch - Roll;
ahdyer 2:91f2479501a0 168 if(SC2 < motor_min) {
ahdyer 2:91f2479501a0 169 SC2 = motor_min;
ahdyer 2:91f2479501a0 170 }
ahdyer 2:91f2479501a0 171 if(SC2 > 2000) {
ahdyer 2:91f2479501a0 172 SC2 = 2000;
ahdyer 2:91f2479501a0 173 }
ahdyer 2:91f2479501a0 174 float NSC2 = (SC2-1003)/1003;
ahdyer 2:91f2479501a0 175 ESC2 = NSC2;
ahdyer 2:91f2479501a0 176
ahdyer 2:91f2479501a0 177 //Back Right
ahdyer 2:91f2479501a0 178 float SC3 = Thrust + Yaw - Pitch - Roll;
ahdyer 2:91f2479501a0 179 if(SC3 < motor_min) {
ahdyer 2:91f2479501a0 180 SC3 = motor_min;
ahdyer 2:91f2479501a0 181 }
ahdyer 2:91f2479501a0 182 if(SC3 > 2000) {
ahdyer 2:91f2479501a0 183 SC3 = 2000;
ahdyer 2:91f2479501a0 184 }
ahdyer 2:91f2479501a0 185 float NSC3 = (SC3-1003)/1003;
ahdyer 2:91f2479501a0 186 ESC3 = NSC3;
ahdyer 2:91f2479501a0 187
ahdyer 2:91f2479501a0 188
ahdyer 2:91f2479501a0 189 //pc.printf("BL:%.3f, FL:%.3f, FR:%.3f, BR:%.3f ", SC0, SC1, SC2, SC3);
ahdyer 2:91f2479501a0 190 //pc.printf("NBL:%.3f, NFL:%.3f, NFR:%.3f, NBR:%.3f ", NSC0, NSC1, NSC2, NSC3);
ahdyer 2:91f2479501a0 191 }
ahdyer 2:91f2479501a0 192
ahdyer 2:91f2479501a0 193 int main()
ahdyer 2:91f2479501a0 194 {
ahdyer 2:91f2479501a0 195 // Setup a serial interrupt function to receive data
ahdyer 2:91f2479501a0 196 led1 = 1;
ahdyer 2:91f2479501a0 197 Receiver.baud(115200);
ahdyer 2:91f2479501a0 198 pc.baud (115200);
ahdyer 2:91f2479501a0 199 Receiver.attach(&Rx_interrupt);
ahdyer 2:91f2479501a0 200
ahdyer 2:91f2479501a0 201 for (int c = 0; c<14; c++) {
ahdyer 2:91f2479501a0 202 channel[c] = 0;
ahdyer 2:91f2479501a0 203 }
ahdyer 2:91f2479501a0 204 ESC0 = ESC1 = ESC2 = ESC3 = 0;
ahdyer 2:91f2479501a0 205 wait(0.5);
ahdyer 2:91f2479501a0 206 ESC0 = ESC1 = ESC2 = ESC3 = 1100;
ahdyer 2:91f2479501a0 207 wait(0.5);
ahdyer 2:91f2479501a0 208 ESC0 = ESC1 = ESC2 = ESC3 = 0;
ahdyer 2:91f2479501a0 209 wait(0.5);
ahdyer 2:91f2479501a0 210
ahdyer 2:91f2479501a0 211
ahdyer 2:91f2479501a0 212 //
ahdyer 2:91f2479501a0 213 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
ahdyer 2:91f2479501a0 214 wait(1);
ahdyer 2:91f2479501a0 215 mpu6050.init();
ahdyer 2:91f2479501a0 216 wait(0.5); // Initialize the sensor
ahdyer 2:91f2479501a0 217 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
ahdyer 2:91f2479501a0 218 pc.printf("Calibration is completed. \r\n");
ahdyer 2:91f2479501a0 219 wait(1);
ahdyer 2:91f2479501a0 220 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
ahdyer 2:91f2479501a0 221 wait_ms(500);
ahdyer 2:91f2479501a0 222
ahdyer 2:91f2479501a0 223
ahdyer 2:91f2479501a0 224
ahdyer 2:91f2479501a0 225
ahdyer 2:91f2479501a0 226 //pc.printf("%f ,%f",Roll Angle, pitchAngle)
ahdyer 2:91f2479501a0 227
ahdyer 2:91f2479501a0 228 PitchPID.setLimit(-250,250);
ahdyer 2:91f2479501a0 229 RollPID.setLimit(-250,250);
ahdyer 2:91f2479501a0 230 YawPID.setLimit(-250,250);
ahdyer 2:91f2479501a0 231 //YawPID.setLimit(-0.25,0.25);
ahdyer 2:91f2479501a0 232
ahdyer 2:91f2479501a0 233 pc.printf("Ready to go...\n\r");
ahdyer 2:91f2479501a0 234 led1 = 0;
ahdyer 2:91f2479501a0 235
ahdyer 2:91f2479501a0 236
ahdyer 2:91f2479501a0 237
ahdyer 2:91f2479501a0 238 while (1) {
ahdyer 2:91f2479501a0 239 loop_time.start();
ahdyer 2:91f2479501a0 240 t.start();
Forest1213 1:0acb5ca94031 241
ahdyer 2:91f2479501a0 242 // TO ADD
ahdyer 2:91f2479501a0 243 // Timer timeout for reciving new commands, if after 10 s no new commands cut motor output
ahdyer 2:91f2479501a0 244 // AUTO LEVEL
ahdyer 2:91f2479501a0 245 // Read MPU Values with complimantary filter allready applied
ahdyer 2:91f2479501a0 246 //mpu6050.complementaryFilter(&pitchAngle, &rollAngle, &yawAngle);
ahdyer 2:91f2479501a0 247 //pc.printf("Pitch: %.3f, Roll: %.3f, Yaw: %.3f \n\r", pitchAngle, rollAngle, yawAngle);
ahdyer 2:91f2479501a0 248
ahdyer 2:91f2479501a0 249 // RATE MODE
ahdyer 2:91f2479501a0 250 mpu6050.readAccelData(acc);
ahdyer 2:91f2479501a0 251 mpu6050.readGyroData(gr);
ahdyer 2:91f2479501a0 252 accC[0] = ((acc[0]-accelBias[0])/16384);
ahdyer 2:91f2479501a0 253 grC[0] = ((gr[0]-gyroBias[0])/65.536);
ahdyer 2:91f2479501a0 254
ahdyer 2:91f2479501a0 255 accC[1] = ((acc[1]-accelBias[1])/16384);
ahdyer 2:91f2479501a0 256 grC[1] = ((gr[1]-gyroBias[1])/65.536);
ahdyer 2:91f2479501a0 257
ahdyer 2:91f2479501a0 258 accC[2] = ((acc[2]-accelBias[2])/16384);
ahdyer 2:91f2479501a0 259 grC[2] = ((gr[2]-gyroBias[2])/65.536)*-1;
ahdyer 2:91f2479501a0 260
ahdyer 2:91f2479501a0 261 pitchAngle = (pitchAngle*0.7) + (grC[0]*0.3);
ahdyer 2:91f2479501a0 262 rollAngle = (rollAngle*0.7) + (grC[1]*0.3);
ahdyer 2:91f2479501a0 263 yawAngle = (yawAngle*0.7) + (grC[2]*0.3);
ahdyer 2:91f2479501a0 264
ahdyer 2:91f2479501a0 265 //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]);
ahdyer 2:91f2479501a0 266
ahdyer 2:91f2479501a0 267 Calculate_set_points();
ahdyer 2:91f2479501a0 268
ahdyer 2:91f2479501a0 269 // Set Reciver channels
ahdyer 2:91f2479501a0 270
ahdyer 2:91f2479501a0 271
ahdyer 2:91f2479501a0 272 //pc.printf("T:%.1f R:%.1f P:%.1f ARM: %u \n\r",Throttle, pid_setpoint_roll, pid_setpoint_pitch, channel[6]);
ahdyer 2:91f2479501a0 273
ahdyer 2:91f2479501a0 274 // Set PIDS
ahdyer 2:91f2479501a0 275 pid_output_roll = RollPID.computeOutput(pid_setpoint_roll,rollAngle);
ahdyer 2:91f2479501a0 276 pid_output_pitch = PitchPID.computeOutput(pid_setpoint_pitch,pitchAngle);
ahdyer 2:91f2479501a0 277 pid_output_yaw = YawPID.computeOutput(pid_setpoint_yaw,yawAngle);
ahdyer 2:91f2479501a0 278 //pc.printf("Radj: %.1f, Padj %.1f ",pid_output_roll, pid_output_pitch);
ahdyer 2:91f2479501a0 279 // Yaw angle is currently very broken and constantly rises (Check MPU6050 complimentry filter function as it looks
ahdyer 2:91f2479501a0 280 // like it doesnt apply the z axis config offset values)
ahdyer 2:91f2479501a0 281 //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
Forest1213 1:0acb5ca94031 282
Forest1213 1:0acb5ca94031 283
ahdyer 2:91f2479501a0 284 // Update Motor PWM if the arming switch is triggered
ahdyer 2:91f2479501a0 285 if (channel[6] > 1600){UpdateMotors(Throttle,pid_output_yaw, pid_output_pitch, pid_output_roll);}
Forest1213 1:0acb5ca94031 286 else{
ahdyer 2:91f2479501a0 287 ESC0 = 0;
ahdyer 2:91f2479501a0 288 ESC1 = 0;
ahdyer 2:91f2479501a0 289 ESC2 = 0;
ahdyer 2:91f2479501a0 290 ESC3 = 0;
ahdyer 2:91f2479501a0 291 }
ahdyer 2:91f2479501a0 292
ahdyer 2:91f2479501a0 293
ahdyer 2:91f2479501a0 294
ahdyer 2:91f2479501a0 295
ahdyer 2:91f2479501a0 296
ahdyer 2:91f2479501a0 297
ahdyer 2:91f2479501a0 298
ahdyer 2:91f2479501a0 299
ahdyer 2:91f2479501a0 300
ahdyer 2:91f2479501a0 301
ahdyer 2:91f2479501a0 302
ahdyer 2:91f2479501a0 303
ahdyer 2:91f2479501a0 304 //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
ahdyer 2:91f2479501a0 305 //yawAdjustment*=Ykc;
ahdyer 2:91f2479501a0 306 //printf("Adj: %f, Targ: %f, Ang: %f\n\r",yawAdjustment,yawTarget,yawAngle);
ahdyer 2:91f2479501a0 307 //printf("%f\n\r",yawAdjustment);
ahdyer 2:91f2479501a0 308 // Limit pitch adjustment if needed
ahdyer 2:91f2479501a0 309
ahdyer 2:91f2479501a0 310 //if (arm) {}
ahdyer 2:91f2479501a0 311
ahdyer 2:91f2479501a0 312 t.stop();
ahdyer 2:91f2479501a0 313 //pc.printf("Loop time %f \n\r", t.read());
ahdyer 2:91f2479501a0 314 if(t.read()*1000 > 3) {
ahdyer 2:91f2479501a0 315 pc.printf("LOOP TIME TOO LONG!");
ahdyer 2:91f2479501a0 316 led1 = 1;
ahdyer 2:91f2479501a0 317 } else {
ahdyer 2:91f2479501a0 318 wait_ms(3.04-t.read()*1000);
ahdyer 2:91f2479501a0 319 }
ahdyer 2:91f2479501a0 320 t.reset();
ahdyer 2:91f2479501a0 321 loop_time.stop();
ahdyer 2:91f2479501a0 322 //pc.printf("Loop time %f \n\r", loop_time.read());
ahdyer 2:91f2479501a0 323 loop_time.reset();
Forest1213 1:0acb5ca94031 324 }
Forest1213 0:7f6319a73219 325 }
Forest1213 0:7f6319a73219 326
Forest1213 0:7f6319a73219 327 //float MotorCheck(float Motor){
Forest1213 0:7f6319a73219 328
Forest1213 0:7f6319a73219 329 //}
Forest1213 0:7f6319a73219 330