Revision 4:b37fd183e46a, committed 2016-07-09

Comitter:

gelmes

Date:

Sat Jul 09 20:41:49 2016 +0000

Parent:

3:5ffe7e9c0bb3

Child:

5:07bbe020eb65

Commit message:

Implementing Update function that controls motors

Changed in this revision

--- a/PID.cpp	Mon Jul 04 18:56:23 2016 +0000
+++ b/PID.cpp	Sat Jul 09 20:41:49 2016 +0000
@@ -28,7 +28,7 @@
     PID::SetControllerDirection(ControllerDirection);
     PID::SetTunings(Kp, Ki, Kd);
 
-    lastTime = t.read_us()-SampleTime;				
+    lastTime = t.read_us()-SampleTime;			
 }
  
  
@@ -38,6 +38,9 @@
  *   pid Output needs to be computed.  returns true when the output is computed,
  *   false when nothing has been done.
  **********************************************************************************/ 
+ 
+Serial pt(USBTX, USBRX); // tx, rx
+
 bool PID::Compute()
 {
    if(!inAuto) return false;
@@ -49,7 +52,10 @@
    {
       /*Compute all the working error variables*/
 	  double input = *myInput;
+	  
+      
       double error = *mySetpoint - input;
+      pt.printf("pid1: %f, %f, %f, %f\n\r", error,  error, *mySetpoint, input);
       ITerm+= (ki * error);
       if(ITerm > outMax) ITerm= outMax;
       else if(ITerm < outMin) ITerm= outMin;
@@ -58,10 +64,14 @@
       /*Compute PID Output*/
       double output = kp * error + ITerm- kd * dInput;
       
+      pt.printf("pid2: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ITerm);
+      
 	  if(output > outMax) output = outMax;
       else if(output < outMin) output = outMin;
 	  *myOutput = output;
 	  
+      pt.printf("pid3: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ki);
+	  
       /*Remember some variables for next time*/
       lastInput = input;
       lastTime = now;
@@ -93,6 +103,7 @@
       ki = (0 - ki);
       kd = (0 - kd);
    }
+   ITerm = 0.0;	
 }
   
 /* SetSampleTime(...) *********************************************************

--- a/main.cpp	Mon Jul 04 18:56:23 2016 +0000
+++ b/main.cpp	Sat Jul 09 20:41:49 2016 +0000
@@ -5,9 +5,12 @@
 int main()
 {  
     Vessel seagoat; //Starts the seagoat
-    seagoat.SetYawPID(1,1,1);    
-    seagoat.SetRollPID(1,1,1);    
-    seagoat.SetPitchPID(1,1,1);    
+    seagoat.SetYawPID(1,0,0);    
+    seagoat.SetRollPID(1,0,0);    
+    seagoat.SetPitchPID(1,0,0);    
     
-    while(1) {}
+    while(1) {
+        seagoat.update();
+        wait(0.1);
+    }
 }
\ No newline at end of file

--- 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