Diff: main.cpp

Revision:

0:c425cf9d5096

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 12 16:44:04 2012 +0000
@@ -0,0 +1,134 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "Camera_LS_Y201.h"
+#include "SDFileSystem.h"
+#include "Motor.h"
+
+#define USE_SDCARD 1
+
+#if USE_SDCARD
+#define FILENAME    "/sd/IMG_%04d.jpg"
+SDFileSystem fs(p5, p6, p7, p8, "sd");
+#else
+#define FILENAME    "/local/IMG_%04d.jpg"
+LocalFileSystem fs("local");
+#endif
+
+Camera_LS_Y201 cam1(p13, p14);
+
+typedef struct work {
+    FILE *fp;
+} work_t;
+
+work_t work;
+
+void callback_func(int done, int total, uint8_t *buf, size_t siz)
+{
+    fwrite(buf, siz, 1, work.fp);
+
+    static int n = 0;
+    int tmp = done * 100 / total;
+    if (n != tmp) {
+        n = tmp;
+    }
+}
+
+int capture(Camera_LS_Y201 *cam, char *filename)
+{
+    
+    if (cam->takePicture() != 0) {
+        return -1;
+    }
+    
+    work.fp = fopen(filename, "wb");
+    if (work.fp == NULL) {
+        return -2;
+    }
+    
+    if (cam->readJpegFileContent(callback_func) != 0) {
+        fclose(work.fp);
+        return -3;
+    }
+    fclose(work.fp);
+
+    cam->stopTakingPictures();
+
+    return 0;
+}
+
+Motor M_A(p25, p27, p28);    // pwm, fwd, rev
+Motor M_B(p26, p29, p30);    // pwm, fwd, rev
+
+Servo servo_01(p23);
+Servo servo_02(p24);
+
+int main()
+{
+    int cnt = 0;
+
+    int n = 3;
+    int m = 3;
+
+    float range = 0.0009;
+    float degrees = 90.0;
+    float init_pos_01 = -90.0;
+    float init_pos_02 = 30.0;
+    
+    cam1.reset();
+    cam1.setImageSize(Camera_LS_Y201::ImageSize160x120);
+
+    servo_01.calibrate(range,degrees);
+    servo_02.calibrate(range,degrees);
+    
+    char fname[64];
+    int r;
+    
+    for(int kk=0; kk<=1; kk++) {
+        
+        for(int i=0; i<=n; i++) {
+
+            servo_01.position(init_pos_01 + ((2*degrees)*i/n));
+
+            for(int j=0; j<m; j++) {
+                
+                if(i%2==0) {
+                    servo_02.position((j*(degrees/m)) + init_pos_02);
+                } else {
+                    servo_02.position((m-j-1)*(degrees/m) + init_pos_02);
+                }
+                
+                // A wait to make certain the servos have moved into place.
+                wait(0.75);
+               
+                snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
+                r = capture(&cam1, fname);
+                
+                // Only move to the next position if the write is successful.
+                if(r == 0) {
+                    cnt++;
+                }
+                else {
+                    j--;
+                }
+                
+            }
+
+        }
+
+        if (kk==0) {
+            M_A.speed(0.5);
+            M_B.speed(-0.5);
+            wait(1.525);
+            M_A.speed(0.0);
+            M_B.speed(0.0);
+            wait(0.5);
+            M_A.speed(-0.5);
+            M_B.speed(-0.5);
+            wait(0.775);
+            M_A.speed(0.0);
+            M_B.speed(0.0);
+        }
+
+    }
+    
+}
\ No newline at end of file