Diff: PanVelocityLoop.cpp

Revision:

2:be3b6a72f823

Parent:

1:0f0423207b62

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