Diff: PanVelocityLoop.cpp

Revision:

1:0f0423207b62

Child:

2:be3b6a72f823

diff -r 3a132f85c1a8 -r 0f0423207b62 PanVelocityLoop.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PanVelocityLoop.cpp	Tue Mar 22 10:57:41 2016 +0000
@@ -0,0 +1,69 @@
+#include "mbed.h"
+#include "iC_MU.h"
+#include "rtos.h"
+#include "Classic_PID.h"
+
+// Define limits for zero crossing
+// These values should allow operation upto 3750 RPM
+#define bits 18                 // The number of bits we want to use
+#define OneTurn (1<<bits)       // 262144 counts per rev
+#define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
+#define Upper OneTurn - Lower   // 262144 - 8192 = 253952
+
+extern iC_MU icMu;
+extern PwmOut Pan_Motor_PWM;
+extern DigitalOut Pan_Motor_Direction;
+extern Classic_PID PanVelocityPID;
+extern Serial pc;
+
+extern bool running;
+extern float amplitude;
+extern float a;
+extern float b;
+
+int LasticMuPosition = 0;
+int run_count = 0;
+
+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     
+        } else {
+            running = 0;        // Stop the test
+            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
+
+    
+    Duty_Cycle = PanVelocityPID.compute();
+
+    if(Duty_Cycle < 0) {
+        Pan_Motor_Direction = 1;
+        Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
+    } else {
+        Pan_Motor_Direction = 0;
+        Pan_Motor_PWM = 1 - Duty_Cycle;
+    }
+}
\ No newline at end of file