File content as of revision 0:7ce0bc67f60f:

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

#define _Kp 0.18            // Kp.Freq product
#define _Ki 0.006           // Ki.Freq product

Serial pc(USBTX,USBRX);

// iC-MU Encoder Objects
iC_MU tilt_ic_mu(p5,p6,p7,p8);
iC_MU TiltPos(p5,p6,p7,p11);
iC_MU pan_ic_mu(p5,p6,p7,p12);
iC_MU PanPos(p5,p6,p7,p13);
/*
// Tilt Motor
PwmOut Tilt_Motor_PWM(p27);                     // Purple wire
DigitalOut Tilt_Motor_Direction(p28);           // Yellow wire

// Pan Motor
PwmOut Pan_Motor_PWM(p25);                      // Purple wire
DigitalOut Pan_Motor_Direction(p26);            // Yellow wire
*/
// Tilt Motor
PwmOut Tilt_Motor_PWM(p23);                     // Purple wire
DigitalOut Tilt_Motor_Direction(p24);           // Yellow wire

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

// Joystick stuff
AnalogIn Pan_Joystick(p16);
AnalogIn Tilt_Joystick(p17);
AnalogIn Zoom_Joystick(p18);
AnalogIn Focus_Pot(p19);            // The top Pot (Pot 1)

// Camera Stuff
Serial Camera(p9,p10);
InterruptIn Switch(p14);
DigitalOut led1(LED1),led2(LED2), led3(LED3), led4(LED4);
#define AUTO 1
#define MANUAL 0

/* 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
Classic_PID TiltVelocityPID(0.00018, 0.000006, 0.0, 0.0001);    //Kp, ki, kd, kvelff

// Globals
int ZoomPos = 0;
bool AutofocusFlag = true;
bool ManualfocusFlag = false;
bool Mode = AUTO;
bool joystick = true;

extern int LastPanPosition;
extern int LastTiltPosition;
float Pan_JoyStickDem = 0.0;
float Tilt_JoyStickDem = 0.0;

void PanVelocityLoop(void const *args);
void TiltVelocityLoop(void const *args);
//int ServiceKeyboard();
void UpdateCamera(float, float);
/*
// Code for interrupt on Rx from Camera
extern "C" void MyUart3Handler(void)
{
    static int index = 0;
    static char msg[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

    // Read the byte and put it into the next available element of the array
    msg[index] = Camera.getc();

    // Check to see if that is the end of the message
    if(msg[index] == 0xFF) {
        // Check to see if the response is zoom data i.e. 7 bytes long and starts with 0x90, 0x50...
        if(index == 6 && msg[0] == 0x90 && msg[1] == 0x50) {
            ZoomPos = (msg[2] & 0x0F) << 12;
            ZoomPos += (msg[3] & 0x0F) << 8;
            ZoomPos += (msg[4] & 0x0F) << 4;
            ZoomPos += (msg[5] & 0x0F);
        }
        index = 0;
    } else {
        index++;
    }
}

void isr_Switch(void)
{
    wait_ms(1);                 // Wait 1ms and check if the switch is still pressed
    if (Switch) {
        wait_ms(1);             // Wait another 1ms and double check the switch
        if (Switch) {
            Mode = !Mode;
            if(Mode == AUTO) {
                AutofocusFlag = true;
            } else {
                ManualfocusFlag = true;
            }
        }
    }
}*/

void Anti_Lock(void const *args)
{
    Pan_Motor_Direction = !Pan_Motor_Direction.read();      // Toggle motor direction to overcome lock protection
    Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
    wait_us(10);
    Pan_Motor_Direction = !Pan_Motor_Direction.read();
    Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
}

int main()
{
    // Increase the Camera baud rate
    Camera.baud(38400);
/*    
    // Set up the Camera Rx Interrupt & enable it
    NVIC_SetVector(UART3_IRQn, (uint32_t)MyUart3Handler);
    NVIC_EnableIRQ(UART3_IRQn);
    LPC_UART3->IER = 0x01;

    // Set up the switch to toggle autofocus
    Switch.mode(PullDown);                  //Set the internal pull down resistor
    Switch.rise(&isr_Switch);               //ISR for the switch
    led4 = 1;                               // We start in autofocus mode
    led1 = 1;                               // Turn all other LEDs off
    led2 = 1;
    led3 = 0;
*/    
    // Set up the Pan motor
    Pan_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
    Pan_Motor_PWM = 1;                          // Start with motor static

    // Set up the Tilt motor
    Tilt_Motor_PWM.period_us(50);               // Set PWM to 20 kHz
    Tilt_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 Tilt Velocity loop RtosTimer thread
    RtosTimer TiltVelocityLoop_timer(TiltVelocityLoop, osTimerPeriodic);
    TiltVelocityLoop_timer.start(1);            // Run at 1kHz

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

    // Initalise Tilt PID Loop
    TiltVelocityPID.setProcessLimits(1.0, -1.0);
    TiltVelocityPID.setSetPoint(0);
    LastTiltPosition = tilt_ic_mu.ReadPOSITION() >> 1;

    // Initalise Anti-Lock RtosTimer thread
    RtosTimer Anti_Lock_timer(Anti_Lock, osTimerPeriodic);
    Anti_Lock_timer.start(1000);                // Run at 1Hz

    while(1) {
        Thread::wait(100);
        // Update the Zoom and Focus Demands
        UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
        Thread::wait(100);
        
        if(joystick) {
           Pan_JoyStickDem = Pan_Joystick.read() - 0.5;
           Tilt_JoyStickDem = Tilt_Joystick.read() - 0.5;
           Pan_JoyStickDem *= 80;
           Tilt_JoyStickDem *= -80;
           // Square the demand for pan joystick profile
           if(Pan_JoyStickDem > 0) {
               Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem;
           } else {
               Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem * -1;
           }
           // Square the demand for tilt joystick profile
           if(Tilt_JoyStickDem > 0) {
               Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem;
           } else {
               Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem * -1;
           }
           PanVelocityPID.setSetPoint((int)Pan_JoyStickDem);            // Read the joystick and apply the gain
           TiltVelocityPID.setSetPoint((int)Tilt_JoyStickDem);          // Read the joystick and apply the gain
       }
/*
        if(AutofocusFlag) {
            // Turn on the autofocus
            led4 = 1;                               // Turn the LED on to show we are in auto-focus mode
            Camera.putc(0x81);                      // Camera
            Camera.putc(0x01);                      // Turn on auto-focus cmd
            Camera.putc(0x04);
            Camera.putc(0x38);
            Camera.putc(0x02);
            Camera.putc(0xFF);                      // Terminator
            AutofocusFlag = false;
            Mode = AUTO;
        }
        if(ManualfocusFlag) {
            // Turn on Manual focus
            led4 = 0;                               // Turn the LED off to show we are in manual-focus mode
            Camera.putc(0x81);                      // Camera
            Camera.putc(0x01);                      // Turn on manual-focus cmd
            Camera.putc(0x04);
            Camera.putc(0x38);
            Camera.putc(0x03);
            Camera.putc(0xFF);                      // Terminator
            ManualfocusFlag = false;
            Mode = MANUAL;
        }*/
    }
}

 /*   
    // Increase the serial baud rate
    // pc.baud(921600);

    while(1) {
        Thread::wait(100);

       if(pc.readable()) {
           ServiceKeyboard();
       }
       if(Capture) {
           // Print to terminal the current motor encoder positions
           pc.printf("\n\r %d %d ",pan_ic_mu.ReadPOSITION(),tilt_ic_mu.ReadPOSITION());
       }
       
       if(Demand) {
           pc.printf("\n\r %0.3f ",PanVelocityPID.getSetPoint());
       }
   }
}*/