File content as of revision 2:be3b6a72f823:

#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, icMu_Output;
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();          // Read the current position from the iC-MU
    PanVelocityPID.setProcessValue(icMuPosition);    // Pass the Position onto the PID loop

    //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 %d",run_count,icMuPosition,icMu_Output.ReadPOSITION());

        if(run_count < 5000) {
            PanVelocityPID.setSetPoint(262144 + (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_ff();

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