Lowe Dantzler / Mbed 2 deprecated RoboPanorama

Description.

Dependencies:   Camera_LS_Y201 FatFileSystemSD Motor SDFileSystem Servo mbed

main.cpp@0:c425cf9d5096, 2012-10-12 (annotated)

Committer:
lndv3
Date:
Fri Oct 12 16:44:04 2012 +0000
Revision:
0:c425cf9d5096
Commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lndv3 0:c425cf9d5096 1 #include "mbed.h"
lndv3 0:c425cf9d5096 2 #include "Servo.h"
lndv3 0:c425cf9d5096 3 #include "Camera_LS_Y201.h"
lndv3 0:c425cf9d5096 4 #include "SDFileSystem.h"
lndv3 0:c425cf9d5096 5 #include "Motor.h"
lndv3 0:c425cf9d5096 6
lndv3 0:c425cf9d5096 7 #define USE_SDCARD 1
lndv3 0:c425cf9d5096 8
lndv3 0:c425cf9d5096 9 #if USE_SDCARD
lndv3 0:c425cf9d5096 10 #define FILENAME "/sd/IMG_%04d.jpg"
lndv3 0:c425cf9d5096 11 SDFileSystem fs(p5, p6, p7, p8, "sd");
lndv3 0:c425cf9d5096 12 #else
lndv3 0:c425cf9d5096 13 #define FILENAME "/local/IMG_%04d.jpg"
lndv3 0:c425cf9d5096 14 LocalFileSystem fs("local");
lndv3 0:c425cf9d5096 15 #endif
lndv3 0:c425cf9d5096 16
lndv3 0:c425cf9d5096 17 Camera_LS_Y201 cam1(p13, p14);
lndv3 0:c425cf9d5096 18
lndv3 0:c425cf9d5096 19 typedef struct work {
lndv3 0:c425cf9d5096 20 FILE *fp;
lndv3 0:c425cf9d5096 21 } work_t;
lndv3 0:c425cf9d5096 22
lndv3 0:c425cf9d5096 23 work_t work;
lndv3 0:c425cf9d5096 24
lndv3 0:c425cf9d5096 25 void callback_func(int done, int total, uint8_t *buf, size_t siz)
lndv3 0:c425cf9d5096 26 {
lndv3 0:c425cf9d5096 27 fwrite(buf, siz, 1, work.fp);
lndv3 0:c425cf9d5096 28
lndv3 0:c425cf9d5096 29 static int n = 0;
lndv3 0:c425cf9d5096 30 int tmp = done * 100 / total;
lndv3 0:c425cf9d5096 31 if (n != tmp) {
lndv3 0:c425cf9d5096 32 n = tmp;
lndv3 0:c425cf9d5096 33 }
lndv3 0:c425cf9d5096 34 }
lndv3 0:c425cf9d5096 35
lndv3 0:c425cf9d5096 36 int capture(Camera_LS_Y201 *cam, char *filename)
lndv3 0:c425cf9d5096 37 {
lndv3 0:c425cf9d5096 38
lndv3 0:c425cf9d5096 39 if (cam->takePicture() != 0) {
lndv3 0:c425cf9d5096 40 return -1;
lndv3 0:c425cf9d5096 41 }
lndv3 0:c425cf9d5096 42
lndv3 0:c425cf9d5096 43 work.fp = fopen(filename, "wb");
lndv3 0:c425cf9d5096 44 if (work.fp == NULL) {
lndv3 0:c425cf9d5096 45 return -2;
lndv3 0:c425cf9d5096 46 }
lndv3 0:c425cf9d5096 47
lndv3 0:c425cf9d5096 48 if (cam->readJpegFileContent(callback_func) != 0) {
lndv3 0:c425cf9d5096 49 fclose(work.fp);
lndv3 0:c425cf9d5096 50 return -3;
lndv3 0:c425cf9d5096 51 }
lndv3 0:c425cf9d5096 52 fclose(work.fp);
lndv3 0:c425cf9d5096 53
lndv3 0:c425cf9d5096 54 cam->stopTakingPictures();
lndv3 0:c425cf9d5096 55
lndv3 0:c425cf9d5096 56 return 0;
lndv3 0:c425cf9d5096 57 }
lndv3 0:c425cf9d5096 58
lndv3 0:c425cf9d5096 59 Motor M_A(p25, p27, p28); // pwm, fwd, rev
lndv3 0:c425cf9d5096 60 Motor M_B(p26, p29, p30); // pwm, fwd, rev
lndv3 0:c425cf9d5096 61
lndv3 0:c425cf9d5096 62 Servo servo_01(p23);
lndv3 0:c425cf9d5096 63 Servo servo_02(p24);
lndv3 0:c425cf9d5096 64
lndv3 0:c425cf9d5096 65 int main()
lndv3 0:c425cf9d5096 66 {
lndv3 0:c425cf9d5096 67 int cnt = 0;
lndv3 0:c425cf9d5096 68
lndv3 0:c425cf9d5096 69 int n = 3;
lndv3 0:c425cf9d5096 70 int m = 3;
lndv3 0:c425cf9d5096 71
lndv3 0:c425cf9d5096 72 float range = 0.0009;
lndv3 0:c425cf9d5096 73 float degrees = 90.0;
lndv3 0:c425cf9d5096 74 float init_pos_01 = -90.0;
lndv3 0:c425cf9d5096 75 float init_pos_02 = 30.0;
lndv3 0:c425cf9d5096 76
lndv3 0:c425cf9d5096 77 cam1.reset();
lndv3 0:c425cf9d5096 78 cam1.setImageSize(Camera_LS_Y201::ImageSize160x120);
lndv3 0:c425cf9d5096 79
lndv3 0:c425cf9d5096 80 servo_01.calibrate(range,degrees);
lndv3 0:c425cf9d5096 81 servo_02.calibrate(range,degrees);
lndv3 0:c425cf9d5096 82
lndv3 0:c425cf9d5096 83 char fname[64];
lndv3 0:c425cf9d5096 84 int r;
lndv3 0:c425cf9d5096 85
lndv3 0:c425cf9d5096 86 for(int kk=0; kk<=1; kk++) {
lndv3 0:c425cf9d5096 87
lndv3 0:c425cf9d5096 88 for(int i=0; i<=n; i++) {
lndv3 0:c425cf9d5096 89
lndv3 0:c425cf9d5096 90 servo_01.position(init_pos_01 + ((2*degrees)*i/n));
lndv3 0:c425cf9d5096 91
lndv3 0:c425cf9d5096 92 for(int j=0; j<m; j++) {
lndv3 0:c425cf9d5096 93
lndv3 0:c425cf9d5096 94 if(i%2==0) {
lndv3 0:c425cf9d5096 95 servo_02.position((j*(degrees/m)) + init_pos_02);
lndv3 0:c425cf9d5096 96 } else {
lndv3 0:c425cf9d5096 97 servo_02.position((m-j-1)*(degrees/m) + init_pos_02);
lndv3 0:c425cf9d5096 98 }
lndv3 0:c425cf9d5096 99
lndv3 0:c425cf9d5096 100 // A wait to make certain the servos have moved into place.
lndv3 0:c425cf9d5096 101 wait(0.75);
lndv3 0:c425cf9d5096 102
lndv3 0:c425cf9d5096 103 snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
lndv3 0:c425cf9d5096 104 r = capture(&cam1, fname);
lndv3 0:c425cf9d5096 105
lndv3 0:c425cf9d5096 106 // Only move to the next position if the write is successful.
lndv3 0:c425cf9d5096 107 if(r == 0) {
lndv3 0:c425cf9d5096 108 cnt++;
lndv3 0:c425cf9d5096 109 }
lndv3 0:c425cf9d5096 110 else {
lndv3 0:c425cf9d5096 111 j--;
lndv3 0:c425cf9d5096 112 }
lndv3 0:c425cf9d5096 113
lndv3 0:c425cf9d5096 114 }
lndv3 0:c425cf9d5096 115
lndv3 0:c425cf9d5096 116 }
lndv3 0:c425cf9d5096 117
lndv3 0:c425cf9d5096 118 if (kk==0) {
lndv3 0:c425cf9d5096 119 M_A.speed(0.5);
lndv3 0:c425cf9d5096 120 M_B.speed(-0.5);
lndv3 0:c425cf9d5096 121 wait(1.525);
lndv3 0:c425cf9d5096 122 M_A.speed(0.0);
lndv3 0:c425cf9d5096 123 M_B.speed(0.0);
lndv3 0:c425cf9d5096 124 wait(0.5);
lndv3 0:c425cf9d5096 125 M_A.speed(-0.5);
lndv3 0:c425cf9d5096 126 M_B.speed(-0.5);
lndv3 0:c425cf9d5096 127 wait(0.775);
lndv3 0:c425cf9d5096 128 M_A.speed(0.0);
lndv3 0:c425cf9d5096 129 M_B.speed(0.0);
lndv3 0:c425cf9d5096 130 }
lndv3 0:c425cf9d5096 131
lndv3 0:c425cf9d5096 132 }
lndv3 0:c425cf9d5096 133
lndv3 0:c425cf9d5096 134 }