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@6:b45b74fd6a07, 2016-07-27 (annotated)

Committer:
aolgu003
Date:
Wed Jul 27 02:45:45 2016 +0000
Revision:
6:b45b74fd6a07
Parent:
5:07bbe020eb65
Child:
7:396fa2a8648d
Added command parsing

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>
gelmes 3:5ffe7e9c0bb3 11 /*
gelmes 3:5ffe7e9c0bb3 12 Cameras
gelmes 3:5ffe7e9c0bb3 13 FL ----- F ->--- FR
gelmes 3:5ffe7e9c0bb3 14 | | |
gelmes 3:5ffe7e9c0bb3 15 ˄ | |
gelmes 3:5ffe7e9c0bb3 16 | | |
gelmes 3:5ffe7e9c0bb3 17 L | R
gelmes 3:5ffe7e9c0bb3 18 | | |
gelmes 3:5ffe7e9c0bb3 19 | | ˅
gelmes 3:5ffe7e9c0bb3 20 | | |
gelmes 3:5ffe7e9c0bb3 21 BL ---<- B ----- BR
gelmes 3:5ffe7e9c0bb3 22
gelmes 3:5ffe7e9c0bb3 23 0 ----- 1 ->--- 2
gelmes 3:5ffe7e9c0bb3 24 | | |
gelmes 3:5ffe7e9c0bb3 25 ˄ | |
gelmes 3:5ffe7e9c0bb3 26 | | |
gelmes 3:5ffe7e9c0bb3 27 7 | 3
gelmes 3:5ffe7e9c0bb3 28 | | |
gelmes 3:5ffe7e9c0bb3 29 | | ˅
gelmes 3:5ffe7e9c0bb3 30 | | |
gelmes 3:5ffe7e9c0bb3 31 6 ---<- 5 ----- 4
gelmes 3:5ffe7e9c0bb3 32 */
aolgu003 6:b45b74fd6a07 33 #define BUFFER_SIZE 255
aolgu003 6:b45b74fd6a07 34
gelmes 3:5ffe7e9c0bb3 35 class Vessel
gelmes 3:5ffe7e9c0bb3 36 {
gelmes 3:5ffe7e9c0bb3 37
gelmes 3:5ffe7e9c0bb3 38 private:
gelmes 4:b37fd183e46a 39 // Servo m0;
gelmes 4:b37fd183e46a 40 // Servo m1;
gelmes 4:b37fd183e46a 41 // Servo m2;
gelmes 4:b37fd183e46a 42 // Servo m3;
gelmes 4:b37fd183e46a 43 // Servo m4;
gelmes 4:b37fd183e46a 44 // Servo m5;
gelmes 4:b37fd183e46a 45 // Servo m6;
gelmes 4:b37fd183e46a 46 // Servo m7;
gelmes 4:b37fd183e46a 47
gelmes 4:b37fd183e46a 48 PwmOut m0;
gelmes 4:b37fd183e46a 49 PwmOut m1;
gelmes 4:b37fd183e46a 50 PwmOut m2;
gelmes 4:b37fd183e46a 51 PwmOut m3;
gelmes 4:b37fd183e46a 52 PwmOut m4;
gelmes 4:b37fd183e46a 53 PwmOut m5;
gelmes 4:b37fd183e46a 54 PwmOut m6;
gelmes 4:b37fd183e46a 55 PwmOut m7;
gelmes 3:5ffe7e9c0bb3 56
gelmes 3:5ffe7e9c0bb3 57 PwmOut led1;
gelmes 3:5ffe7e9c0bb3 58 MPU6050 mpu6050;
gelmes 3:5ffe7e9c0bb3 59 double yawPoint, yawIn, yawOut;
gelmes 3:5ffe7e9c0bb3 60 double rollPoint, rollIn, rollOut;
gelmes 3:5ffe7e9c0bb3 61 double pitchPoint, pitchIn, pitchOut;
gelmes 5:07bbe020eb65 62 double xPoint, xIn, xOut;
gelmes 5:07bbe020eb65 63 double yPoint, yIn, yOut;
gelmes 5:07bbe020eb65 64 double zPoint, zIn, zOut;
aolgu003 6:b45b74fd6a07 65 double p_gain, i_gain, d_gain;
gelmes 5:07bbe020eb65 66 PID pidy, pidr, pidp, pidX, pidY, pidZ;
aolgu003 6:b45b74fd6a07 67 char buffer[BUFFER_SIZE];
gelmes 3:5ffe7e9c0bb3 68 public:
gelmes 3:5ffe7e9c0bb3 69 void Start_IMU() {
gelmes 3:5ffe7e9c0bb3 70 pc.printf("Starting up\n\r");
gelmes 3:5ffe7e9c0bb3 71 pc.baud(9600);
gelmes 3:5ffe7e9c0bb3 72 i2c.frequency(400000); // use fast (400 kHz) I2C
gelmes 3:5ffe7e9c0bb3 73 IMUinit(mpu6050);
gelmes 3:5ffe7e9c0bb3 74 IMUPrintData(mpu6050);
gelmes 3:5ffe7e9c0bb3 75 }
gelmes 3:5ffe7e9c0bb3 76
gelmes 3:5ffe7e9c0bb3 77 //Initialise all of the vessels starting parameters
gelmes 5:07bbe020eb65 78 Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
gelmes 3:5ffe7e9c0bb3 79 pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
gelmes 3:5ffe7e9c0bb3 80 pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 81 pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 82 pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 83 pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
aolgu003 6:b45b74fd6a07 84 pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT)
aolgu003 6:b45b74fd6a07 85 {
gelmes 3:5ffe7e9c0bb3 86
gelmes 3:5ffe7e9c0bb3 87 pidy.SetMode(AUTOMATIC); //Yaw PID
gelmes 4:b37fd183e46a 88 pidy.SetOutputLimits(-255,255);
gelmes 4:b37fd183e46a 89 yawPoint = 0;
gelmes 5:07bbe020eb65 90 pidr.SetMode(AUTOMATIC); //Roll PID
gelmes 4:b37fd183e46a 91 pidr.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 92 pitchPoint = 0;
gelmes 5:07bbe020eb65 93 pidp.SetMode(AUTOMATIC); //Pitch PID
gelmes 4:b37fd183e46a 94 pidp.SetOutputLimits(-255,255);
gelmes 4:b37fd183e46a 95 rollPoint = 0;
gelmes 5:07bbe020eb65 96 pidX.SetMode(AUTOMATIC); //Pitch PID
gelmes 5:07bbe020eb65 97 pidX.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 98 xPoint = 0;
gelmes 5:07bbe020eb65 99 pidY.SetMode(AUTOMATIC); //Pitch PID
gelmes 5:07bbe020eb65 100 pidY.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 101 yPoint = 0;
gelmes 5:07bbe020eb65 102 pidZ.SetMode(AUTOMATIC); //Pitch PID
gelmes 5:07bbe020eb65 103 pidZ.SetOutputLimits(-255,255);
gelmes 5:07bbe020eb65 104 zPoint = 0;
gelmes 5:07bbe020eb65 105
gelmes 5:07bbe020eb65 106 m0 = 0.5;
gelmes 5:07bbe020eb65 107 m1 = 0.5;
gelmes 5:07bbe020eb65 108 m2 = 0.5;
gelmes 5:07bbe020eb65 109 m3 = 0.5;
gelmes 5:07bbe020eb65 110 m4 = 0.5;
gelmes 5:07bbe020eb65 111 m5 = 0.5;
gelmes 5:07bbe020eb65 112 m6 = 0.5;
gelmes 5:07bbe020eb65 113 m7 = 0.5;
gelmes 3:5ffe7e9c0bb3 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 }
gelmes 3:5ffe7e9c0bb3 142
gelmes 3:5ffe7e9c0bb3 143 //This is where the magic happens
gelmes 5:07bbe020eb65 144 void motorTest(){
gelmes 5:07bbe020eb65 145 pwmSweep(m0);
gelmes 5:07bbe020eb65 146 pwmSweep(m1);
gelmes 5:07bbe020eb65 147 pwmSweep(m2);
gelmes 5:07bbe020eb65 148 pwmSweep(m3);
gelmes 5:07bbe020eb65 149 pwmSweep(m4);
gelmes 5:07bbe020eb65 150 pwmSweep(m5);
gelmes 5:07bbe020eb65 151 pwmSweep(m6);
gelmes 5:07bbe020eb65 152 pwmSweep(m7);
gelmes 5:07bbe020eb65 153 }
gelmes 5:07bbe020eb65 154
gelmes 5:07bbe020eb65 155 void pwmSweep(PwmOut motor){
gelmes 5:07bbe020eb65 156 for(float i = 0; i < 80; i++){
gelmes 5:07bbe020eb65 157 motor = i/255;
gelmes 5:07bbe020eb65 158 wait(0.002);
gelmes 5:07bbe020eb65 159 }
gelmes 5:07bbe020eb65 160 // for(float i = 80; i >= 0; i--){
gelmes 5:07bbe020eb65 161 // motor = i/255;
gelmes 5:07bbe020eb65 162 // wait(0.002);
gelmes 5:07bbe020eb65 163 // }
gelmes 5:07bbe020eb65 164 }
gelmes 5:07bbe020eb65 165 void calibrate(){
gelmes 5:07bbe020eb65 166 IMUUpdate(mpu6050);
gelmes 5:07bbe020eb65 167 pc.printf("Calibrating...\n\r");
gelmes 5:07bbe020eb65 168 }
gelmes 5:07bbe020eb65 169
gelmes 3:5ffe7e9c0bb3 170 void update(){
gelmes 4:b37fd183e46a 171 //Update IMU Values
gelmes 4:b37fd183e46a 172 IMUUpdate(mpu6050);
gelmes 4:b37fd183e46a 173 yawIn = yaw;
gelmes 4:b37fd183e46a 174 rollIn = roll;
gelmes 4:b37fd183e46a 175 pitchIn = pitch;
gelmes 5:07bbe020eb65 176 xIn = ax;
gelmes 5:07bbe020eb65 177 yIn = ay;
gelmes 5:07bbe020eb65 178 zIn = az;
gelmes 4:b37fd183e46a 179
gelmes 4:b37fd183e46a 180 //Calculate PID values
gelmes 4:b37fd183e46a 181 pidy.Compute();
gelmes 5:07bbe020eb65 182 pidr.Compute();
gelmes 5:07bbe020eb65 183 pidp.Compute();
gelmes 5:07bbe020eb65 184 pidX.Compute();
gelmes 5:07bbe020eb65 185 pidY.Compute();
gelmes 5:07bbe020eb65 186 pidZ.Compute();
gelmes 5:07bbe020eb65 187
gelmes 5:07bbe020eb65 188 /*
gelmes 5:07bbe020eb65 189 Cameras
gelmes 5:07bbe020eb65 190 FL ----- F ->--- FR
gelmes 5:07bbe020eb65 191 | | |
gelmes 5:07bbe020eb65 192 ˄ | |
gelmes 5:07bbe020eb65 193 | | |
gelmes 5:07bbe020eb65 194 L | R
gelmes 5:07bbe020eb65 195 | | |
gelmes 5:07bbe020eb65 196 | | ˅
gelmes 5:07bbe020eb65 197 | | |
gelmes 5:07bbe020eb65 198 BL ---<- B ----- BR
gelmes 5:07bbe020eb65 199
gelmes 5:07bbe020eb65 200 0 ----- 1 ->--- 2
gelmes 5:07bbe020eb65 201 | | |
gelmes 5:07bbe020eb65 202 ˄ | |
gelmes 5:07bbe020eb65 203 | | |
gelmes 5:07bbe020eb65 204 7 | 3
gelmes 5:07bbe020eb65 205 | | |
gelmes 5:07bbe020eb65 206 | | ˅
gelmes 5:07bbe020eb65 207 | | |
gelmes 5:07bbe020eb65 208 6 ---<- 5 ----- 4
gelmes 5:07bbe020eb65 209
gelmes 5:07bbe020eb65 210 */
gelmes 5:07bbe020eb65 211
gelmes 5:07bbe020eb65 212 //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
gelmes 5:07bbe020eb65 213
gelmes 5:07bbe020eb65 214 //Values used in Dynamic Magnitude Calculations
gelmes 5:07bbe020eb65 215 float accxs = xOut * xOut * abs(xOut) / xOut;
gelmes 5:07bbe020eb65 216 float accys = yOut * yOut * abs(yOut) / yOut;
gelmes 5:07bbe020eb65 217 float acczs = zOut * zOut * abs(zOut) / zOut;
gelmes 5:07bbe020eb65 218 float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
gelmes 5:07bbe020eb65 219 float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
gelmes 5:07bbe020eb65 220 float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
gelmes 5:07bbe020eb65 221
gelmes 5:07bbe020eb65 222 //Values used for Influence calculations
gelmes 5:07bbe020eb65 223 float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
gelmes 5:07bbe020eb65 224 float yy = (abs(yOut) + abs(yawOut)) * 255;
gelmes 5:07bbe020eb65 225 float xy = (abs(xOut) + abs(yawOut)) * 255;
gelmes 5:07bbe020eb65 226
gelmes 5:07bbe020eb65 227 // float zpr = (zOut + pitchOut + rollOut) * 255;
gelmes 5:07bbe020eb65 228 // float yy = (yOut + yawOut) * 255;
gelmes 5:07bbe020eb65 229 // float xy = (xOut + yawOut) * 255;
gelmes 5:07bbe020eb65 230
gelmes 5:07bbe020eb65 231 // if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255;
gelmes 5:07bbe020eb65 232 // if (abs(yy)<255 && abs(yy)>=0) yy = 255;
gelmes 5:07bbe020eb65 233 // if (abs(xy)<255 && abs(xy)>=0) xy = 255;
gelmes 5:07bbe020eb65 234 // if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255;
gelmes 5:07bbe020eb65 235 // if (abs(yy)>-255 && abs(yy)<0) yy = -255;
gelmes 5:07bbe020eb65 236 // if (abs(xy)>-255 && abs(xy)<0) xy = -255;
gelmes 5:07bbe020eb65 237
gelmes 5:07bbe020eb65 238 if (abs(zpr)<255) zpr = 255;
gelmes 5:07bbe020eb65 239 if (abs(yy)<255) yy = 255;
gelmes 5:07bbe020eb65 240 if (abs(xy)<255) xy = 255;
gelmes 5:07bbe020eb65 241
gelmes 5:07bbe020eb65 242 //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr));
gelmes 4:b37fd183e46a 243
gelmes 4:b37fd183e46a 244 //Spit out PID values
gelmes 5:07bbe020eb65 245
gelmes 5:07bbe020eb65 246 // m0 = abs((acczs + pitchs + rolls) / zpr);//
gelmes 5:07bbe020eb65 247 // m1 = abs((accys + yaws) / yy);
gelmes 5:07bbe020eb65 248 // m2 = abs((acczs + pitchs - rolls) / zpr);//
gelmes 5:07bbe020eb65 249 // m3 = abs((accxs + yaws) / xy);
gelmes 5:07bbe020eb65 250 // m4 = abs((acczs - pitchs - rolls) / zpr);//
gelmes 5:07bbe020eb65 251 // m5 = abs((accys + yaws) / yy);
gelmes 5:07bbe020eb65 252 // m6 = abs((acczs - pitchs + rolls) / zpr);//
gelmes 5:07bbe020eb65 253 // m7 = abs((accxs + yaws) / yy);
gelmes 4:b37fd183e46a 254
gelmes 5:07bbe020eb65 255 m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 256 m1 = (accys + yaws) / yy + 0.5;
gelmes 5:07bbe020eb65 257 m2 = (acczs + pitchs - rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 258 m3 = (accxs + yaws) / xy + 0.5;
gelmes 5:07bbe020eb65 259 m4 = (acczs - pitchs - rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 260 m5 = (accys + yaws) / yy + 0.5;
gelmes 5:07bbe020eb65 261 m6 = (acczs - pitchs + rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 262 m7 = (accxs + yaws) / yy + 0.5;
gelmes 4:b37fd183e46a 263
gelmes 5:07bbe020eb65 264 //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
gelmes 5:07bbe020eb65 265 //pc.printf("%f,%f,%f,%f,%f \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / zpr + 0.5);
gelmes 5:07bbe020eb65 266 //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 267 // pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
gelmes 5:07bbe020eb65 268 //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll);
gelmes 3:5ffe7e9c0bb3 269 }
aolgu003 6:b45b74fd6a07 270
aolgu003 6:b45b74fd6a07 271 void updateCommand() {
aolgu003 6:b45b74fd6a07 272 char c = 0;
aolgu003 6:b45b74fd6a07 273 string command;
aolgu003 6:b45b74fd6a07 274 char buffer[BUFFER_SIZE] = {' '};
aolgu003 6:b45b74fd6a07 275 int buffer_iter = 0;
aolgu003 6:b45b74fd6a07 276 pc.printf("Checking for command\n");
aolgu003 6:b45b74fd6a07 277
aolgu003 6:b45b74fd6a07 278 // Note: you need to actually read from the serial to clear the RX interrupt
aolgu003 6:b45b74fd6a07 279 if (pc.readable()) {
aolgu003 6:b45b74fd6a07 280 pc.printf("Found command\n");
aolgu003 6:b45b74fd6a07 281 while (pc.readable()) {
aolgu003 6:b45b74fd6a07 282 c = pc.getc();
aolgu003 6:b45b74fd6a07 283 pc.putc(c);
aolgu003 6:b45b74fd6a07 284 buffer[buffer_iter] = c;
aolgu003 6:b45b74fd6a07 285 buffer_iter++;
aolgu003 6:b45b74fd6a07 286 }
aolgu003 6:b45b74fd6a07 287 pc.printf("Command saved to buffer\n");
aolgu003 6:b45b74fd6a07 288 command = strtok (buffer," ,\n");
aolgu003 6:b45b74fd6a07 289
aolgu003 6:b45b74fd6a07 290 if (strcmp(command.c_str(), "a")) {
aolgu003 6:b45b74fd6a07 291 this->yawPoint = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 292 this->pitchPoint = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 293 this->rollPoint = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 294 pc.printf("Received Attitude points: yawPoint: %f, pitchPoint: %f, rollPoint: %f\n", this->yawPoint, this->pitchPoint, this->rollPoint);
aolgu003 6:b45b74fd6a07 295 } else if (strcmp(command.c_str(), "b")) {
aolgu003 6:b45b74fd6a07 296 this->xPoint = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 297 this->yPoint = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 298 this->zPoint = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 299 pc.printf("Received X,Y,Z points: X: %f, Y: %f, Z: %f\n", this->xPoint, this->yPoint, this->zPoint);
aolgu003 6:b45b74fd6a07 300 } else if (strcmp(command.c_str(), "c")) {
aolgu003 6:b45b74fd6a07 301 this->p_gain = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 302 this->i_gain = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 303 this->d_gain = atof(strtok (NULL, " ,\n"));
aolgu003 6:b45b74fd6a07 304
aolgu003 6:b45b74fd6a07 305 this->SetYawPID(this->p_gain, this->i_gain, this->d_gain);
aolgu003 6:b45b74fd6a07 306 pc.printf("Received PID values P: %f, I: %f, D: %f\n", this->p_gain, this->i_gain, this->d_gain);
aolgu003 6:b45b74fd6a07 307
aolgu003 6:b45b74fd6a07 308 }
aolgu003 6:b45b74fd6a07 309
aolgu003 6:b45b74fd6a07 310 memset(buffer, ' ', sizeof(buffer));
aolgu003 6:b45b74fd6a07 311 buffer_iter = 0;
aolgu003 6:b45b74fd6a07 312 fflush(stdout);
aolgu003 6:b45b74fd6a07 313 }
aolgu003 6:b45b74fd6a07 314 }
gelmes 3:5ffe7e9c0bb3 315 };
aolgu003 6:b45b74fd6a07 316
gelmes 3:5ffe7e9c0bb3 317 #endif