Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v3

Updated version with comments

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller_v3 by Marco Rubio

vessel.h@9:9aaa7f0c8960, 2016-07-30 (annotated)

Committer:
gelmes
Date:
Sat Jul 30 17:19:21 2016 +0000
Revision:
9:9aaa7f0c8960
Parent:
8:2db98df6fbbb
Child:
10:8cd741a65646
Untested Switching Program;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gelmes 3:5ffe7e9c0bb3 1 #ifndef VESSEL_H
gelmes 3:5ffe7e9c0bb3 2 #define VESSEL_H
gelmes 3:5ffe7e9c0bb3 3
gelmes 3:5ffe7e9c0bb3 4 #include "mbed.h"
aolgu003 6:b45b74fd6a07 5 #include "MODSERIAL.h"
gelmes 3:5ffe7e9c0bb3 6 #include "MPU6050.h"
gelmes 3:5ffe7e9c0bb3 7 #include "Servo.h"
gelmes 3:5ffe7e9c0bb3 8 #include "IMU.h"
gelmes 3:5ffe7e9c0bb3 9 #include "PID.h"
aolgu003 6:b45b74fd6a07 10 #include <string>
aolgu003 7:396fa2a8648d 11 #include "MS5837.h"
aolgu003 7:396fa2a8648d 12
aolgu003 7:396fa2a8648d 13 MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);
aolgu003 7:396fa2a8648d 14
aolgu003 6:b45b74fd6a07 15 #define BUFFER_SIZE 255
aolgu003 6:b45b74fd6a07 16
gelmes 3:5ffe7e9c0bb3 17 class Vessel
gelmes 3:5ffe7e9c0bb3 18 {
gelmes 3:5ffe7e9c0bb3 19
gelmes 3:5ffe7e9c0bb3 20 private:
aolgu003 7:396fa2a8648d 21 Servo m0;
aolgu003 7:396fa2a8648d 22 Servo m1;
aolgu003 7:396fa2a8648d 23 Servo m2;
aolgu003 7:396fa2a8648d 24 Servo m3;
aolgu003 7:396fa2a8648d 25 Servo m4;
aolgu003 7:396fa2a8648d 26 Servo m5;
aolgu003 7:396fa2a8648d 27 Servo m6;
aolgu003 7:396fa2a8648d 28 Servo m7;
aolgu003 7:396fa2a8648d 29
aolgu003 7:396fa2a8648d 30 AnalogIn powerPin;
aolgu003 7:396fa2a8648d 31 int motorState;
gelmes 9:9aaa7f0c8960 32 int runningState;
gelmes 9:9aaa7f0c8960 33 Timer runningTime;
gelmes 9:9aaa7f0c8960 34
aolgu003 7:396fa2a8648d 35 // PwmOut m0;
aolgu003 7:396fa2a8648d 36 // PwmOut m1;
aolgu003 7:396fa2a8648d 37 // PwmOut m2;
aolgu003 7:396fa2a8648d 38 // PwmOut m3;
aolgu003 7:396fa2a8648d 39 // PwmOut m4;
aolgu003 7:396fa2a8648d 40 // PwmOut m5;
aolgu003 7:396fa2a8648d 41 // PwmOut m6;
aolgu003 7:396fa2a8648d 42 // PwmOut m7;
gelmes 3:5ffe7e9c0bb3 43
gelmes 3:5ffe7e9c0bb3 44 PwmOut led1;
gelmes 3:5ffe7e9c0bb3 45 MPU6050 mpu6050;
aolgu003 7:396fa2a8648d 46
gelmes 9:9aaa7f0c8960 47 double yawPoint, yawIn, yawOut, lastyawPoint;
gelmes 9:9aaa7f0c8960 48 double rollPoint, rollIn, rollOut, lastrollPoint;
gelmes 9:9aaa7f0c8960 49 double pitchPoint, pitchIn, pitchOut, lastpitchPoint;
gelmes 5:07bbe020eb65 50 double xPoint, xIn, xOut;
gelmes 5:07bbe020eb65 51 double yPoint, yIn, yOut;
gelmes 5:07bbe020eb65 52 double zPoint, zIn, zOut;
aolgu003 7:396fa2a8648d 53 double dPoint, dIn, dOut;
aolgu003 6:b45b74fd6a07 54 double p_gain, i_gain, d_gain;
aolgu003 7:396fa2a8648d 55 PID pidy, pidr, pidp, pidX, pidY, pidZ, pidd;
aolgu003 6:b45b74fd6a07 56 char buffer[BUFFER_SIZE];
gelmes 3:5ffe7e9c0bb3 57 public:
aolgu003 7:396fa2a8648d 58 float depth;
aolgu003 7:396fa2a8648d 59
gelmes 3:5ffe7e9c0bb3 60 void Start_IMU() {
gelmes 3:5ffe7e9c0bb3 61 pc.printf("Starting up\n\r");
gelmes 3:5ffe7e9c0bb3 62 pc.baud(9600);
gelmes 3:5ffe7e9c0bb3 63 i2c.frequency(400000); // use fast (400 kHz) I2C
gelmes 3:5ffe7e9c0bb3 64 IMUinit(mpu6050);
aolgu003 7:396fa2a8648d 65 pressure_sensor.MS5837Init();
gelmes 3:5ffe7e9c0bb3 66 IMUPrintData(mpu6050);
gelmes 9:9aaa7f0c8960 67 runningTime.start();
gelmes 3:5ffe7e9c0bb3 68 }
gelmes 3:5ffe7e9c0bb3 69
gelmes 3:5ffe7e9c0bb3 70 //Initialise all of the vessels starting parameters
gelmes 5:07bbe020eb65 71 Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
gelmes 3:5ffe7e9c0bb3 72 pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
gelmes 3:5ffe7e9c0bb3 73 pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
aolgu003 7:396fa2a8648d 74 pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 75 pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 76 pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
aolgu003 7:396fa2a8648d 77 pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT),
aolgu003 7:396fa2a8648d 78 pidd(&dIn, &dOut, &dPoint,1,1,1, DIRECT),
gelmes 9:9aaa7f0c8960 79 powerPin(A5) {
gelmes 3:5ffe7e9c0bb3 80
gelmes 3:5ffe7e9c0bb3 81 pidy.SetMode(AUTOMATIC); //Yaw PID
gelmes 4:b37fd183e46a 82 pidy.SetOutputLimits(-255,255);
gelmes 4:b37fd183e46a 83 yawPoint = 0;
gelmes 5:07bbe020eb65 84 pidr.SetMode(AUTOMATIC); //Roll PID
gelmes 4:b37fd183e46a 85 pidr.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 86 pitchPoint = 0;
gelmes 5:07bbe020eb65 87 pidp.SetMode(AUTOMATIC); //Pitch PID
gelmes 4:b37fd183e46a 88 pidp.SetOutputLimits(-255,255);
gelmes 4:b37fd183e46a 89 rollPoint = 0;
gelmes 5:07bbe020eb65 90 pidX.SetMode(AUTOMATIC); //Pitch PID
gelmes 5:07bbe020eb65 91 pidX.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 92 xPoint = 0;
gelmes 5:07bbe020eb65 93 pidY.SetMode(AUTOMATIC); //Pitch PID
gelmes 5:07bbe020eb65 94 pidY.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 95 yPoint = 0;
gelmes 5:07bbe020eb65 96 pidZ.SetMode(AUTOMATIC); //Pitch PID
gelmes 5:07bbe020eb65 97 pidZ.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 98 zPoint = 0;
aolgu003 7:396fa2a8648d 99 pidd.SetMode(AUTOMATIC); //Pitch PID
aolgu003 7:396fa2a8648d 100 pidd.SetOutputLimits(-255,255);
aolgu003 7:396fa2a8648d 101 wait(0.5);
aolgu003 7:396fa2a8648d 102 dPoint = depth;
aolgu003 7:396fa2a8648d 103
gelmes 5:07bbe020eb65 104 m0 = 0.5;
gelmes 5:07bbe020eb65 105 m1 = 0.5;
gelmes 5:07bbe020eb65 106 m2 = 0.5;
gelmes 5:07bbe020eb65 107 m3 = 0.5;
gelmes 5:07bbe020eb65 108 m4 = 0.5;
gelmes 5:07bbe020eb65 109 m5 = 0.5;
gelmes 5:07bbe020eb65 110 m6 = 0.5;
gelmes 5:07bbe020eb65 111 m7 = 0.5;
gelmes 9:9aaa7f0c8960 112
aolgu003 7:396fa2a8648d 113 motorState = 1;
aolgu003 7:396fa2a8648d 114
gelmes 3:5ffe7e9c0bb3 115 Start_IMU();
gelmes 5:07bbe020eb65 116 pc.printf("Seagoat Initialized \n\r");
gelmes 3:5ffe7e9c0bb3 117 }
gelmes 3:5ffe7e9c0bb3 118
gelmes 3:5ffe7e9c0bb3 119 void SetYawPID(double Kp, double Ki, double Kd) {
gelmes 3:5ffe7e9c0bb3 120 pidy.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 121 }
gelmes 3:5ffe7e9c0bb3 122
gelmes 3:5ffe7e9c0bb3 123 void SetRollPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 124 pidr.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 125 }
gelmes 3:5ffe7e9c0bb3 126
gelmes 3:5ffe7e9c0bb3 127 void SetPitchPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 128 pidp.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 129 }
gelmes 5:07bbe020eb65 130
gelmes 5:07bbe020eb65 131 void SetXPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 132 pidX.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 133 }
gelmes 5:07bbe020eb65 134
gelmes 5:07bbe020eb65 135 void SetYPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 136 pidY.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 137 }
gelmes 5:07bbe020eb65 138
gelmes 5:07bbe020eb65 139 void SetZPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 140 pidZ.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 141 }
aolgu003 7:396fa2a8648d 142 void SetdPID(double Kp, double Ki, double Kd) {
aolgu003 7:396fa2a8648d 143 pidd.SetTunings(Kp, Ki, Kd);
aolgu003 7:396fa2a8648d 144 }
aolgu003 7:396fa2a8648d 145
aolgu003 7:396fa2a8648d 146 void calibrate() {
gelmes 5:07bbe020eb65 147 IMUUpdate(mpu6050);
gelmes 5:07bbe020eb65 148 pc.printf("Calibrating...\n\r");
aolgu003 7:396fa2a8648d 149 //pressure_sensor.Barometer_MS5837();
aolgu003 7:396fa2a8648d 150 //depth = pressure_sensor.MS5837_Pressure();
gelmes 5:07bbe020eb65 151 }
aolgu003 7:396fa2a8648d 152
aolgu003 7:396fa2a8648d 153 void update() {
gelmes 4:b37fd183e46a 154 //Update IMU Values
gelmes 4:b37fd183e46a 155 IMUUpdate(mpu6050);
aolgu003 7:396fa2a8648d 156
aolgu003 7:396fa2a8648d 157 //pressure_sensor.Barometer_MS5837();
aolgu003 7:396fa2a8648d 158 depth = pressure_sensor.MS5837_Pressure();
aolgu003 7:396fa2a8648d 159 //pc.printf("Pressure: %f %f\n", depth, dPoint);
gelmes 9:9aaa7f0c8960 160
aolgu003 7:396fa2a8648d 161 //Detect if the switch is turned on
gelmes 9:9aaa7f0c8960 162 if(!motorState && powerPin.read() == 1) {
gelmes 9:9aaa7f0c8960 163 runningTime.stop();
gelmes 9:9aaa7f0c8960 164 initMotors();
gelmes 9:9aaa7f0c8960 165 motorState = 1;
gelmes 9:9aaa7f0c8960 166 runningState += 1;
gelmes 9:9aaa7f0c8960 167 if(runningState == 2) runningState = 0;
gelmes 9:9aaa7f0c8960 168
aolgu003 7:396fa2a8648d 169 pc.printf("Motors Detected");
gelmes 9:9aaa7f0c8960 170
gelmes 9:9aaa7f0c8960 171 yawPoint = yaw;
gelmes 9:9aaa7f0c8960 172 if(runningState == 1) pitchPoint = pitch;
gelmes 9:9aaa7f0c8960 173 else pitchPoint = 0;
gelmes 9:9aaa7f0c8960 174 dPoint = depth;
gelmes 9:9aaa7f0c8960 175 runningTime.reset();
gelmes 9:9aaa7f0c8960 176 runningTime.start();
gelmes 9:9aaa7f0c8960 177 } else if(powerPin.read() != 1) {
aolgu003 7:396fa2a8648d 178 motorState = 0;
aolgu003 7:396fa2a8648d 179 neutralizeMotors();
aolgu003 7:396fa2a8648d 180 }
aolgu003 7:396fa2a8648d 181
gelmes 4:b37fd183e46a 182 yawIn = yaw;
gelmes 4:b37fd183e46a 183 rollIn = roll;
gelmes 4:b37fd183e46a 184 pitchIn = pitch;
gelmes 5:07bbe020eb65 185 xIn = ax;
gelmes 5:07bbe020eb65 186 yIn = ay;
gelmes 5:07bbe020eb65 187 zIn = az;
aolgu003 7:396fa2a8648d 188
gelmes 4:b37fd183e46a 189 //Calculate PID values
gelmes 4:b37fd183e46a 190 pidy.Compute();
gelmes 5:07bbe020eb65 191 pidr.Compute();
gelmes 5:07bbe020eb65 192 pidp.Compute();
gelmes 5:07bbe020eb65 193 pidX.Compute();
gelmes 5:07bbe020eb65 194 pidY.Compute();
gelmes 5:07bbe020eb65 195 pidZ.Compute();
aolgu003 7:396fa2a8648d 196
gelmes 5:07bbe020eb65 197 /*
gelmes 5:07bbe020eb65 198 Cameras
gelmes 5:07bbe020eb65 199 FL ----- F ->--- FR
gelmes 5:07bbe020eb65 200 | | |
gelmes 5:07bbe020eb65 201 ˄ | |
gelmes 5:07bbe020eb65 202 | | |
gelmes 5:07bbe020eb65 203 L | R
gelmes 5:07bbe020eb65 204 | | |
gelmes 5:07bbe020eb65 205 | | ˅
gelmes 5:07bbe020eb65 206 | | |
gelmes 5:07bbe020eb65 207 BL ---<- B ----- BR
aolgu003 7:396fa2a8648d 208
gelmes 5:07bbe020eb65 209 0 ----- 1 ->--- 2
gelmes 5:07bbe020eb65 210 | | |
gelmes 5:07bbe020eb65 211 ˄ | |
gelmes 5:07bbe020eb65 212 | | |
gelmes 5:07bbe020eb65 213 7 | 3
gelmes 5:07bbe020eb65 214 | | |
gelmes 5:07bbe020eb65 215 | | ˅
gelmes 5:07bbe020eb65 216 | | |
gelmes 5:07bbe020eb65 217 6 ---<- 5 ----- 4
aolgu003 7:396fa2a8648d 218
gelmes 5:07bbe020eb65 219 */
aolgu003 7:396fa2a8648d 220
gelmes 5:07bbe020eb65 221 //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
aolgu003 7:396fa2a8648d 222
gelmes 9:9aaa7f0c8960 223 float forwardThrust = 100;
gelmes 9:9aaa7f0c8960 224
gelmes 5:07bbe020eb65 225 //Values used in Dynamic Magnitude Calculations
gelmes 5:07bbe020eb65 226 float accxs = xOut * xOut * abs(xOut) / xOut;
gelmes 5:07bbe020eb65 227 float accys = yOut * yOut * abs(yOut) / yOut;
gelmes 5:07bbe020eb65 228 float acczs = zOut * zOut * abs(zOut) / zOut;
aolgu003 7:396fa2a8648d 229 float depths = dOut * dOut;
gelmes 5:07bbe020eb65 230 float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
gelmes 5:07bbe020eb65 231 float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
gelmes 5:07bbe020eb65 232 float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
aolgu003 7:396fa2a8648d 233
gelmes 5:07bbe020eb65 234 //Values used for Influence calculations
aolgu003 7:396fa2a8648d 235 float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut) + abs(dOut)) * 255;
gelmes 5:07bbe020eb65 236 float yy = (abs(yOut) + abs(yawOut)) * 255;
aolgu003 7:396fa2a8648d 237 float xy = (abs(xOut) + abs(yawOut)) * 255;
aolgu003 7:396fa2a8648d 238
gelmes 5:07bbe020eb65 239 if (abs(zpr)<255) zpr = 255;
gelmes 5:07bbe020eb65 240 if (abs(yy)<255) yy = 255;
gelmes 5:07bbe020eb65 241 if (abs(xy)<255) xy = 255;
aolgu003 7:396fa2a8648d 242
gelmes 9:9aaa7f0c8960 243 if(runningState == 0) {
gelmes 9:9aaa7f0c8960 244 if(runningTime < 1) {
gelmes 9:9aaa7f0c8960 245 SetYawPID(1,0,0);
gelmes 9:9aaa7f0c8960 246 SetRollPID(1,0,0);
gelmes 9:9aaa7f0c8960 247 SetPitchPID(1,0,0);
gelmes 9:9aaa7f0c8960 248 wait(1);
gelmes 9:9aaa7f0c8960 249 }
gelmes 9:9aaa7f0c8960 250 else{
gelmes 9:9aaa7f0c8960 251 m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;//
gelmes 9:9aaa7f0c8960 252 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 9:9aaa7f0c8960 253 m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
gelmes 9:9aaa7f0c8960 254 //m3 = (accys + yaws) / yy + 0.5;
gelmes 9:9aaa7f0c8960 255 m3 = 0.7;
gelmes 9:9aaa7f0c8960 256 m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
gelmes 9:9aaa7f0c8960 257 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 9:9aaa7f0c8960 258 m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
gelmes 9:9aaa7f0c8960 259 //m7 = (-accys + yaws) / -yy + 0.5;
gelmes 9:9aaa7f0c8960 260 m7 = 0.7;
gelmes 9:9aaa7f0c8960 261 }
gelmes 9:9aaa7f0c8960 262 }
gelmes 9:9aaa7f0c8960 263 else if(runningState == 1) {
gelmes 9:9aaa7f0c8960 264 if(runningTime < 1) {
gelmes 9:9aaa7f0c8960 265 SetYawPID(2,0,0.3);
gelmes 9:9aaa7f0c8960 266 SetRollPID(2,0,0.3);
gelmes 9:9aaa7f0c8960 267 SetPitchPID(2,0,0.3);
gelmes 9:9aaa7f0c8960 268 wait(1);
gelmes 9:9aaa7f0c8960 269 } else if(runningTime < 27) {
gelmes 9:9aaa7f0c8960 270 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 9:9aaa7f0c8960 271 m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
gelmes 9:9aaa7f0c8960 272 //m3 = (accys + yaws) / yy + 0.5;
gelmes 9:9aaa7f0c8960 273 m3 = 0.7;
gelmes 9:9aaa7f0c8960 274 m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
gelmes 9:9aaa7f0c8960 275 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 9:9aaa7f0c8960 276 m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
gelmes 9:9aaa7f0c8960 277 //m7 = (-accys + yaws) / -yy + 0.5;
gelmes 9:9aaa7f0c8960 278 m7 = 0.7;
gelmes 9:9aaa7f0c8960 279 } else if(runningTime < 5) {
gelmes 9:9aaa7f0c8960 280 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 9:9aaa7f0c8960 281 m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
gelmes 9:9aaa7f0c8960 282 //m3 = (accys + yaws) / yy + 0.5;
gelmes 9:9aaa7f0c8960 283 m3 = 0.7;
gelmes 9:9aaa7f0c8960 284 m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
gelmes 9:9aaa7f0c8960 285 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 9:9aaa7f0c8960 286 m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
gelmes 9:9aaa7f0c8960 287 //m7 = (-accys + yaws) / -yy + 0.5;
gelmes 9:9aaa7f0c8960 288 m7 = 0.7;
gelmes 9:9aaa7f0c8960 289 }
aolgu003 7:396fa2a8648d 290 }
aolgu003 7:396fa2a8648d 291
gelmes 9:9aaa7f0c8960 292 //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
gelmes 9:9aaa7f0c8960 293 //pc.printf("%f,%f,%f, %f,%f,%f, %f, %f \n\r",powerPin.read(), ay, yaws, pitchs, rolls, yy, accys, (-accys + yaws - (forwardThrust * forwardThrust)) / -yy + 0.5);
gelmes 9:9aaa7f0c8960 294 pc.printf("YAW: %f, %f, %f, %f, %f, %f, %f, %f\n\r", abs((acczs + pitchs + rolls) / zpr),abs((accys + yaws) / yy),abs((acczs + pitchs - rolls) / zpr),abs((accxs + yaws) / xy),abs((acczs - pitchs - rolls) / zpr),abs((accys + yaws) / yy),abs((acczs - pitchs + rolls) / zpr),abs((accxs + yaws) / yy));
gelmes 9:9aaa7f0c8960 295 //pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
gelmes 9:9aaa7f0c8960 296 //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
aolgu003 7:396fa2a8648d 297 }
gelmes 9:9aaa7f0c8960 298
gelmes 9:9aaa7f0c8960 299 void updateCommand() {
gelmes 9:9aaa7f0c8960 300 char a = 0;
gelmes 9:9aaa7f0c8960 301 char i = 0;
gelmes 9:9aaa7f0c8960 302 char buffer[10] = {' '};
gelmes 9:9aaa7f0c8960 303 if (device.readable()) {
gelmes 9:9aaa7f0c8960 304 while(a != 'd') {
gelmes 9:9aaa7f0c8960 305 a = device.getc();
gelmes 9:9aaa7f0c8960 306 if ((a >= '0' && a <='9') || a == '.') {
gelmes 9:9aaa7f0c8960 307 buffer[i] = a;
gelmes 9:9aaa7f0c8960 308 i++;
gelmes 9:9aaa7f0c8960 309 }
gelmes 9:9aaa7f0c8960 310 }
gelmes 9:9aaa7f0c8960 311 depth = atof(buffer);
gelmes 9:9aaa7f0c8960 312 pc.printf("Depth: '%f'\n", depth);
gelmes 9:9aaa7f0c8960 313 }
gelmes 9:9aaa7f0c8960 314 }
gelmes 9:9aaa7f0c8960 315
gelmes 9:9aaa7f0c8960 316 void initMotors() {
aolgu003 7:396fa2a8648d 317
gelmes 9:9aaa7f0c8960 318 neutralizeMotors();
gelmes 9:9aaa7f0c8960 319 wait(0.5);
gelmes 9:9aaa7f0c8960 320 m0.calibrate();
gelmes 9:9aaa7f0c8960 321 m1.calibrate();
gelmes 9:9aaa7f0c8960 322 m2.calibrate();
gelmes 9:9aaa7f0c8960 323 m3.calibrate();
gelmes 9:9aaa7f0c8960 324 m4.calibrate();
gelmes 9:9aaa7f0c8960 325 m5.calibrate();
gelmes 9:9aaa7f0c8960 326 m6.calibrate();
gelmes 9:9aaa7f0c8960 327 m7.calibrate();
gelmes 9:9aaa7f0c8960 328 wait(3);
gelmes 9:9aaa7f0c8960 329 }
aolgu003 6:b45b74fd6a07 330
gelmes 9:9aaa7f0c8960 331 void neutralizeMotors() {
gelmes 9:9aaa7f0c8960 332 m0 = 0.5;
gelmes 9:9aaa7f0c8960 333 m1 = 0.5;
gelmes 9:9aaa7f0c8960 334 m2 = 0.5;
gelmes 9:9aaa7f0c8960 335 m3 = 0.5;
gelmes 9:9aaa7f0c8960 336 m4 = 0.5;
gelmes 9:9aaa7f0c8960 337 m5 = 0.5;
gelmes 9:9aaa7f0c8960 338 m6 = 0.5;
gelmes 9:9aaa7f0c8960 339 m7 = 0.5;
gelmes 9:9aaa7f0c8960 340 }
gelmes 9:9aaa7f0c8960 341
gelmes 9:9aaa7f0c8960 342 };
aolgu003 6:b45b74fd6a07 343
gelmes 3:5ffe7e9c0bb3 344 #endif