File content as of revision 0:7ce0bc67f60f:

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