File content as of revision 1:0f0423207b62:

#include "mbed.h"
#include "iC_MU.h"
#include "rtos.h"
#include "Classic_PID.h"

// iC-MU Encoder Objects
iC_MU icMu(p5,p6,p7,p8);

// Pan Motor
PwmOut Pan_Motor_PWM(p21);                      // Purple wire
DigitalOut Pan_Motor_Direction(p22);            // Yellow wire

/* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001);     //Kp, ki, kd, kvelff

// Serial objects
Serial pc (USBTX,USBRX);

// Global Variables
float run_time = 5.0;
float start_Hz = 1.0, stop_Hz = 2.0;
float amplitude;
bool running = 0;
float w1;
float w2;
float a;
float b;

// External variables
extern int LasticMuPosition;

// Functions
void PanVelocityLoop(void const *args);

int main()
{
    // Set up the Pan motor
    Pan_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
    Pan_Motor_PWM = 1;                          // Start with motor static

    // Initalise Pan Velocity loop RtosTimer thread
    RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
    PanVelocityLoop_timer.start(1);             // Run at 1kHz

    // Initalise Pan PID Loop
    PanVelocityPID.setProcessLimits(1.0, -1.0);
    PanVelocityPID.setSetPoint(0);
    LasticMuPosition = icMu.ReadPOSITION() >> 1;

    // Increase terminal speed
    pc.baud(921600);

    // Prompt the user to enter the test parameters
    pc.printf("\n\r Enter Start frequency (Hz): ");
    pc.scanf("%f", &start_Hz);
    pc.printf("%f",start_Hz);

    pc.printf("\n\r Enter Stop frequency (Hz): ");
    pc.scanf("%f", &stop_Hz);
    pc.printf("%f",stop_Hz);

    pc.printf("\n\r Enter Amplitude (encoder counts): ");
    pc.scanf("%f", &amplitude);
    pc.printf("%f",amplitude);

    pc.printf("\n\n\r Press any key to start test...");
    pc.getc();

    w1 = start_Hz * 3.14159 * 2;
    w2 = stop_Hz * 3.14159 * 2;
    a = (w2 - w1) / (2 * run_time * 1000000);
    b = w1 / 1000;

    // Set the test running
    running = 1;

    while(1) {
        ;
    }

}