Diff: PanVelocityLoop.cpp

Revision:

0:7ce0bc67f60f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PanVelocityLoop.cpp	Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,45 @@
+#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 pan_ic_mu;
+extern PwmOut Pan_Motor_PWM;
+extern DigitalOut Pan_Motor_Direction;
+extern Classic_PID PanVelocityPID;
+
+int LastPanPosition = 0;
+
+void PanVelocityLoop(void const *args)
+{
+    int Position = pan_ic_mu.ReadPOSITION() >> (19 - bits); // Read the current position from the iC-MU and bitshift to reduce noise
+    int Velocity = Position - LastPanPosition;              // Calculate change in position (i.e. Velocity)
+    float Duty_Cycle = 0.0;
+
+    // Check to see if we have gone past the index point
+    if(Position < Lower & LastPanPosition > Upper) {        // We have gone over the index point in 1 direction
+        Velocity += OneTurn;
+    } else if(Position > Upper & LastPanPosition < Lower) { // We have gone over the index point in the other direction
+        Velocity -= OneTurn;
+    }  
+    LastPanPosition = Position;                             // Update new position from next time
+
+    PanVelocityPID.setProcessValue(Velocity);               // Pass the Velocity onto the PID loop
+    //Duty_Cycle = VelocityPID.compute_ff();                  // Adjust the PWM output with no feed forward
+    Duty_Cycle = PanVelocityPID.compute();
+
+    if(Duty_Cycle < 0) {
+        Pan_Motor_Direction = 0;
+        Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
+    } else {
+        Pan_Motor_Direction = 1;
+        Pan_Motor_PWM = 1 - Duty_Cycle;
+    }
+}
\ No newline at end of file