File content as of revision 0:c425cf9d5096:

#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);
        }

    }
    
}