File content as of revision 1:0f0423207b62:

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