Revision 2:be3b6a72f823, committed 2016-03-22

Comitter:

ms523

Date:

Tue Mar 22 15:35:01 2016 +0000

Parent:

1:0f0423207b62

Commit message:

working code

Changed in this revision

--- a/Classic_PID.lib	Tue Mar 22 10:57:41 2016 +0000
+++ b/Classic_PID.lib	Tue Mar 22 15:35:01 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Vitec-Group/code/Classic_PID/#7b42de70b65f
+http://developer.mbed.org/teams/Vitec-Group/code/Classic_PID/#972fa3842c17

--- a/PanVelocityLoop.cpp	Tue Mar 22 10:57:41 2016 +0000
+++ b/PanVelocityLoop.cpp	Tue Mar 22 15:35:01 2016 +0000
@@ -10,7 +10,7 @@
 #define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
 #define Upper OneTurn - Lower   // 262144 - 8192 = 253952
 
-extern iC_MU icMu;
+extern iC_MU icMu, icMu_Output;
 extern PwmOut Pan_Motor_PWM;
 extern DigitalOut Pan_Motor_Direction;
 extern Classic_PID PanVelocityPID;
@@ -26,38 +26,41 @@
 
 void PanVelocityLoop(void const *args)
 {
-     float Duty_Cycle = 0.0;
-     
-     // Find the icMu data
-    int icMuPosition = icMu.ReadPOSITION() >> (19 - bits);          // Read the current position from the iC-MU and bitshift to reduce noise
-    int icMuVelocity = icMuPosition - LasticMuPosition;             // Calculate change in position (i.e. Velocity)
-    
-    if(running){
-        pc.printf("\n\r%d %d",run_count,icMuPosition);
-        
-        if(run_count < 5000){
-            PanVelocityPID.setSetPoint(amplitude * sin((a*run_count*run_count)+(b*run_count)));
-            run_count++;        // Inc run_count     
+    float Duty_Cycle = 0.0;
+
+    // Find the icMu data
+    int icMuPosition = icMu.ReadPOSITION();          // Read the current position from the iC-MU
+    PanVelocityPID.setProcessValue(icMuPosition);    // Pass the Position onto the PID loop
+
+    //int icMuPosition = icMu.ReadPOSITION() >> (19 - bits);          // Read the current position from the iC-MU and bitshift to reduce noise
+    //int icMuVelocity = icMuPosition - LasticMuPosition;             // Calculate change in position (i.e. Velocity)
+
+    if(running) {
+        pc.printf("\n\r%d %d %d",run_count,icMuPosition,icMu_Output.ReadPOSITION());
+
+        if(run_count < 5000) {
+            PanVelocityPID.setSetPoint(262144 + (amplitude * sin((a*run_count*run_count)+(b*run_count))));
+            run_count++;        // Inc run_count
         } else {
             running = 0;        // Stop the test
-            PanVelocityPID.setSetPoint(0);
+            //PanVelocityPID.setSetPoint(0);
         }
     }
-    
-    // Check to see if the icMu has gone past the index point
-    if(icMuPosition < Lower & LasticMuPosition > Upper) {           // The icMu has gone over the index point in 1 direction
-        icMuVelocity += OneTurn;
-    } else if(icMuPosition > Upper & LasticMuPosition < Lower) {    // The icMu has gone over the index point in the other direction
-        icMuVelocity -= OneTurn;
-    }
-    
-    LasticMuPosition = icMuPosition;                                // Update new position from next time
-    
-    // Set the process value
-    PanVelocityPID.setProcessValue(icMuVelocity);                   // Pass the Velocity onto the PID loop
+    /*
+       // Check to see if the icMu has gone past the index point
+       if(icMuPosition < Lower & LasticMuPosition > Upper) {           // The icMu has gone over the index point in 1 direction
+           icMuVelocity += OneTurn;
+       } else if(icMuPosition > Upper & LasticMuPosition < Lower) {    // The icMu has gone over the index point in the other direction
+           icMuVelocity -= OneTurn;
+       }
 
-    
-    Duty_Cycle = PanVelocityPID.compute();
+       LasticMuPosition = icMuPosition;                                // Update new position from next time
+
+       // Set the process value
+       PanVelocityPID.setProcessValue(icMuVelocity);                   // Pass the Velocity onto the PID loop
+    */
+
+    Duty_Cycle = PanVelocityPID.compute_ff();
 
     if(Duty_Cycle < 0) {
         Pan_Motor_Direction = 1;

--- a/main.cpp	Tue Mar 22 10:57:41 2016 +0000
+++ b/main.cpp	Tue Mar 22 15:35:01 2016 +0000
@@ -5,13 +5,15 @@
 
 // iC-MU Encoder Objects
 iC_MU icMu(p5,p6,p7,p8);
+iC_MU icMu_Output(p5,p6,p7,p11);
 
 // Pan Motor
 PwmOut Pan_Motor_PWM(p21);                      // Purple wire
 DigitalOut Pan_Motor_Direction(p22);            // Yellow wire
 
 /* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
-Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001);     //Kp, ki, kd, kvelff
+//Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0);     //Kp, ki, kd, kvelff
+Classic_PID PanVelocityPID(0.000006, 0.0000001, 0.0, 0.0);     //Kp, ki, kd, kvelff
 
 // Serial objects
 Serial pc (USBTX,USBRX);
@@ -44,8 +46,10 @@
 
     // Initalise Pan PID Loop
     PanVelocityPID.setProcessLimits(1.0, -1.0);
-    PanVelocityPID.setSetPoint(0);
-    LasticMuPosition = icMu.ReadPOSITION() >> 1;
+    PanVelocityPID.setSetPoint(262144);    // Set the motor to 2^18
+    //PanVelocityPID.setSetPoint(0);
+    //LasticMuPosition = icMu.ReadPOSITION() >> 1;
+    
 
     // Increase terminal speed
     pc.baud(921600);