File content as of revision 2:dc684c402296:

// Control Test Harness
// AC 19/05/2015.  Based on MS NewMotorVelLoop


#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(2, 0.000000, 0.0, 1);  //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;
bool scoping = false;

extern int LastPanPosition;
extern int LastTiltPosition;
float Pan_JoyStickDem = 0.8;
float Tilt_JoyStickDem = 0.8;
float Demand_Count_Rate = 0.0;
float Actual_Motor_Speed = 0.0;  //counts/ms
float Velocity_Error = 0.0;   // count/ms
float P_Error = 0.0;  // degrees
extern float Tilt_motor_max_count_rate; //encoder counts / ms

extern float T_Position; // True Tilt Position (Degrees)
extern float T_sf;  // counts per degree
extern int DoMove;
extern float s_profile;
extern float P_vel;
extern float real_time;
extern float T_Joy;
float Joy_DeadBand = 55;
float Joy_Zoom = 5;  // valid numbers from 1 - 9
float Time = 0.0;
extern float P;

void PanVelocityLoop(void const *args);
void TiltVelocityLoop(void const *args);
void Profile(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(Tilt_motor_max_count_rate, (Tilt_motor_max_count_rate*-1));
    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

    pc.baud(921600);
    
    T_Position = 360 - (TiltPos.ReadPOSITION()/T_sf);    // Prime the system on startup, this is not used once running.
    P = T_Position;                                      // Priming
    pc.printf("\n\r Startup: T_Position = %f, P = %f, \n\r", T_Position, P);
    
    while(1) {
        // Check to see if a key has been pressed
        if(pc.readable()) {
            ServiceKeyboard();
        }
        //T_Position = TiltPos.ReadPOSITION()/T_sf;
        Demand_Count_Rate = TiltVelocityPID.getSetPoint();

        if(1==0) {
            Velocity_Error = Demand_Count_Rate - Actual_Motor_Speed;
            pc.printf("\n\r Demand Ms = %f, V Error = %f, Pos = %f, Demand P = %f",Demand_Count_Rate, Velocity_Error, T_Position, P);
        }
        
         if(scoping) {
            P_Error = T_Position - P;
            pc.printf("\n\r %f, %f, %f, %f, %f", Time, Tilt_JoyStickDem, P_Error, T_Position, P);
            Time = Time + 0.05;
        }

       // if(DoMove ==1) {
       //     pc.printf("\n\r %f,   %f,   %f,     %f",s_profile, P_vel, T_Position, real_time);
       // }



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


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

        Pan_JoyStickDem *= 10000;                                   // Apply scalefactor
        Tilt_JoyStickDem *= 10000;                                  // Apply scalefactor
        
       if ((Tilt_JoyStickDem * Tilt_JoyStickDem) < (Joy_DeadBand * Joy_DeadBand)){
            Tilt_JoyStickDem = 0;                                   //Apply Deadband
         }
         
        Tilt_JoyStickDem = Tilt_JoyStickDem / Joy_Zoom;
        
        if(joystick) {
            PanVelocityPID.setSetPoint((int)Pan_JoyStickDem);           // Read the joystick and apply the gain
            TiltVelocityPID.setSetPoint((int)Tilt_JoyStickDem);         // Read the joystick and apply the gain
        } else {

            T_Joy = -Tilt_JoyStickDem / 100;

        }

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