Marco Rubio / Mbed 2 deprecated RTOS_Controller

Controller for Seagoat in the RoboSub competition

Dependencies:   Servo mbed

Fork of ESC by Matteo Terruzzi

vessel.h@5:07bbe020eb65, 2016-07-26 (annotated)

Committer:
gelmes
Date:
Tue Jul 26 17:22:33 2016 +0000
Revision:
5:07bbe020eb65
Parent:
4:b37fd183e46a 
This is a working implementation of the Controller;

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"
gelmes 3:5ffe7e9c0bb3 5 #include "MPU6050.h"
gelmes 3:5ffe7e9c0bb3 6 #include "Servo.h"
gelmes 3:5ffe7e9c0bb3 7 #include "IMU.h"
gelmes 3:5ffe7e9c0bb3 8 #include "PID.h"
gelmes 3:5ffe7e9c0bb3 9 /*
gelmes 3:5ffe7e9c0bb3 10 Cameras
gelmes 3:5ffe7e9c0bb3 11 FL ----- F ->--- FR
gelmes 3:5ffe7e9c0bb3 12 | | |
gelmes 3:5ffe7e9c0bb3 13 ˄ | |
gelmes 3:5ffe7e9c0bb3 14 | | |
gelmes 3:5ffe7e9c0bb3 15 L | R
gelmes 3:5ffe7e9c0bb3 16 | | |
gelmes 3:5ffe7e9c0bb3 17 | | ˅
gelmes 3:5ffe7e9c0bb3 18 | | |
gelmes 3:5ffe7e9c0bb3 19 BL ---<- B ----- BR
gelmes 3:5ffe7e9c0bb3 20
gelmes 3:5ffe7e9c0bb3 21 0 ----- 1 ->--- 2
gelmes 3:5ffe7e9c0bb3 22 | | |
gelmes 3:5ffe7e9c0bb3 23 ˄ | |
gelmes 3:5ffe7e9c0bb3 24 | | |
gelmes 3:5ffe7e9c0bb3 25 7 | 3
gelmes 3:5ffe7e9c0bb3 26 | | |
gelmes 3:5ffe7e9c0bb3 27 | | ˅
gelmes 3:5ffe7e9c0bb3 28 | | |
gelmes 3:5ffe7e9c0bb3 29 6 ---<- 5 ----- 4
gelmes 3:5ffe7e9c0bb3 30 */
gelmes 3:5ffe7e9c0bb3 31 class Vessel
gelmes 3:5ffe7e9c0bb3 32 {
gelmes 3:5ffe7e9c0bb3 33
gelmes 3:5ffe7e9c0bb3 34 private:
gelmes 4:b37fd183e46a 35 // Servo m0;
gelmes 4:b37fd183e46a 36 // Servo m1;
gelmes 4:b37fd183e46a 37 // Servo m2;
gelmes 4:b37fd183e46a 38 // Servo m3;
gelmes 4:b37fd183e46a 39 // Servo m4;
gelmes 4:b37fd183e46a 40 // Servo m5;
gelmes 4:b37fd183e46a 41 // Servo m6;
gelmes 4:b37fd183e46a 42 // Servo m7;
gelmes 4:b37fd183e46a 43
gelmes 4:b37fd183e46a 44 PwmOut m0;
gelmes 4:b37fd183e46a 45 PwmOut m1;
gelmes 4:b37fd183e46a 46 PwmOut m2;
gelmes 4:b37fd183e46a 47 PwmOut m3;
gelmes 4:b37fd183e46a 48 PwmOut m4;
gelmes 4:b37fd183e46a 49 PwmOut m5;
gelmes 4:b37fd183e46a 50 PwmOut m6;
gelmes 4:b37fd183e46a 51 PwmOut m7;
gelmes 3:5ffe7e9c0bb3 52
gelmes 3:5ffe7e9c0bb3 53 PwmOut led1;
gelmes 3:5ffe7e9c0bb3 54 MPU6050 mpu6050;
gelmes 3:5ffe7e9c0bb3 55 double yawPoint, yawIn, yawOut;
gelmes 3:5ffe7e9c0bb3 56 double rollPoint, rollIn, rollOut;
gelmes 3:5ffe7e9c0bb3 57 double pitchPoint, pitchIn, pitchOut;
gelmes 5:07bbe020eb65 58 double xPoint, xIn, xOut;
gelmes 5:07bbe020eb65 59 double yPoint, yIn, yOut;
gelmes 5:07bbe020eb65 60 double zPoint, zIn, zOut;
gelmes 5:07bbe020eb65 61 PID pidy, pidr, pidp, pidX, pidY, pidZ;
gelmes 3:5ffe7e9c0bb3 62
gelmes 3:5ffe7e9c0bb3 63 public:
gelmes 3:5ffe7e9c0bb3 64 void Start_IMU() {
gelmes 3:5ffe7e9c0bb3 65 pc.printf("Starting up\n\r");
gelmes 3:5ffe7e9c0bb3 66 pc.baud(9600);
gelmes 3:5ffe7e9c0bb3 67 i2c.frequency(400000); // use fast (400 kHz) I2C
gelmes 3:5ffe7e9c0bb3 68 IMUinit(mpu6050);
gelmes 3:5ffe7e9c0bb3 69 IMUPrintData(mpu6050);
gelmes 3:5ffe7e9c0bb3 70 }
gelmes 3:5ffe7e9c0bb3 71
gelmes 3:5ffe7e9c0bb3 72 //Initialise all of the vessels starting parameters
gelmes 5:07bbe020eb65 73 Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
gelmes 3:5ffe7e9c0bb3 74 pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
gelmes 3:5ffe7e9c0bb3 75 pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 76 pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 77 pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 78 pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
gelmes 5:07bbe020eb65 79 pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT){
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;
gelmes 5:07bbe020eb65 99
gelmes 5:07bbe020eb65 100 m0 = 0.5;
gelmes 5:07bbe020eb65 101 m1 = 0.5;
gelmes 5:07bbe020eb65 102 m2 = 0.5;
gelmes 5:07bbe020eb65 103 m3 = 0.5;
gelmes 5:07bbe020eb65 104 m4 = 0.5;
gelmes 5:07bbe020eb65 105 m5 = 0.5;
gelmes 5:07bbe020eb65 106 m6 = 0.5;
gelmes 5:07bbe020eb65 107 m7 = 0.5;
gelmes 3:5ffe7e9c0bb3 108
gelmes 3:5ffe7e9c0bb3 109 Start_IMU();
gelmes 5:07bbe020eb65 110 pc.printf("Seagoat Initialized \n\r");
gelmes 3:5ffe7e9c0bb3 111 }
gelmes 3:5ffe7e9c0bb3 112
gelmes 3:5ffe7e9c0bb3 113 void SetYawPID(double Kp, double Ki, double Kd) {
gelmes 3:5ffe7e9c0bb3 114 pidy.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 115 }
gelmes 3:5ffe7e9c0bb3 116
gelmes 3:5ffe7e9c0bb3 117 void SetRollPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 118 pidr.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 119 }
gelmes 3:5ffe7e9c0bb3 120
gelmes 3:5ffe7e9c0bb3 121 void SetPitchPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 122 pidp.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 123 }
gelmes 5:07bbe020eb65 124
gelmes 5:07bbe020eb65 125 void SetXPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 126 pidX.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 127 }
gelmes 5:07bbe020eb65 128
gelmes 5:07bbe020eb65 129 void SetYPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 130 pidY.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 131 }
gelmes 5:07bbe020eb65 132
gelmes 5:07bbe020eb65 133 void SetZPID(double Kp, double Ki, double Kd) {
gelmes 5:07bbe020eb65 134 pidZ.SetTunings(Kp, Ki, Kd);
gelmes 5:07bbe020eb65 135 }
gelmes 3:5ffe7e9c0bb3 136
gelmes 3:5ffe7e9c0bb3 137 //This is where the magic happens
gelmes 5:07bbe020eb65 138 void motorTest(){
gelmes 5:07bbe020eb65 139 pwmSweep(m0);
gelmes 5:07bbe020eb65 140 pwmSweep(m1);
gelmes 5:07bbe020eb65 141 pwmSweep(m2);
gelmes 5:07bbe020eb65 142 pwmSweep(m3);
gelmes 5:07bbe020eb65 143 pwmSweep(m4);
gelmes 5:07bbe020eb65 144 pwmSweep(m5);
gelmes 5:07bbe020eb65 145 pwmSweep(m6);
gelmes 5:07bbe020eb65 146 pwmSweep(m7);
gelmes 5:07bbe020eb65 147 }
gelmes 5:07bbe020eb65 148
gelmes 5:07bbe020eb65 149 void pwmSweep(PwmOut motor){
gelmes 5:07bbe020eb65 150 for(float i = 0; i < 80; i++){
gelmes 5:07bbe020eb65 151 motor = i/255;
gelmes 5:07bbe020eb65 152 wait(0.002);
gelmes 5:07bbe020eb65 153 }
gelmes 5:07bbe020eb65 154 // for(float i = 80; i >= 0; i--){
gelmes 5:07bbe020eb65 155 // motor = i/255;
gelmes 5:07bbe020eb65 156 // wait(0.002);
gelmes 5:07bbe020eb65 157 // }
gelmes 5:07bbe020eb65 158 }
gelmes 5:07bbe020eb65 159 void calibrate(){
gelmes 5:07bbe020eb65 160 IMUUpdate(mpu6050);
gelmes 5:07bbe020eb65 161 pc.printf("Calibrating...\n\r");
gelmes 5:07bbe020eb65 162 }
gelmes 5:07bbe020eb65 163
gelmes 3:5ffe7e9c0bb3 164 void update(){
gelmes 4:b37fd183e46a 165 //Update IMU Values
gelmes 4:b37fd183e46a 166 IMUUpdate(mpu6050);
gelmes 4:b37fd183e46a 167 yawIn = yaw;
gelmes 4:b37fd183e46a 168 rollIn = roll;
gelmes 4:b37fd183e46a 169 pitchIn = pitch;
gelmes 5:07bbe020eb65 170 xIn = ax;
gelmes 5:07bbe020eb65 171 yIn = ay;
gelmes 5:07bbe020eb65 172 zIn = az;
gelmes 4:b37fd183e46a 173
gelmes 4:b37fd183e46a 174 //Calculate PID values
gelmes 4:b37fd183e46a 175 pidy.Compute();
gelmes 5:07bbe020eb65 176 pidr.Compute();
gelmes 5:07bbe020eb65 177 pidp.Compute();
gelmes 5:07bbe020eb65 178 pidX.Compute();
gelmes 5:07bbe020eb65 179 pidY.Compute();
gelmes 5:07bbe020eb65 180 pidZ.Compute();
gelmes 5:07bbe020eb65 181
gelmes 5:07bbe020eb65 182 /*
gelmes 5:07bbe020eb65 183 Cameras
gelmes 5:07bbe020eb65 184 FL ----- F ->--- FR
gelmes 5:07bbe020eb65 185 | | |
gelmes 5:07bbe020eb65 186 ˄ | |
gelmes 5:07bbe020eb65 187 | | |
gelmes 5:07bbe020eb65 188 L | R
gelmes 5:07bbe020eb65 189 | | |
gelmes 5:07bbe020eb65 190 | | ˅
gelmes 5:07bbe020eb65 191 | | |
gelmes 5:07bbe020eb65 192 BL ---<- B ----- BR
gelmes 5:07bbe020eb65 193
gelmes 5:07bbe020eb65 194 0 ----- 1 ->--- 2
gelmes 5:07bbe020eb65 195 | | |
gelmes 5:07bbe020eb65 196 ˄ | |
gelmes 5:07bbe020eb65 197 | | |
gelmes 5:07bbe020eb65 198 7 | 3
gelmes 5:07bbe020eb65 199 | | |
gelmes 5:07bbe020eb65 200 | | ˅
gelmes 5:07bbe020eb65 201 | | |
gelmes 5:07bbe020eb65 202 6 ---<- 5 ----- 4
gelmes 5:07bbe020eb65 203
gelmes 5:07bbe020eb65 204 */
gelmes 5:07bbe020eb65 205
gelmes 5:07bbe020eb65 206 //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
gelmes 5:07bbe020eb65 207
gelmes 5:07bbe020eb65 208 //Values used in Dynamic Magnitude Calculations
gelmes 5:07bbe020eb65 209 float accxs = xOut * xOut * abs(xOut) / xOut;
gelmes 5:07bbe020eb65 210 float accys = yOut * yOut * abs(yOut) / yOut;
gelmes 5:07bbe020eb65 211 float acczs = zOut * zOut * abs(zOut) / zOut;
gelmes 5:07bbe020eb65 212 float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
gelmes 5:07bbe020eb65 213 float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
gelmes 5:07bbe020eb65 214 float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
gelmes 5:07bbe020eb65 215
gelmes 5:07bbe020eb65 216 //Values used for Influence calculations
gelmes 5:07bbe020eb65 217 float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
gelmes 5:07bbe020eb65 218 float yy = (abs(yOut) + abs(yawOut)) * 255;
gelmes 5:07bbe020eb65 219 float xy = (abs(xOut) + abs(yawOut)) * 255;
gelmes 5:07bbe020eb65 220
gelmes 5:07bbe020eb65 221 // float zpr = (zOut + pitchOut + rollOut) * 255;
gelmes 5:07bbe020eb65 222 // float yy = (yOut + yawOut) * 255;
gelmes 5:07bbe020eb65 223 // float xy = (xOut + yawOut) * 255;
gelmes 5:07bbe020eb65 224
gelmes 5:07bbe020eb65 225 // if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255;
gelmes 5:07bbe020eb65 226 // if (abs(yy)<255 && abs(yy)>=0) yy = 255;
gelmes 5:07bbe020eb65 227 // if (abs(xy)<255 && abs(xy)>=0) xy = 255;
gelmes 5:07bbe020eb65 228 // if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255;
gelmes 5:07bbe020eb65 229 // if (abs(yy)>-255 && abs(yy)<0) yy = -255;
gelmes 5:07bbe020eb65 230 // if (abs(xy)>-255 && abs(xy)<0) xy = -255;
gelmes 5:07bbe020eb65 231
gelmes 5:07bbe020eb65 232 if (abs(zpr)<255) zpr = 255;
gelmes 5:07bbe020eb65 233 if (abs(yy)<255) yy = 255;
gelmes 5:07bbe020eb65 234 if (abs(xy)<255) xy = 255;
gelmes 5:07bbe020eb65 235
gelmes 5:07bbe020eb65 236 //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr));
gelmes 4:b37fd183e46a 237
gelmes 4:b37fd183e46a 238 //Spit out PID values
gelmes 5:07bbe020eb65 239
gelmes 5:07bbe020eb65 240 // m0 = abs((acczs + pitchs + rolls) / zpr);//
gelmes 5:07bbe020eb65 241 // m1 = abs((accys + yaws) / yy);
gelmes 5:07bbe020eb65 242 // m2 = abs((acczs + pitchs - rolls) / zpr);//
gelmes 5:07bbe020eb65 243 // m3 = abs((accxs + yaws) / xy);
gelmes 5:07bbe020eb65 244 // m4 = abs((acczs - pitchs - rolls) / zpr);//
gelmes 5:07bbe020eb65 245 // m5 = abs((accys + yaws) / yy);
gelmes 5:07bbe020eb65 246 // m6 = abs((acczs - pitchs + rolls) / zpr);//
gelmes 5:07bbe020eb65 247 // m7 = abs((accxs + yaws) / yy);
gelmes 4:b37fd183e46a 248
gelmes 5:07bbe020eb65 249 m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 250 m1 = (accys + yaws) / yy + 0.5;
gelmes 5:07bbe020eb65 251 m2 = (acczs + pitchs - rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 252 m3 = (accxs + yaws) / xy + 0.5;
gelmes 5:07bbe020eb65 253 m4 = (acczs - pitchs - rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 254 m5 = (accys + yaws) / yy + 0.5;
gelmes 5:07bbe020eb65 255 m6 = (acczs - pitchs + rolls) / zpr + 0.5;//
gelmes 5:07bbe020eb65 256 m7 = (accxs + yaws) / yy + 0.5;
gelmes 4:b37fd183e46a 257
gelmes 5:07bbe020eb65 258 //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
gelmes 5:07bbe020eb65 259 //pc.printf("%f,%f,%f,%f,%f \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / zpr + 0.5);
gelmes 5:07bbe020eb65 260 //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 5:07bbe020eb65 261 // pc.printf("YAW: %f, %f, %f, %f\n\r", xOut, yawOut, yawIn, yawPoint);
gelmes 5:07bbe020eb65 262 //pc.printf("YAW: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
gelmes 5:07bbe020eb65 263 //pc.printf("ACC: %f, %f, %f\n\r", ax, ay, az);
gelmes 5:07bbe020eb65 264 //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll);
gelmes 3:5ffe7e9c0bb3 265 }
gelmes 3:5ffe7e9c0bb3 266 };
gelmes 3:5ffe7e9c0bb3 267 #endif