running on mbed now

Dependencies:   Classic_PID iC_MU mbed-rtos mbed

Files at this revision

API Documentation at this revision

Comitter:
ms523
Date:
Tue Mar 22 15:35:01 2016 +0000
Parent:
1:0f0423207b62
Commit message:
working code

Changed in this revision

Classic_PID.lib Show annotated file Show diff for this revision Revisions of this file
PanVelocityLoop.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 0f0423207b62 -r be3b6a72f823 Classic_PID.lib
--- 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
diff -r 0f0423207b62 -r be3b6a72f823 PanVelocityLoop.cpp
--- 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;
diff -r 0f0423207b62 -r be3b6a72f823 main.cpp
--- 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);