Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v2

Robosub controller

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller by Marco Rubio

vessel.h@7:396fa2a8648d, 2016-07-29 (annotated)

Committer:
aolgu003
Date:
Fri Jul 29 15:34:59 2016 +0000
Revision:
7:396fa2a8648d
Parent:
6:b45b74fd6a07 
Fixed issues and integrated with the sub.

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