Diff: vessel.h

Revision:

5:07bbe020eb65

Parent:

4:b37fd183e46a

Child:

6:b45b74fd6a07

--- a/vessel.h	Sat Jul 09 20:41:49 2016 +0000
+++ b/vessel.h	Tue Jul 26 17:22:33 2016 +0000
@@ -55,7 +55,10 @@
     double yawPoint, yawIn, yawOut;
     double rollPoint, rollIn, rollOut;
     double pitchPoint, pitchIn, pitchOut;
-    PID pidy, pidr, pidp;
+    double xPoint, xIn, xOut;
+    double yPoint, yIn, yOut;
+    double zPoint, zIn, zOut;
+    PID pidy, pidr, pidp, pidX, pidY, pidZ;
 
 public:
     void Start_IMU() {
@@ -67,23 +70,44 @@
     }
 
     //Initialise all of the vessels starting parameters
-    Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D9), led1(LED1),
+    Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
         pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
         pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
-        pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT) {
+        pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT), 
+        pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
+        pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
+        pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT){
 
         pidy.SetMode(AUTOMATIC);  //Yaw PID
         pidy.SetOutputLimits(-255,255);
         yawPoint = 0;
-        pidr.SetMode(AUTOMATIC);  //Yaw PID
+        pidr.SetMode(AUTOMATIC);  //Roll PID
         pidr.SetOutputLimits(-255,255);
-        rollPoint = 0;
-        pidp.SetMode(AUTOMATIC);  //Yaw PID
+        pitchPoint = 0;
+        pidp.SetMode(AUTOMATIC);  //Pitch PID
         pidp.SetOutputLimits(-255,255);
         rollPoint = 0;
+        pidX.SetMode(AUTOMATIC);  //Pitch PID
+        pidX.SetOutputLimits(-255,255);
+        xPoint = 0;
+        pidY.SetMode(AUTOMATIC);  //Pitch PID
+        pidY.SetOutputLimits(-255,255);
+        yPoint = 0;
+        pidZ.SetMode(AUTOMATIC);  //Pitch PID
+        pidZ.SetOutputLimits(-255,255);
+        zPoint = 0;
+        
+        m0 = 0.5;
+        m1 = 0.5;
+        m2 = 0.5;
+        m3 = 0.5;
+        m4 = 0.5;
+        m5 = 0.5;
+        m6 = 0.5;
+        m7 = 0.5;
         
         Start_IMU();
-        pc.printf("Seagoat Ready to Go\n\r");
+        pc.printf("Seagoat Initialized \n\r");
     }
 
     void SetYawPID(double Kp, double Ki, double Kd) {
@@ -97,33 +121,147 @@
     void SetPitchPID(double Kp, double Ki, double Kd) {
         pidp.SetTunings(Kp, Ki, Kd);
     }
+
+    void SetXPID(double Kp, double Ki, double Kd) {
+        pidX.SetTunings(Kp, Ki, Kd);
+    }
+
+    void SetYPID(double Kp, double Ki, double Kd) {
+        pidY.SetTunings(Kp, Ki, Kd);
+    }
+
+    void SetZPID(double Kp, double Ki, double Kd) {
+        pidZ.SetTunings(Kp, Ki, Kd);
+    }
     
     //This is where the magic happens
+    void motorTest(){
+            pwmSweep(m0);
+            pwmSweep(m1);
+            pwmSweep(m2);
+            pwmSweep(m3);
+            pwmSweep(m4);
+            pwmSweep(m5);
+            pwmSweep(m6);
+            pwmSweep(m7);
+    }
+    
+    void pwmSweep(PwmOut motor){
+        for(float i = 0; i < 80; i++){
+            motor = i/255;
+            wait(0.002);
+        }   
+   //     for(float i = 80; i >= 0; i--){
+//            motor = i/255;
+//            wait(0.002);
+//        }   
+    }
+    void calibrate(){
+        IMUUpdate(mpu6050);
+        pc.printf("Calibrating...\n\r");
+    }
+    
     void update(){
         //Update IMU Values
         IMUUpdate(mpu6050);
         yawIn = yaw;
         rollIn = roll;
         pitchIn = pitch;
+        xIn = ax;
+        yIn = ay;
+        zIn = az;
         
         //Calculate PID values
         pidy.Compute();
-        //pidr.Compute();
-        //pidp.Compute();
+        pidr.Compute();
+        pidp.Compute();
+        pidX.Compute();
+        pidY.Compute();
+        pidZ.Compute();
+        
+        /*
+                Cameras
+          FL ----- F ->--- FR
+          |        |       |
+          ˄        |       |
+          |        |       |
+          L        |       R
+          |        |       |
+          |        |       ˅
+          |        |       |
+          BL ---<- B ----- BR
+    
+          0  ----- 1 ->--- 2
+          |        |       |
+          ˄        |       |
+          |        |       |
+          7        |       3
+          |        |       |
+          |        |       ˅
+          |        |       |
+          6  ---<- 5 ----- 4
+          
+        */
+        
+        //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
+        
+        //Values used in Dynamic Magnitude Calculations
+        float accxs = xOut * xOut * abs(xOut) / xOut;
+        float accys = yOut * yOut * abs(yOut) / yOut;
+        float acczs = zOut * zOut * abs(zOut) / zOut;
+        float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
+        float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
+        float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
+        
+        //Values used for Influence calculations
+        float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
+        float yy  = (abs(yOut) + abs(yawOut)) * 255;
+        float xy  = (abs(xOut) + abs(yawOut)) * 255; 
+        
+//        float zpr = (zOut + pitchOut + rollOut) * 255;
+//        float yy  = (yOut + yawOut) * 255;
+//        float xy  = (xOut + yawOut) * 255; 
+        
+//        if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255;
+//        if (abs(yy)<255 && abs(yy)>=0) yy = 255;
+//        if (abs(xy)<255 && abs(xy)>=0) xy = 255;        
+//        if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255;
+//        if (abs(yy)>-255 && abs(yy)<0) yy = -255;
+//        if (abs(xy)>-255 && abs(xy)<0) xy = -255;  
+     
+        if (abs(zpr)<255) zpr = 255;
+        if (abs(yy)<255) yy = 255;
+        if (abs(xy)<255) xy = 255;
+        
+        //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr));
         
         //Spit out PID values
-        double yo = abs(yawOut/255); //Dividing once to reduce overhead
+        
+//        m0 = abs((acczs + pitchs + rolls) / zpr);//
+//        m1 = abs((accys + yaws) / yy);
+//        m2 = abs((acczs + pitchs - rolls) / zpr);//
+//        m3 = abs((accxs + yaws) / xy);
+//        m4 = abs((acczs - pitchs - rolls) / zpr);//
+//        m5 = abs((accys + yaws) / yy);
+//        m6 = abs((acczs - pitchs + rolls) / zpr);//
+//        m7 = abs((accxs + yaws) / yy);
         
-        m0 = yo;
-        m1 = yo;
-        m2 = yo;
-        m3 = yo;
-        m4 = yo;
-        m5 = yo;
-        m6 = yo;
-        m7 = yo;
+        m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+        m1 = (accys + yaws) / yy + 0.5;
+        m2 = (acczs + pitchs - rolls) / zpr + 0.5;//
+        m3 = (accxs + yaws) / xy + 0.5;
+        m4 = (acczs - pitchs - rolls) / zpr + 0.5;//
+        m5 = (accys + yaws) / yy + 0.5;
+        m6 = (acczs - pitchs + rolls) / zpr + 0.5;//
+        m7 = (accxs + yaws) / yy + 0.5;
         
-        pc.printf("yaw0: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
+        //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
+        //pc.printf("%f,%f,%f,%f,%f \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / zpr + 0.5);
+        //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));
+        // pc.printf("YAW: %f, %f, %f, %f\n\r", xOut, yawOut, yawIn, yawPoint);
+        //pc.printf("YAW: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
+        //pc.printf("ACC: %f, %f, %f\n\r", ax, ay, az);
+        //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll);
     }
 };
 #endif
\ No newline at end of file