Marco Rubio / Mbed 2 deprecated RTOS_Controller_v3

Working

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller_v2 by Andrew Olguin

vessel.h@10:8cd741a65646, 2016-07-30 (annotated)

Committer:
gelmes
Date:
Sat Jul 30 21:32:40 2016 +0000
Revision:
10:8cd741a65646
Parent:
9:9aaa7f0c8960 
Working switching implementation;

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);
gelmes 10:8cd741a65646 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;
gelmes 10:8cd741a65646 114 runningState = -1;
aolgu003 7:396fa2a8648d 115
gelmes 3:5ffe7e9c0bb3 116 Start_IMU();
gelmes 5:07bbe020eb65 117 pc.printf("Seagoat Initialized \n\r");
gelmes 3:5ffe7e9c0bb3 118 }
gelmes 3:5ffe7e9c0bb3 119
gelmes 3:5ffe7e9c0bb3 120 void SetYawPID(double Kp, double Ki, double Kd) {
gelmes 3:5ffe7e9c0bb3 121 pidy.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 122 }
gelmes 3:5ffe7e9c0bb3 123
gelmes 3:5ffe7e9c0bb3 124 void SetRollPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 125 pidr.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 126 }
gelmes 3:5ffe7e9c0bb3 127
gelmes 3:5ffe7e9c0bb3 128 void SetPitchPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 129 pidp.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 130 }
gelmes 5:07bbe020eb65 131
gelmes 5:07bbe020eb65 132 void SetXPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 133 pidX.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 134 }
gelmes 5:07bbe020eb65 135
gelmes 5:07bbe020eb65 136 void SetYPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 137 pidY.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 138 }
gelmes 5:07bbe020eb65 139
gelmes 5:07bbe020eb65 140 void SetZPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 141 pidZ.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 142 }
aolgu003 7:396fa2a8648d 143 void SetdPID(double Kp, double Ki, double Kd) {
aolgu003 7:396fa2a8648d 144 pidd.SetTunings(Kp, Ki, Kd);
aolgu003 7:396fa2a8648d 145 }
aolgu003 7:396fa2a8648d 146
aolgu003 7:396fa2a8648d 147 void calibrate() {
gelmes 5:07bbe020eb65 148 IMUUpdate(mpu6050);
gelmes 5:07bbe020eb65 149 pc.printf("Calibrating...\n\r");
aolgu003 7:396fa2a8648d 150 //pressure_sensor.Barometer_MS5837();
aolgu003 7:396fa2a8648d 151 //depth = pressure_sensor.MS5837_Pressure();
gelmes 5:07bbe020eb65 152 }
aolgu003 7:396fa2a8648d 153
aolgu003 7:396fa2a8648d 154 void update() {
gelmes 4:b37fd183e46a 155 //Update IMU Values
gelmes 4:b37fd183e46a 156 IMUUpdate(mpu6050);
aolgu003 7:396fa2a8648d 157
aolgu003 7:396fa2a8648d 158 //pressure_sensor.Barometer_MS5837();
gelmes 10:8cd741a65646 159 //depth = pressure_sensor.MS5837_Pressure();
aolgu003 7:396fa2a8648d 160 //pc.printf("Pressure: %f %f\n", depth, dPoint);
gelmes 9:9aaa7f0c8960 161
aolgu003 7:396fa2a8648d 162 //Detect if the switch is turned on
gelmes 9:9aaa7f0c8960 163 if(!motorState && powerPin.read() == 1) {
gelmes 9:9aaa7f0c8960 164 runningTime.stop();
gelmes 9:9aaa7f0c8960 165 initMotors();
gelmes 9:9aaa7f0c8960 166 motorState = 1;
gelmes 9:9aaa7f0c8960 167 runningState += 1;
gelmes 9:9aaa7f0c8960 168 if(runningState == 2) runningState = 0;
gelmes 9:9aaa7f0c8960 169
aolgu003 7:396fa2a8648d 170 pc.printf("Motors Detected");
gelmes 9:9aaa7f0c8960 171
gelmes 9:9aaa7f0c8960 172 yawPoint = yaw;
gelmes 10:8cd741a65646 173 if(runningState == 0) pitchPoint = pitch;
gelmes 9:9aaa7f0c8960 174 else pitchPoint = 0;
gelmes 9:9aaa7f0c8960 175 dPoint = depth;
gelmes 9:9aaa7f0c8960 176 runningTime.reset();
gelmes 9:9aaa7f0c8960 177 runningTime.start();
gelmes 9:9aaa7f0c8960 178 } else if(powerPin.read() != 1) {
aolgu003 7:396fa2a8648d 179 motorState = 0;
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
gelmes 10:8cd741a65646 235 float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 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 10:8cd741a65646 250 else if(runningTime < 15){ //15
gelmes 10:8cd741a65646 251 m0 = (acczs + pitchs + rolls) / zpr + 0.5;
gelmes 9:9aaa7f0c8960 252 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 253 m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 9:9aaa7f0c8960 254 //m3 = (accys + yaws) / yy + 0.5;
gelmes 9:9aaa7f0c8960 255 m3 = 0.7;
gelmes 10:8cd741a65646 256 m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 9:9aaa7f0c8960 257 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 258 m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 259 //m7 = (-accys + yaws) / -yy + 0.5;
gelmes 10:8cd741a65646 260 m7 = 0.7;
gelmes 10:8cd741a65646 261 }
gelmes 10:8cd741a65646 262 else if(runningTime < 16){ //16
gelmes 10:8cd741a65646 263 neutralizeMotors();
gelmes 10:8cd741a65646 264 pitchPoint = 0;
gelmes 10:8cd741a65646 265 }
gelmes 10:8cd741a65646 266 else{
gelmes 10:8cd741a65646 267 m0 = (acczs + pitchs + rolls) / zpr + 0.5;
gelmes 10:8cd741a65646 268 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 269 m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 270 //m3 = (accys + yaws) / yy + 0.5;
gelmes 10:8cd741a65646 271 m3 = 0.7;
gelmes 10:8cd741a65646 272 m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 10:8cd741a65646 273 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 274 m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 9:9aaa7f0c8960 275 //m7 = (-accys + yaws) / -yy + 0.5;
gelmes 9:9aaa7f0c8960 276 m7 = 0.7;
gelmes 9:9aaa7f0c8960 277 }
gelmes 9:9aaa7f0c8960 278 }
gelmes 9:9aaa7f0c8960 279 else if(runningState == 1) {
gelmes 9:9aaa7f0c8960 280 if(runningTime < 1) {
gelmes 9:9aaa7f0c8960 281 SetYawPID(2,0,0.3);
gelmes 9:9aaa7f0c8960 282 SetRollPID(2,0,0.3);
gelmes 9:9aaa7f0c8960 283 SetPitchPID(2,0,0.3);
gelmes 10:8cd741a65646 284 lastyawPoint = yawPoint;
gelmes 9:9aaa7f0c8960 285 wait(1);
gelmes 10:8cd741a65646 286 } else if(runningTime < 27) { //27
gelmes 10:8cd741a65646 287 //Go straight to Gate
gelmes 10:8cd741a65646 288 m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 9:9aaa7f0c8960 289 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 290 m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 9:9aaa7f0c8960 291 m3 = 0.7;
gelmes 10:8cd741a65646 292 m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 9:9aaa7f0c8960 293 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 294 m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 9:9aaa7f0c8960 295 m7 = 0.7;
gelmes 10:8cd741a65646 296 } else if(runningTime < 28) { //28
gelmes 10:8cd741a65646 297 neutralizeMotors();
gelmes 10:8cd741a65646 298 //Set turning 180 angle aim for Gate
gelmes 10:8cd741a65646 299 yawPoint = correctAngle(lastyawPoint - 160);
gelmes 10:8cd741a65646 300 } else if(runningTime < 32) { //32
gelmes 10:8cd741a65646 301 //allign to new yaw
gelmes 10:8cd741a65646 302 m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 10:8cd741a65646 303 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 304 m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 305 m3 = 0.5;
gelmes 10:8cd741a65646 306 m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 10:8cd741a65646 307 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 308 m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 309 m7 = 0.5;
gelmes 10:8cd741a65646 310 } else if(runningTime < 42) { //42
gelmes 10:8cd741a65646 311 //go go go
gelmes 10:8cd741a65646 312 m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 9:9aaa7f0c8960 313 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 314 m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 315 m3 = 0.3;
gelmes 10:8cd741a65646 316 m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 10:8cd741a65646 317 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 318 m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 319 m7 = 0.3;
gelmes 10:8cd741a65646 320 } else if(runningTime < 43) { //43
gelmes 10:8cd741a65646 321 neutralizeMotors();
gelmes 10:8cd741a65646 322 //Set turning 180 angle aim for Gate
gelmes 10:8cd741a65646 323 yawPoint = correctAngle(lastyawPoint - 70);
gelmes 10:8cd741a65646 324 } else if(runningTime < 47) { //47
gelmes 10:8cd741a65646 325 m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 10:8cd741a65646 326 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 327 m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 328 m3 = 0.5;
gelmes 10:8cd741a65646 329 m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 10:8cd741a65646 330 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 331 m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 332 m7 = 0.5;
gelmes 10:8cd741a65646 333 } else if(runningTime < 52) { //52
gelmes 10:8cd741a65646 334 m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 10:8cd741a65646 335 m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 336 m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 9:9aaa7f0c8960 337 m3 = 0.7;
gelmes 10:8cd741a65646 338 m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 9:9aaa7f0c8960 339 m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 340 m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 9:9aaa7f0c8960 341 m7 = 0.7;
gelmes 9:9aaa7f0c8960 342 }
gelmes 10:8cd741a65646 343 // else if(runningTime < 36) { //53
gelmes 10:8cd741a65646 344 // neutralizeMotors();
gelmes 10:8cd741a65646 345 // //Go Upside down
gelmes 10:8cd741a65646 346 // rollPoint = correctAngle(lastrollPoint + 170);
gelmes 10:8cd741a65646 347 // pc.printf("%f\n", rollPoint);
gelmes 10:8cd741a65646 348 // } else if(runningTime < 40) { //57
gelmes 10:8cd741a65646 349 // m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 10:8cd741a65646 350 // m1 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 351 // m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 352 // m3 = 0.5;
gelmes 10:8cd741a65646 353 // m4 = (acczs - pitchs - rolls) / zpr + 0.5;
gelmes 10:8cd741a65646 354 // m5 = (accxs + yaws) / -xy + 0.5;
gelmes 10:8cd741a65646 355 // m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
gelmes 10:8cd741a65646 356 // m7 = 0.5;
gelmes 10:8cd741a65646 357 // }
gelmes 10:8cd741a65646 358 else if(runningTime < 58) { //63
gelmes 10:8cd741a65646 359 //Surface
gelmes 10:8cd741a65646 360 m0 = 0.7;
gelmes 10:8cd741a65646 361 m1 = 0.5;
gelmes 10:8cd741a65646 362 m2 = 0.7;
gelmes 10:8cd741a65646 363 m3 = 0.5;
gelmes 10:8cd741a65646 364 m4 = 0.7;
gelmes 10:8cd741a65646 365 m5 = 0.5;
gelmes 10:8cd741a65646 366 m6 = 0.7;
gelmes 10:8cd741a65646 367 m7 = 0.5;
gelmes 10:8cd741a65646 368 }
gelmes 10:8cd741a65646 369 else{
gelmes 10:8cd741a65646 370 neutralizeMotors();
gelmes 10:8cd741a65646 371 }
aolgu003 7:396fa2a8648d 372 }
aolgu003 7:396fa2a8648d 373
gelmes 9:9aaa7f0c8960 374 //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
gelmes 9:9aaa7f0c8960 375 //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 10:8cd741a65646 376 //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 377 //pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
gelmes 9:9aaa7f0c8960 378 //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
aolgu003 7:396fa2a8648d 379 }
gelmes 10:8cd741a65646 380
gelmes 10:8cd741a65646 381 float correctAngle(float angle){
gelmes 10:8cd741a65646 382 if(angle > 180) return (angle - 360);
gelmes 10:8cd741a65646 383 else if(angle < -180) return (angle + 360);
gelmes 10:8cd741a65646 384 else return angle;
gelmes 10:8cd741a65646 385 }
gelmes 10:8cd741a65646 386
gelmes 9:9aaa7f0c8960 387 void updateCommand() {
gelmes 10:8cd741a65646 388 //char a = 0;
gelmes 10:8cd741a65646 389 // char i = 0;
gelmes 10:8cd741a65646 390 // char buffer[10] = {' '};
gelmes 10:8cd741a65646 391 // if (device.readable()) {
gelmes 10:8cd741a65646 392 // while(a != 'd') {
gelmes 10:8cd741a65646 393 // a = device.getc();
gelmes 10:8cd741a65646 394 // if ((a >= '0' && a <='9') || a == '.') {
gelmes 10:8cd741a65646 395 // buffer[i] = a;
gelmes 10:8cd741a65646 396 // i++;
gelmes 10:8cd741a65646 397 // }
gelmes 10:8cd741a65646 398 // }
gelmes 10:8cd741a65646 399 // depth = atof(buffer);
gelmes 10:8cd741a65646 400 // pc.printf("Depth: '%f'\n", depth);
gelmes 10:8cd741a65646 401 // }
gelmes 9:9aaa7f0c8960 402 }
gelmes 9:9aaa7f0c8960 403
gelmes 9:9aaa7f0c8960 404 void initMotors() {
aolgu003 7:396fa2a8648d 405
gelmes 9:9aaa7f0c8960 406 neutralizeMotors();
gelmes 9:9aaa7f0c8960 407 wait(0.5);
gelmes 9:9aaa7f0c8960 408 m0.calibrate();
gelmes 9:9aaa7f0c8960 409 m1.calibrate();
gelmes 9:9aaa7f0c8960 410 m2.calibrate();
gelmes 9:9aaa7f0c8960 411 m3.calibrate();
gelmes 9:9aaa7f0c8960 412 m4.calibrate();
gelmes 9:9aaa7f0c8960 413 m5.calibrate();
gelmes 9:9aaa7f0c8960 414 m6.calibrate();
gelmes 9:9aaa7f0c8960 415 m7.calibrate();
gelmes 9:9aaa7f0c8960 416 wait(3);
gelmes 9:9aaa7f0c8960 417 }
aolgu003 6:b45b74fd6a07 418
gelmes 9:9aaa7f0c8960 419 void neutralizeMotors() {
gelmes 9:9aaa7f0c8960 420 m0 = 0.5;
gelmes 9:9aaa7f0c8960 421 m1 = 0.5;
gelmes 9:9aaa7f0c8960 422 m2 = 0.5;
gelmes 9:9aaa7f0c8960 423 m3 = 0.5;
gelmes 9:9aaa7f0c8960 424 m4 = 0.5;
gelmes 9:9aaa7f0c8960 425 m5 = 0.5;
gelmes 9:9aaa7f0c8960 426 m6 = 0.5;
gelmes 9:9aaa7f0c8960 427 m7 = 0.5;
gelmes 9:9aaa7f0c8960 428 }
gelmes 9:9aaa7f0c8960 429
gelmes 9:9aaa7f0c8960 430 };
aolgu003 6:b45b74fd6a07 431
gelmes 3:5ffe7e9c0bb3 432 #endif