vessel.h

00001 #ifndef VESSEL_H
00002 #define VESSEL_H
00003 
00004 #include "mbed.h"
00005 #include "MODSERIAL.h"
00006 #include "MPU6050.h"
00007 #include "Servo.h"
00008 #include "IMU.h"
00009 #include "PID.h"
00010 #include <string>
00011 #include "MS5837.h"
00012 
00013 MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);
00014 
00015 #define BUFFER_SIZE 255
00016 
00017 class Vessel
00018 {
00019 
00020 private:
00021     Servo m0;
00022     Servo m1;
00023     Servo m2;
00024     Servo m3;
00025     Servo m4;
00026     Servo m5;
00027     Servo m6;
00028     Servo m7;
00029 
00030     AnalogIn powerPin;
00031     int motorState;
00032     int runningState;
00033     Timer runningTime;
00034 
00035 //    PwmOut m0;
00036 //    PwmOut m1;
00037 //    PwmOut m2;
00038 //    PwmOut m3;
00039 //    PwmOut m4;
00040 //    PwmOut m5;
00041 //    PwmOut m6;
00042 //    PwmOut m7;
00043 
00044     PwmOut led1;
00045     MPU6050 mpu6050;
00046 
00047     double yawPoint, yawIn, yawOut, lastyawPoint;
00048     double rollPoint, rollIn, rollOut, lastrollPoint;
00049     double pitchPoint, pitchIn, pitchOut, lastpitchPoint;
00050     double xPoint, xIn, xOut;
00051     double yPoint, yIn, yOut;
00052     double zPoint, zIn, zOut;
00053     double dPoint, dIn, dOut;
00054     double p_gain, i_gain, d_gain;
00055     PID pidy, pidr, pidp, pidX, pidY, pidZ, pidd;
00056     char buffer[BUFFER_SIZE];
00057 public:
00058     float depth;
00059 
00060     void Start_IMU() {
00061         pc.printf("Starting up\n\r");
00062         pc.baud(9600);
00063         i2c.frequency(400000);  // use fast (400 kHz) I2C
00064         IMUinit(mpu6050);
00065         pressure_sensor.MS5837Init();
00066         IMUPrintData(mpu6050);
00067         runningTime.start();
00068     }
00069 
00070     //Initialise all of the vessels starting parameters
00071     Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
00072         pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
00073         pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
00074         pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT),
00075         pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
00076         pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
00077         pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT),
00078         pidd(&dIn, &dOut, &dPoint,1,1,1, DIRECT),
00079         powerPin(A5) {
00080 
00081         pidy.SetMode(AUTOMATIC);  //Yaw PID
00082         pidy.SetOutputLimits(-255,255);
00083         yawPoint = 0;
00084         pidr.SetMode(AUTOMATIC);  //Roll PID
00085         pidr.SetOutputLimits(-255,255);
00086         pitchPoint = 0;
00087         pidp.SetMode(AUTOMATIC);  //Pitch PID
00088         pidp.SetOutputLimits(-255,255);
00089         rollPoint = 0;
00090         pidX.SetMode(AUTOMATIC);  //Pitch PID
00091         pidX.SetOutputLimits(-255,255);
00092         xPoint = 0;
00093         pidY.SetMode(AUTOMATIC);  //Pitch PID
00094         pidY.SetOutputLimits(-255,255);
00095         yPoint = 0;
00096         pidZ.SetMode(AUTOMATIC);  //Pitch PID
00097         pidZ.SetOutputLimits(-255,255);
00098         zPoint = 0;
00099         pidd.SetMode(AUTOMATIC);  //Pitch PID
00100         pidd.SetOutputLimits(-255,255);
00101         wait(0.5);
00102        dPoint = depth;
00103 
00104         m0 = 0.5;
00105         m1 = 0.5;
00106         m2 = 0.5;
00107         m3 = 0.5;
00108         m4 = 0.5;
00109         m5 = 0.5;
00110         m6 = 0.5;
00111         m7 = 0.5;
00112 
00113         motorState = 1;
00114         runningState = -1;
00115 
00116         Start_IMU();
00117         pc.printf("Seagoat Initialized \n\r");
00118     }
00119 
00120     void SetYawPID(double Kp, double Ki, double Kd) {
00121         pidy.SetTunings(Kp, Ki, Kd);
00122     }
00123 
00124     void SetRollPID(double Kp, double Ki, double Kd) {
00125         pidr.SetTunings(Kp, Ki, Kd);
00126     }
00127 
00128     void SetPitchPID(double Kp, double Ki, double Kd) {
00129         pidp.SetTunings(Kp, Ki, Kd);
00130     }
00131 
00132     void SetXPID(double Kp, double Ki, double Kd) {
00133         pidX.SetTunings(Kp, Ki, Kd);
00134     }
00135 
00136     void SetYPID(double Kp, double Ki, double Kd) {
00137         pidY.SetTunings(Kp, Ki, Kd);
00138     }
00139 
00140     void SetZPID(double Kp, double Ki, double Kd) {
00141         pidZ.SetTunings(Kp, Ki, Kd);
00142     }
00143     void SetdPID(double Kp, double Ki, double Kd) {
00144         pidd.SetTunings(Kp, Ki, Kd);
00145     }
00146 
00147     void calibrate() {
00148         IMUUpdate(mpu6050);
00149         pc.printf("Calibrating...\n\r");
00150         //pressure_sensor.Barometer_MS5837();
00151         //depth = pressure_sensor.MS5837_Pressure();
00152     }
00153 
00154     void update() {
00155         //Update IMU Values
00156         IMUUpdate(mpu6050);
00157 
00158         //pressure_sensor.Barometer_MS5837();
00159         //depth = pressure_sensor.MS5837_Pressure();
00160         //pc.printf("Pressure: %f %f\n", depth, dPoint);
00161 
00162         //Detect if the switch is turned on
00163         if(!motorState && powerPin.read() == 1) {
00164             runningTime.stop();
00165             initMotors();
00166             motorState = 1;
00167             runningState += 1;
00168             if(runningState == 2) runningState = 0;
00169 
00170             pc.printf("Motors Detected");
00171 
00172             yawPoint = yaw;
00173             if(runningState == 0) pitchPoint = pitch;
00174             else pitchPoint = 0;
00175             dPoint = depth;
00176             runningTime.reset();
00177             runningTime.start();
00178         } else if(powerPin.read() != 1) {
00179             motorState = 0;
00180         }
00181 
00182         yawIn = yaw;
00183         rollIn = roll;
00184         pitchIn = pitch;
00185         xIn = ax;
00186         yIn = ay;
00187         zIn = az;
00188 
00189         //Calculate PID values
00190         pidy.Compute();
00191         pidr.Compute();
00192         pidp.Compute();
00193         pidX.Compute();
00194         pidY.Compute();
00195         pidZ.Compute();
00196 
00197         /*
00198                 Cameras
00199           FL ----- F ->--- FR
00200           |        |       |
00201           ˄        |       |
00202           |        |       |
00203           L        |       R
00204           |        |       |
00205           |        |       ˅
00206           |        |       |
00207           BL ---<- B ----- BR
00208 
00209           0  ----- 1 ->--- 2
00210           |        |       |
00211           ˄        |       |
00212           |        |       |
00213           7        |       3
00214           |        |       |
00215           |        |       ˅
00216           |        |       |
00217           6  ---<- 5 ----- 4
00218 
00219         */
00220 
00221         //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
00222 
00223         float forwardThrust = 100;
00224 
00225         //Values used in Dynamic Magnitude Calculations
00226         float accxs = xOut * xOut * abs(xOut) / xOut;
00227         float accys = yOut * yOut * abs(yOut) / yOut;
00228         float acczs = zOut * zOut * abs(zOut) / zOut;
00229         float depths = dOut * dOut;
00230         float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
00231         float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
00232         float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
00233 
00234         //Values used for Influence calculations
00235         float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
00236         float yy  = (abs(yOut) + abs(yawOut)) * 255;
00237         float xy  = (abs(xOut) + abs(yawOut)) * 255;
00238 
00239         if (abs(zpr)<255) zpr = 255;
00240         if (abs(yy)<255) yy = 255;
00241         if (abs(xy)<255) xy = 255;
00242 
00243         if(runningState == 0) {
00244                 if(runningTime < 1) {
00245                     SetYawPID(1,0,0);
00246                     SetRollPID(1,0,0);
00247                     SetPitchPID(1,0,0);
00248                     wait(1);
00249                 }
00250                 else if(runningTime < 15){ //15
00251                     m0 = (acczs + pitchs + rolls) / zpr + 0.5;
00252                     m1 = (accxs + yaws) / -xy + 0.5;
00253                     m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00254                     //m3 = (accys + yaws) / yy + 0.5;
00255                     m3 = 0.7;
00256                     m4 = (acczs - pitchs - rolls) / zpr + 0.5;
00257                     m5 = (accxs + yaws) / -xy + 0.5;
00258                     m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00259                     //m7 = (-accys + yaws) / -yy + 0.5;
00260                     m7 = 0.7;
00261                 }
00262                 else if(runningTime < 16){ //16
00263                     neutralizeMotors();
00264                     pitchPoint = 0;
00265                 }
00266                 else{
00267                     m0 = (acczs + pitchs + rolls) / zpr + 0.5;
00268                     m1 = (accxs + yaws) / -xy + 0.5;
00269                     m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00270                     //m3 = (accys + yaws) / yy + 0.5;
00271                     m3 = 0.7;
00272                     m4 = (acczs - pitchs - rolls) / zpr + 0.5;
00273                     m5 = (accxs + yaws) / -xy + 0.5;
00274                     m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00275                     //m7 = (-accys + yaws) / -yy + 0.5;
00276                     m7 = 0.7;
00277                 }
00278         } 
00279         else if(runningState == 1) {
00280                 if(runningTime < 1) {
00281                     SetYawPID(2,0,0.3);
00282                     SetRollPID(2,0,0.3);
00283                     SetPitchPID(2,0,0.3);
00284                     lastyawPoint = yawPoint;
00285                     wait(1);
00286                 } else if(runningTime < 27) { //27
00287                     //Go straight to Gate
00288                     m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
00289                     m1 = (accxs + yaws) / -xy + 0.5;
00290                     m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00291                     m3 = 0.7;
00292                     m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
00293                     m5 = (accxs + yaws) / -xy + 0.5;
00294                     m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00295                     m7 = 0.7;
00296                 } else if(runningTime < 28) { //28
00297                     neutralizeMotors();
00298                     //Set turning 180 angle aim for Gate
00299                     yawPoint = correctAngle(lastyawPoint - 160);
00300                 } else if(runningTime < 32) { //32
00301                     //allign to new yaw
00302                     m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
00303                     m1 = (accxs + yaws) / -xy + 0.5;
00304                     m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00305                     m3 = 0.5;
00306                     m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
00307                     m5 = (accxs + yaws) / -xy + 0.5;
00308                     m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00309                     m7 = 0.5;
00310                 }  else if(runningTime < 42) { //42
00311                     //go go go
00312                     m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
00313                     m1 = (accxs + yaws) / -xy + 0.5;
00314                     m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00315                     m3 = 0.3;
00316                     m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
00317                     m5 = (accxs + yaws) / -xy + 0.5;
00318                     m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00319                     m7 = 0.3;
00320                 } else if(runningTime < 43) { //43
00321                     neutralizeMotors();
00322                     //Set turning 180 angle aim for Gate
00323                     yawPoint = correctAngle(lastyawPoint - 70);
00324                 } else if(runningTime < 47) { //47
00325                     m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
00326                     m1 = (accxs + yaws) / -xy + 0.5;
00327                     m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00328                     m3 = 0.5;
00329                     m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
00330                     m5 = (accxs + yaws) / -xy + 0.5;
00331                     m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00332                     m7 = 0.5;
00333                 } else if(runningTime < 52) { //52
00334                     m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
00335                     m1 = (accxs + yaws) / -xy + 0.5;
00336                     m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00337                     m3 = 0.7;
00338                     m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
00339                     m5 = (accxs + yaws) / -xy + 0.5;
00340                     m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00341                     m7 = 0.7;
00342                 }
00343        //          else if(runningTime < 36) { //53
00344 //                    neutralizeMotors();
00345 //                    //Go Upside down
00346 //                    rollPoint = correctAngle(lastrollPoint + 170);
00347 //                    pc.printf("%f\n", rollPoint);
00348 //                } else if(runningTime < 40) { //57
00349 //                    m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
00350 //                    m1 = (accxs + yaws) / -xy + 0.5;
00351 //                    m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
00352 //                    m3 = 0.5;
00353 //                    m4 = (acczs - pitchs - rolls) /  zpr + 0.5;
00354 //                    m5 = (accxs + yaws) / -xy + 0.5;
00355 //                    m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
00356 //                    m7 = 0.5;
00357 //                }
00358                  else if(runningTime < 58) { //63
00359                     //Surface
00360                     m0 = 0.7;
00361                     m1 = 0.5;
00362                     m2 = 0.7;
00363                     m3 = 0.5;
00364                     m4 = 0.7;
00365                     m5 = 0.5;
00366                     m6 = 0.7;
00367                     m7 = 0.5;
00368                 }
00369                 else{
00370                     neutralizeMotors();
00371                 }
00372             }
00373 
00374             //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
00375             //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);
00376             //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));
00377             //pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
00378             //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
00379         }
00380         
00381         float correctAngle(float angle){
00382             if(angle > 180) return (angle - 360);
00383             else if(angle < -180) return (angle + 360); 
00384             else return angle;  
00385         }
00386             
00387         void updateCommand() {
00388             //char a = 0;
00389 //            char i = 0;
00390 //            char buffer[10] = {' '};
00391 //            if (device.readable()) {
00392 //                while(a != 'd') {
00393 //                    a = device.getc();
00394 //                    if ((a >= '0' && a <='9') || a == '.') {
00395 //                        buffer[i] = a;
00396 //                        i++;
00397 //                    }
00398 //                }
00399 //                depth = atof(buffer);
00400 //                pc.printf("Depth: '%f'\n", depth);
00401 //            }
00402         }
00403 
00404         void initMotors() {
00405 
00406             neutralizeMotors();
00407             wait(0.5);
00408             m0.calibrate();
00409             m1.calibrate();
00410             m2.calibrate();
00411             m3.calibrate();
00412             m4.calibrate();
00413             m5.calibrate();
00414             m6.calibrate();
00415             m7.calibrate();
00416             wait(3);
00417         }
00418 
00419         void neutralizeMotors() {
00420             m0 = 0.5;
00421             m1 = 0.5;
00422             m2 = 0.5;
00423             m3 = 0.5;
00424             m4 = 0.5;
00425             m5 = 0.5;
00426             m6 = 0.5;
00427             m7 = 0.5;
00428         }
00429 
00430     };
00431 
00432 #endif