Diff: main.cpp

Revision:

1:1ad84260ff59

Parent:

0:7ce0bc67f60f

Child:

2:dc684c402296

--- a/main.cpp	Thu Apr 02 07:35:39 2015 +0000
+++ b/main.cpp	Mon May 18 09:06:07 2015 +0000
@@ -13,22 +13,24 @@
 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);
@@ -39,7 +41,14 @@
 // 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
 
@@ -48,7 +57,7 @@
 Classic_PID TiltVelocityPID(0.00018, 0.000006, 0.0, 0.0001);    //Kp, ki, kd, kvelff
 
 // Globals
-int ZoomPos = 0;
+int ZoomPos = 10248;        // Strat off at max Zoom position to avoid jerks on startup
 bool AutofocusFlag = true;
 bool ManualfocusFlag = false;
 bool Mode = AUTO;
@@ -56,37 +65,13 @@
 
 extern int LastPanPosition;
 extern int LastTiltPosition;
-float Pan_JoyStickDem = 0.0;
-float Tilt_JoyStickDem = 0.0;
+float Pan_JoyStickDem = 0.5;
+float Tilt_JoyStickDem = 0.5;
 
 void PanVelocityLoop(void const *args);
 void TiltVelocityLoop(void const *args);
-//int ServiceKeyboard();
+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)
 {
@@ -102,7 +87,7 @@
             }
         }
     }
-}*/
+}
 
 void Anti_Lock(void const *args)
 {
@@ -113,15 +98,43 @@
     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 & enable it
-    NVIC_SetVector(UART3_IRQn, (uint32_t)MyUart3Handler);
-    NVIC_EnableIRQ(UART3_IRQn);
-    LPC_UART3->IER = 0x01;
+
+    // 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
@@ -130,7 +143,7 @@
     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
@@ -160,37 +173,63 @@
     // Initalise Anti-Lock RtosTimer thread
     RtosTimer Anti_Lock_timer(Anti_Lock, osTimerPeriodic);
     Anti_Lock_timer.start(1000);                // Run at 1Hz
-
+    
+    int tiltPosition;
+    
     while(1) {
-        Thread::wait(100);
+        // 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());
-        Thread::wait(100);
         
+        //pc.printf("\n\r %d ",tiltPosition);
+        Thread::wait(50);
+
         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
-       }
-/*
+            // 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
-            led4 = 1;                               // Turn the LED on to show we are in auto-focus mode
+            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);
@@ -202,7 +241,7 @@
         }
         if(ManualfocusFlag) {
             // Turn on Manual focus
-            led4 = 0;                               // Turn the LED off to show we are in manual-focus mode
+            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);
@@ -211,27 +250,23 @@
             Camera.putc(0xFF);                      // Terminator
             ManualfocusFlag = false;
             Mode = MANUAL;
-        }*/
+        }
     }
 }
 
- /*   
-    // Increase the serial baud rate
-    // pc.baud(921600);
+/*
+   // Increase the serial baud rate
+   // pc.baud(921600);
 
-    while(1) {
-        Thread::wait(100);
+   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());
-       }
-   }
+      if(pc.readable()) {
+          ServiceKeyboard();
+      }
+
+      if(Demand) {
+          pc.printf("\n\r %0.3f ",PanVelocityPID.getSetPoint());
+      }
+  }
 }*/