File content as of revision 1:1ad84260ff59:

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

#ifdef TARGET_LPC4088
// 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
#endif

#ifdef TARGET_LPC1768
// 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
#endif

// 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);
#ifdef TARGET_LPC4088
DigitalOut led4(LED1),led2(LED2), led3(LED3), led1(LED4);
#endif
#ifdef TARGET_LPC1768
DigitalOut led1(LED1),led2(LED2), led3(LED3), led4(LED4);
#endif


#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 = 10248;        // Strat off at max Zoom position to avoid jerks on startup
bool AutofocusFlag = true;
bool ManualfocusFlag = false;
bool Mode = AUTO;
bool joystick = true;

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

void PanVelocityLoop(void const *args);
void TiltVelocityLoop(void const *args);
int ServiceKeyboard();
void UpdateCamera(float, float);

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


// IRQ for Rx camera interrupt
void UART3_rxInterrupt(void)
{
    // Create an array to read the whole message
    static int index = 0;
    static char msg[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

    NVIC_DisableIRQ(UART3_IRQn);            // Disable the interrupt
    uint32_t IRR3 = LPC_UART3->IIR;
    msg[index] = LPC_UART3->RBR;            // Read the byte and put it into the next available element of the array

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

    led3 = !led3;                           // Flash the LED
    NVIC_EnableIRQ(UART3_IRQn);             // Enable the interrupt again
}

int main()
{
    // Increase the Camera baud rate
    Camera.baud(38400);

    // Set up the Camera Rx Interrupt
    Camera.attach(&UART3_rxInterrupt,Serial::RxIrq);

    // 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
    
    int tiltPosition;
    
    while(1) {
        // Check to see if a key has been pressed
        if(pc.readable()) {
            ServiceKeyboard();
        }

        Thread::wait(50);
        // Update the Zoom and Focus Demands
        UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
        
        //pc.printf("\n\r %d ",tiltPosition);
        Thread::wait(50);

        if(joystick) {
            // Apply Offset
            Pan_JoyStickDem = Pan_Joystick.read() - 0.5;
            Tilt_JoyStickDem = Tilt_Joystick.read() - 0.5;
            // Square the demand for pan joystick profile & check if neg
            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 & check if neg
            tiltPosition = TiltPos.ReadPOSITION();
            if(Tilt_JoyStickDem < 0) {
                // Check the tilt angle to see if it is within softlimits
                if(tiltPosition < 170000) {
                    Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem;
                } else {
                    Tilt_JoyStickDem = 0.0;
                }
            } else {
                // Check the tilt angle to see if it is within softlimits
                if(tiltPosition > 25000) {
                    Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem * -1;
                } else {
                    Tilt_JoyStickDem = 0.0;
                }
            }
            // Apply scalefactor
            Pan_JoyStickDem *= 20000;                                   // Apply scalefactor
            Tilt_JoyStickDem *= 20000;                                  // Apply scalefactor
            // Apply Zoom %
            Pan_JoyStickDem /= (ZoomPos >> 9) + 1;                      // Catch divide by zeros
            Tilt_JoyStickDem /= (ZoomPos >> 9) + 1;                     // Catch divide by zeros

            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
            led1 = 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
            led1 = 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(Demand) {
          pc.printf("\n\r %0.3f ",PanVelocityPID.getSetPoint());
      }
  }
}*/