Diff: TiltVelocityLoop.cpp

Revision:

0:7ce0bc67f60f

Child:

2:dc684c402296

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TiltVelocityLoop.cpp	Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,44 @@
+#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 tilt_ic_mu;
+extern PwmOut Tilt_Motor_PWM;
+extern DigitalOut Tilt_Motor_Direction;
+extern Classic_PID TiltVelocityPID;
+
+int LastTiltPosition = 0;
+
+void TiltVelocityLoop(void const *args)
+{
+    int Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
+    int Velocity = Position - LastTiltPosition;             // 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 & LastTiltPosition > Upper) {       // We have gone over the index point in 1 direction
+        Velocity += OneTurn;
+    } else if(Position > Upper & LastTiltPosition < Lower) {// We have gone over the index point in the other direction
+        Velocity -= OneTurn;
+    }  
+    LastTiltPosition = Position;                            // Update new position from next time
+
+    TiltVelocityPID.setProcessValue(Velocity);              // Pass the Velocity onto the PID loop
+    Duty_Cycle = TiltVelocityPID.compute();
+
+    if(Duty_Cycle < 0) {
+        Tilt_Motor_Direction = 0;
+        Tilt_Motor_PWM = 1 - (Duty_Cycle * -1.0);
+    } else {
+        Tilt_Motor_Direction = 1;
+        Tilt_Motor_PWM = 1 - Duty_Cycle;
+    }
+}
\ No newline at end of file