Diff: UpdateCamera.cpp

Revision:

0:7ce0bc67f60f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/UpdateCamera.cpp	Thu Apr 02 07:35:39 2015 +0000
@@ -0,0 +1,70 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#define AUTO 1
+#define MANUAL 0
+
+extern Serial Camera;
+extern bool Mode;
+
+void UpdateCamera (float ZoomDemand, float FocusDemand)
+{
+    static signed int LastZoom = 0;
+    static signed int LastFocus = 0;
+
+    // The ZoomDemand value passed to this function is a raw read of the AnalogIn object
+    ZoomDemand -= 0.5;                          // Shift the raw value to the centre-tap
+    ZoomDemand *= -14;                          // Scale to get in the -7 to +7 range
+    int Zoom = (int)ZoomDemand;                 // Cast to an int
+
+    // Do the same thing for Focus
+    FocusDemand *= 0x7000;                      // Scale to the range of 0 to 28672
+    FocusDemand += 0x0800;                      // Add a shift of 2048 to get to 2048 to 30720
+    int Focus = (int)FocusDemand & 0xFFC0;      // Knock off the last 6 bits of noise!
+
+    // See if we need to move the Camera Zoom Position
+    if(Zoom != LastZoom) {
+
+        // First calculate the new Zoom Setting
+        if (Zoom > 0) {
+            // Send the tele command
+            Zoom += 0x20;
+        } else if(Zoom < 0) {
+            // Send the wide command
+            Zoom *= -1;
+            Zoom += 0x30;
+        }
+        LastZoom = Zoom;                        // Update the last know Focus Demand for next time
+
+        // Send first 4 characters (always the same)
+        Camera.putc(0x81);
+        Camera.putc(0x01);
+        Camera.putc(0x04);
+        Camera.putc(0x07);
+        Camera.putc(Zoom);
+        Camera.putc(0xFF);
+    }
+
+    // Now see if we need to move the Camera Focus Position
+    if(Mode == MANUAL && Focus != LastFocus) {
+        Camera.putc(0x81);                      // Camera
+        Camera.putc(0x01);
+        Camera.putc(0x04);
+        Camera.putc(0x48);                      // Direct focus cmd
+        Camera.putc((Focus & 0xF000)>>12);      // p variable
+        Camera.putc((Focus & 0x0F00)>>8);       // q variable
+        Camera.putc((Focus & 0x00F0)>>4);       // r variable
+        Camera.putc(Focus & 0x000F);            // s variable
+        Camera.putc(0xFF);
+
+        LastFocus = Focus;                      // Update the last know Focus Demand for next time
+    }
+
+    // Return the current Zoom position    
+    Camera.putc(0x81);                  // Address byte from host to camera - 0x81
+    Camera.putc(0x09);                  // Zoom position inquiry - 8x 09 04 47 ff
+    Camera.putc(0x04);
+    Camera.putc(0x47);
+    Camera.putc(0xFF);                  // Terminator byte - all 1s
+    
+}
\ No newline at end of file