Diff: vessel.h

Revision:

4:b37fd183e46a

Parent:

3:5ffe7e9c0bb3

Child:

5:07bbe020eb65

--- a/vessel.h	Mon Jul 04 18:56:23 2016 +0000
+++ b/vessel.h	Sat Jul 09 20:41:49 2016 +0000
@@ -32,14 +32,23 @@
 {
 
 private:
-    Servo m0;
-    Servo m1;
-    Servo m2;
-    Servo m3;
-    Servo m4;
-    Servo m5;
-    Servo m6;
-    Servo m7;
+//    Servo m0;
+//    Servo m1;
+//    Servo m2;
+//    Servo m3;
+//    Servo m4;
+//    Servo m5;
+//    Servo m6;
+//    Servo m7;
+    
+    PwmOut m0;
+    PwmOut m1;
+    PwmOut m2;
+    PwmOut m3;
+    PwmOut m4;
+    PwmOut m5;
+    PwmOut m6;
+    PwmOut m7;
 
     PwmOut led1;
     MPU6050 mpu6050;
@@ -64,14 +73,14 @@
         pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT) {
 
         pidy.SetMode(AUTOMATIC);  //Yaw PID
-        pidy.SetOutputLimits(0,255);
-        yawPoint = 125;
+        pidy.SetOutputLimits(-255,255);
+        yawPoint = 0;
         pidr.SetMode(AUTOMATIC);  //Yaw PID
-        pidr.SetOutputLimits(0,255);
-        rollPoint = 125;
+        pidr.SetOutputLimits(-255,255);
+        rollPoint = 0;
         pidp.SetMode(AUTOMATIC);  //Yaw PID
-        pidp.SetOutputLimits(0,255);
-        rollPoint = 125;
+        pidp.SetOutputLimits(-255,255);
+        rollPoint = 0;
         
         Start_IMU();
         pc.printf("Seagoat Ready to Go\n\r");
@@ -82,16 +91,39 @@
     }
 
     void SetRollPID(double Kp, double Ki, double Kd) {
-        pidy.SetTunings(Kp, Ki, Kd);
+        pidr.SetTunings(Kp, Ki, Kd);
     }
 
     void SetPitchPID(double Kp, double Ki, double Kd) {
-        pidy.SetTunings(Kp, Ki, Kd);
+        pidp.SetTunings(Kp, Ki, Kd);
     }
     
     //This is where the magic happens
     void update(){
-            
+        //Update IMU Values
+        IMUUpdate(mpu6050);
+        yawIn = yaw;
+        rollIn = roll;
+        pitchIn = pitch;
+        
+        //Calculate PID values
+        pidy.Compute();
+        //pidr.Compute();
+        //pidp.Compute();
+        
+        //Spit out PID values
+        double yo = abs(yawOut/255); //Dividing once to reduce overhead
+        
+        m0 = yo;
+        m1 = yo;
+        m2 = yo;
+        m3 = yo;
+        m4 = yo;
+        m5 = yo;
+        m6 = yo;
+        m7 = yo;
+        
+        pc.printf("yaw0: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
     }
 };
 #endif
\ No newline at end of file