Diff: main.cpp

Revision:

0:7ce0bc67f60f

Child:

1:1ad84260ff59

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,237 @@
+#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());
+       }
+   }
+}*/