UpdateCamera.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 
00004 #define AUTO 1
00005 #define MANUAL 0
00006 
00007 extern Serial Camera;
00008 extern bool Mode;
00009 
00010 void UpdateCamera (float ZoomDemand, float FocusDemand)
00011 {
00012     static signed int LastZoom = 0;
00013     static signed int LastFocus = 0;
00014 
00015     // The ZoomDemand value passed to this function is a raw read of the AnalogIn object
00016     ZoomDemand -= 0.5;                          // Shift the raw value to the centre-tap
00017     ZoomDemand *= -14;                          // Scale to get in the -7 to +7 range
00018     int Zoom = (int)ZoomDemand;                 // Cast to an int
00019 
00020     // Do the same thing for Focus
00021     FocusDemand *= 0x7000;                      // Scale to the range of 0 to 28672
00022     FocusDemand += 0x0800;                      // Add a shift of 2048 to get to 2048 to 30720
00023     int Focus = (int)FocusDemand & 0xFFC0;      // Knock off the last 6 bits of noise!
00024 
00025     // See if we need to move the Camera Zoom Position
00026     if(Zoom != LastZoom) {
00027 
00028         // First calculate the new Zoom Setting
00029         if (Zoom > 0) {
00030             // Send the tele command
00031             Zoom += 0x20;
00032         } else if(Zoom < 0) {
00033             // Send the wide command
00034             Zoom *= -1;
00035             Zoom += 0x30;
00036         }
00037         LastZoom = Zoom;                        // Update the last know Focus Demand for next time
00038 
00039         // Send first 4 characters (always the same)
00040         Camera.putc(0x81);
00041         Camera.putc(0x01);
00042         Camera.putc(0x04);
00043         Camera.putc(0x07);
00044         Camera.putc(Zoom);
00045         Camera.putc(0xFF);
00046     }
00047 
00048     // Now see if we need to move the Camera Focus Position
00049     if(Mode == MANUAL && Focus != LastFocus) {
00050         Camera.putc(0x81);                      // Camera
00051         Camera.putc(0x01);
00052         Camera.putc(0x04);
00053         Camera.putc(0x48);                      // Direct focus cmd
00054         Camera.putc((Focus & 0xF000)>>12);      // p variable
00055         Camera.putc((Focus & 0x0F00)>>8);       // q variable
00056         Camera.putc((Focus & 0x00F0)>>4);       // r variable
00057         Camera.putc(Focus & 0x000F);            // s variable
00058         Camera.putc(0xFF);
00059 
00060         LastFocus = Focus;                      // Update the last know Focus Demand for next time
00061     }
00062 
00063     // Return the current Zoom position    
00064     Camera.putc(0x81);                  // Address byte from host to camera - 0x81
00065     Camera.putc(0x09);                  // Zoom position inquiry - 8x 09 04 47 ff
00066     Camera.putc(0x04);
00067     Camera.putc(0x47);
00068     Camera.putc(0xFF);                  // Terminator byte - all 1s
00069     
00070 }