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 /*
00016             Cameras
00017       FL ----- F ->--- FR
00018       |        |       |
00019       ˄        |       |
00020       |        |       |
00021       L        |       R
00022       |        |       |
00023       |        |       ˅
00024       |        |       |
00025       BL ---<- B ----- BR
00026 
00027       0  ----- 1 ->--- 2
00028       |        |       |
00029       ˄        |       |
00030       |        |       |
00031       7        |       3
00032       |        |       |
00033       |        |       ˅
00034       |        |       |
00035       6  ---<- 5 ----- 4
00036 */
00037 #define BUFFER_SIZE 255
00038 
00039 class Vessel
00040 {
00041 
00042 private:
00043     Servo m0;
00044     Servo m1;
00045     Servo m2;
00046     Servo m3;
00047     Servo m4;
00048     Servo m5;
00049     Servo m6;
00050     Servo m7;
00051 
00052     AnalogIn powerPin;
00053     int motorState;
00054 //    PwmOut m0;
00055 //    PwmOut m1;
00056 //    PwmOut m2;
00057 //    PwmOut m3;
00058 //    PwmOut m4;
00059 //    PwmOut m5;
00060 //    PwmOut m6;
00061 //    PwmOut m7;
00062 
00063     PwmOut led1;
00064     MPU6050 mpu6050;
00065 
00066     double yawPoint, yawIn, yawOut;
00067     double rollPoint, rollIn, rollOut;
00068     double pitchPoint, pitchIn, pitchOut;
00069     double xPoint, xIn, xOut;
00070     double yPoint, yIn, yOut;
00071     double zPoint, zIn, zOut;
00072     double dPoint, dIn, dOut;
00073     double p_gain, i_gain, d_gain;
00074     PID pidy, pidr, pidp, pidX, pidY, pidZ, pidd;
00075     char buffer[BUFFER_SIZE];
00076 public:
00077     float depth;
00078 
00079     void Start_IMU() {
00080         pc.printf("Starting up\n\r");
00081         pc.baud(9600);
00082         i2c.frequency(400000);  // use fast (400 kHz) I2C
00083         IMUinit(mpu6050);
00084         pressure_sensor.MS5837Init();
00085         IMUPrintData(mpu6050);
00086     }
00087 
00088     //Initialise all of the vessels starting parameters
00089     Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
00090         pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
00091         pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
00092         pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT),
00093         pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
00094         pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
00095         pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT),
00096         pidd(&dIn, &dOut, &dPoint,1,1,1, DIRECT),
00097         powerPin(A5)
00098 {
00099 
00100         pidy.SetMode(AUTOMATIC);  //Yaw PID
00101         pidy.SetOutputLimits(-255,255);
00102         yawPoint = 0;
00103         pidr.SetMode(AUTOMATIC);  //Roll PID
00104         pidr.SetOutputLimits(-255,255);
00105         pitchPoint = 0;
00106         pidp.SetMode(AUTOMATIC);  //Pitch PID
00107         pidp.SetOutputLimits(-255,255);
00108         rollPoint = 0;
00109         pidX.SetMode(AUTOMATIC);  //Pitch PID
00110         pidX.SetOutputLimits(-255,255);
00111         xPoint = 0;
00112         pidY.SetMode(AUTOMATIC);  //Pitch PID
00113         pidY.SetOutputLimits(-255,255);
00114         yPoint = 0;
00115         pidZ.SetMode(AUTOMATIC);  //Pitch PID
00116         pidZ.SetOutputLimits(-255,255);
00117         zPoint = 0;
00118         pidd.SetMode(AUTOMATIC);  //Pitch PID
00119         pidd.SetOutputLimits(-255,255);
00120         wait(0.5);
00121         dPoint = depth;
00122 
00123         m0 = 0.5;
00124         m1 = 0.5;
00125         m2 = 0.5;
00126         m3 = 0.5;
00127         m4 = 0.5;
00128         m5 = 0.5;
00129         m6 = 0.5;
00130         m7 = 0.5;
00131         
00132         motorState = 1;
00133 
00134         Start_IMU();
00135         pc.printf("Seagoat Initialized \n\r");
00136     }
00137 
00138     void SetYawPID(double Kp, double Ki, double Kd) {
00139         pidy.SetTunings(Kp, Ki, Kd);
00140     }
00141 
00142     void SetRollPID(double Kp, double Ki, double Kd) {
00143         pidr.SetTunings(Kp, Ki, Kd);
00144     }
00145 
00146     void SetPitchPID(double Kp, double Ki, double Kd) {
00147         pidp.SetTunings(Kp, Ki, Kd);
00148     }
00149 
00150     void SetXPID(double Kp, double Ki, double Kd) {
00151         pidX.SetTunings(Kp, Ki, Kd);
00152     }
00153 
00154     void SetYPID(double Kp, double Ki, double Kd) {
00155         pidY.SetTunings(Kp, Ki, Kd);
00156     }
00157 
00158     void SetZPID(double Kp, double Ki, double Kd) {
00159         pidZ.SetTunings(Kp, Ki, Kd);
00160     }
00161     void SetdPID(double Kp, double Ki, double Kd) {
00162         pidd.SetTunings(Kp, Ki, Kd);
00163     }
00164 
00165     //This is where the magic happens
00166 //    void motorTest(){
00167 //            pwmSweep(m0);
00168 //            pwmSweep(m1);
00169 //            pwmSweep(m2);
00170 //            pwmSweep(m3);
00171 //            pwmSweep(m4);
00172 //            pwmSweep(m5);
00173 //            pwmSweep(m6);
00174 //            pwmSweep(m7);
00175 //    }
00176 //
00177 //    void pwmSweep(PwmOut motor){
00178 //        for(float i = 0; i < 80; i++){
00179 //            motor = i/255;
00180 //            wait(0.002);
00181 //        }
00182 //   //     for(float i = 80; i >= 0; i--){
00183 ////            motor = i/255;
00184 ////            wait(0.002);
00185 ////        }
00186 //    }
00187     void calibrate() {
00188         IMUUpdate(mpu6050);
00189         pc.printf("Calibrating...\n\r");
00190         //pressure_sensor.Barometer_MS5837();
00191         //depth = pressure_sensor.MS5837_Pressure();
00192     }
00193 
00194     void update() {
00195         //Update IMU Values
00196         IMUUpdate(mpu6050);
00197 
00198         //pressure_sensor.Barometer_MS5837();
00199         depth = pressure_sensor.MS5837_Pressure();
00200         //pc.printf("Pressure: %f %f\n", depth, dPoint);
00201         
00202         //Detect if the switch is turned on
00203         if(!motorState && powerPin.read() == 1){
00204             initMotors(); 
00205             motorState = 1;  
00206             pc.printf("Motors Detected");
00207             
00208              yawPoint = yaw;
00209         }
00210         else if(powerPin.read() != 1){
00211             motorState = 0;
00212             neutralizeMotors();
00213         }
00214 
00215         yawIn = yaw;
00216         rollIn = roll;
00217         pitchIn = pitch;
00218         xIn = ax;
00219         yIn = ay;
00220         zIn = az;
00221 
00222         //Calculate PID values
00223         pidy.Compute();
00224         pidr.Compute();
00225         pidp.Compute();
00226         pidX.Compute();
00227         pidY.Compute();
00228         pidZ.Compute();
00229 
00230         /*
00231                 Cameras
00232           FL ----- F ->--- FR
00233           |        |       |
00234           ˄        |       |
00235           |        |       |
00236           L        |       R
00237           |        |       |
00238           |        |       ˅
00239           |        |       |
00240           BL ---<- B ----- BR
00241 
00242           0  ----- 1 ->--- 2
00243           |        |       |
00244           ˄        |       |
00245           |        |       |
00246           7        |       3
00247           |        |       |
00248           |        |       ˅
00249           |        |       |
00250           6  ---<- 5 ----- 4
00251 
00252         */
00253 
00254         //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
00255 
00256         //Values used in Dynamic Magnitude Calculations
00257         float accxs = xOut * xOut * abs(xOut) / xOut;
00258         float accys = yOut * yOut * abs(yOut) / yOut;
00259         float acczs = zOut * zOut * abs(zOut) / zOut;
00260         float depths = dOut * dOut;
00261         float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
00262         float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
00263         float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
00264 
00265         //Values used for Influence calculations
00266         float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut) + abs(dOut)) * 255;
00267         float yy  = (abs(yOut) + abs(yawOut)) * 255;
00268         float xy  = (abs(xOut) + abs(yawOut)) * 255;
00269 
00270 //        float zpr = (zOut + pitchOut + rollOut) * 255;
00271 //        float yy  = (yOut + yawOut) * 255;
00272 //        float xy  = (xOut + yawOut) * 255;
00273 
00274 //        if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255;
00275 //        if (abs(yy)<255 && abs(yy)>=0) yy = 255;
00276 //        if (abs(xy)<255 && abs(xy)>=0) xy = 255;
00277 //        if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255;
00278 //        if (abs(yy)>-255 && abs(yy)<0) yy = -255;
00279 //        if (abs(xy)>-255 && abs(xy)<0) xy = -255;
00280 
00281         if (abs(zpr)<255) zpr = 255;
00282         if (abs(yy)<255) yy = 255;
00283         if (abs(xy)<255) xy = 255;
00284 
00285         //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr));
00286 
00287         //Spit out PID values
00288 
00289 //        m0 = abs((acczs + pitchs + rolls) / zpr);//
00290 //        m1 = abs((accys + yaws) / yy);
00291 //        m2 = abs((acczs + pitchs - rolls) / zpr);//
00292 //        m3 = abs((accxs + yaws) / xy);
00293 //        m4 = abs((acczs - pitchs - rolls) / zpr);//
00294 //        m5 = abs((accys + yaws) / yy);
00295 //        m6 = abs((acczs - pitchs + rolls) / zpr);//
00296 //        m7 = abs((accxs + yaws) / yy);
00297 
00298         m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;//
00299         m1 = (accys + yaws) / yy + 0.5;
00300         m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
00301         m3 = (accxs + yaws) / -xy + 0.5;
00302         m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
00303         m5 = (accys + yaws) / yy + 0.5;
00304         m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
00305         m7 = (accxs + yaws) / yy + 0.5;
00306 
00307         //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
00308         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);
00309         //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));
00310         // pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az);
00311         //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
00312     }
00313 
00314     void updateCommand() {
00315         char c = 0;
00316         string command;
00317         char buffer[BUFFER_SIZE] = {' '};
00318         int buffer_iter = 0;
00319         //pc.printf("Checking for command\n");
00320 
00321         // Note: you need to actually read from the serial to clear the RX interrupt
00322         if (pc.readable()) {
00323             pc.printf("Found command\n");
00324             while (pc.readable()) {
00325                 c = pc.getc();
00326                 pc.putc(c);
00327                 buffer[buffer_iter] = c;
00328                 buffer_iter++;
00329             }
00330             pc.printf("Command saved to buffer\n");
00331             command = strtok (buffer," ,\n");
00332 
00333             if (strcmp(command.c_str(), "a")) {
00334                 this->yawPoint = atof(strtok (NULL, " ,\n"));
00335                 this->pitchPoint = atof(strtok (NULL, " ,\n"));
00336                 this->rollPoint = atof(strtok (NULL, " ,\n"));
00337                 pc.printf("Received Attitude points: yawPoint: %f, pitchPoint: %f, rollPoint: %f\n", this->yawPoint, this->pitchPoint, this->rollPoint);
00338             } else if (strcmp(command.c_str(), "b")) {
00339                 this->xPoint = atof(strtok (NULL, " ,\n"));
00340                 this->yPoint = atof(strtok (NULL, " ,\n"));
00341                 this->zPoint = atof(strtok (NULL, " ,\n"));
00342                 pc.printf("Received X,Y,Z points: X: %f, Y: %f, Z: %f\n", this->xPoint, this->yPoint, this->zPoint);
00343             } else if (strcmp(command.c_str(), "c")) {
00344                 this->p_gain = atof(strtok (NULL, " ,\n"));
00345                 this->i_gain = atof(strtok (NULL, " ,\n"));
00346                 this->d_gain = atof(strtok (NULL, " ,\n"));
00347 
00348                 this->SetYawPID(this->p_gain, this->i_gain, this->d_gain);
00349                 pc.printf("Received PID values P: %f, I: %f, D: %f\n", this->p_gain, this->i_gain, this->d_gain);
00350 
00351             }
00352 
00353             memset(buffer, ' ', sizeof(buffer));
00354             buffer_iter = 0;
00355             fflush(stdout);
00356         }
00357     }
00358 
00359     void initMotors(){
00360         
00361         neutralizeMotors();
00362         m0.calibrate();
00363         m1.calibrate();
00364         m2.calibrate();
00365         m3.calibrate();
00366         m4.calibrate();
00367         m5.calibrate();
00368         m6.calibrate();
00369         m7.calibrate();
00370     }
00371     
00372     void neutralizeMotors(){
00373         m0 = 0.5;
00374         m1 = 0.5;
00375         m2 = 0.5;
00376         m3 = 0.5;
00377         m4 = 0.5;
00378         m5 = 0.5;
00379         m6 = 0.5;
00380         m7 = 0.5;
00381     }
00382 
00383 };
00384 
00385 #endif