File content as of revision 4:4dafa4113982:

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