Omar Hagrass / CANSAT_CAMERA_servo

cansat

Dependencies:   Servo mbed-src FATFileSystem1515 SDFileSystem1515 Camera_LS_Y201_CANSAT

Fork of Camera_LS_Y201_TestProgram by Shinichiro Nakamura

main2.cpp@3:ff47827160f8, 2015-09-09 (annotated)

Committer:
Hagrass
Date:
Wed Sep 09 16:45:49 2015 +0000
Revision:
3:ff47827160f8
Parent:
2:248e3594ca61 
cansat3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hagrass 2:248e3594ca61 1 #include "mbed.h"
Hagrass 2:248e3594ca61 2 #include "Camera_LS_Y201.h"
Hagrass 2:248e3594ca61 3 #include "SDFileSystem.h"
Hagrass 2:248e3594ca61 4 #include "Servo.h"
Hagrass 2:248e3594ca61 5
Hagrass 2:248e3594ca61 6 Servo myservo(p23);
Hagrass 2:248e3594ca61 7 //#include "FATFileSystem.h"
Hagrass 2:248e3594ca61 8 Serial xbee(p28,p27);
Hagrass 2:248e3594ca61 9 Serial pc(USBTX,USBRX);
Hagrass 2:248e3594ca61 10 //FILE *p1;
Hagrass 2:248e3594ca61 11
Hagrass 2:248e3594ca61 12 #define DEBMSG pc.printf
Hagrass 2:248e3594ca61 13 #define NEWLINE() pc.printf("\r\n")
Hagrass 2:248e3594ca61 14
Hagrass 2:248e3594ca61 15 #define USE_SDCARD 0
Hagrass 2:248e3594ca61 16
Hagrass 2:248e3594ca61 17 #if USE_SDCARD
Hagrass 2:248e3594ca61 18 #define FILENAME "/sd/IMG_%04d.jpg"
Hagrass 2:248e3594ca61 19 SDFileSystem fs(p5, p6, p7, p8, "sd");
Hagrass 2:248e3594ca61 20 #else
Hagrass 2:248e3594ca61 21 #define FILENAME "/local/IMG_%04d.jpg"
Hagrass 2:248e3594ca61 22 LocalFileSystem fs("local");
Hagrass 2:248e3594ca61 23 #endif
Hagrass 2:248e3594ca61 24 Camera_LS_Y201 cam1(p17, p18);
Hagrass 2:248e3594ca61 25
Hagrass 2:248e3594ca61 26 typedef struct work {
Hagrass 2:248e3594ca61 27 FILE *fp;
Hagrass 2:248e3594ca61 28 } work_t;
Hagrass 2:248e3594ca61 29
Hagrass 2:248e3594ca61 30 work_t work;
Hagrass 2:248e3594ca61 31
Hagrass 2:248e3594ca61 32
Hagrass 2:248e3594ca61 33
Hagrass 2:248e3594ca61 34 uint8_t send[16] = {
Hagrass 2:248e3594ca61 35 0x56,
Hagrass 2:248e3594ca61 36 0x00,
Hagrass 2:248e3594ca61 37 0x32,
Hagrass 2:248e3594ca61 38 0x0C,
Hagrass 2:248e3594ca61 39 0x00,
Hagrass 2:248e3594ca61 40 0x0A,
Hagrass 2:248e3594ca61 41 0x00,
Hagrass 2:248e3594ca61 42 0x00,
Hagrass 2:248e3594ca61 43 0x00, // MH
Hagrass 2:248e3594ca61 44 0x00, // ML
Hagrass 2:248e3594ca61 45 0x00,
Hagrass 2:248e3594ca61 46 0x00,
Hagrass 2:248e3594ca61 47 0x00, // KH
Hagrass 2:248e3594ca61 48 0x00, // KL
Hagrass 2:248e3594ca61 49 0x00, // XX
Hagrass 2:248e3594ca61 50 0x00 // XX
Hagrass 2:248e3594ca61 51 };
Hagrass 2:248e3594ca61 52
Hagrass 2:248e3594ca61 53 uint16_t x = 10; // Interval time. XX XX * 0.01m[sec]
Hagrass 2:248e3594ca61 54 bool end = true;
Hagrass 2:248e3594ca61 55 uint16_t m = 0; // Staring address.
Hagrass 2:248e3594ca61 56 /*
Hagrass 2:248e3594ca61 57 * Get the data size.
Hagrass 2:248e3594ca61 58 */
Hagrass 2:248e3594ca61 59 uint8_t body[16000];
Hagrass 2:248e3594ca61 60 uint16_t k = sizeof(body);
Hagrass 2:248e3594ca61 61 int siz_done = 0;
Hagrass 2:248e3594ca61 62 int siz_total = 0;
Hagrass 2:248e3594ca61 63 int cnt = 0;
Hagrass 2:248e3594ca61 64 int z;
Hagrass 2:248e3594ca61 65 //////////////////////////////////////////
Hagrass 2:248e3594ca61 66 void callback_func(int done, int total, uint8_t *buf, size_t siz) {
Hagrass 2:248e3594ca61 67
Hagrass 2:248e3594ca61 68 // fwrite(buf, siz, 1, work.fp);
Hagrass 2:248e3594ca61 69
Hagrass 2:248e3594ca61 70 for(int i=0;i<siz;i++)
Hagrass 2:248e3594ca61 71 {
Hagrass 2:248e3594ca61 72 //xbee.printf("%x",buf[i]);
Hagrass 2:248e3594ca61 73 fprintf(work.fp,"%c",buf[i]);
Hagrass 2:248e3594ca61 74 }
Hagrass 2:248e3594ca61 75
Hagrass 2:248e3594ca61 76 static int n = 0;
Hagrass 2:248e3594ca61 77 int tmp = done * 100 / total;
Hagrass 2:248e3594ca61 78 if (n != tmp) {
Hagrass 2:248e3594ca61 79 n = tmp;
Hagrass 2:248e3594ca61 80 DEBMSG("Writ%3d%%", n);
Hagrass 2:248e3594ca61 81 NEWLINE();
Hagrass 2:248e3594ca61 82 }
Hagrass 2:248e3594ca61 83 }
Hagrass 2:248e3594ca61 84
Hagrass 2:248e3594ca61 85
Hagrass 2:248e3594ca61 86
Hagrass 2:248e3594ca61 87 ////////////////////////////////////////////
Hagrass 2:248e3594ca61 88
Hagrass 2:248e3594ca61 89
Hagrass 2:248e3594ca61 90
Hagrass 2:248e3594ca61 91
Hagrass 2:248e3594ca61 92 void readdJpegFileContent(void (*func)(int done, int total, uint8_t *buf, size_t siz)) {
Hagrass 2:248e3594ca61 93
Hagrass 2:248e3594ca61 94
Hagrass 2:248e3594ca61 95
Hagrass 2:248e3594ca61 96 if(m==0)
Hagrass 2:248e3594ca61 97 {
Hagrass 2:248e3594ca61 98 z=sizeof(body);
Hagrass 2:248e3594ca61 99 }
Hagrass 2:248e3594ca61 100
Hagrass 2:248e3594ca61 101 //if (r != NoError) {
Hagrass 2:248e3594ca61 102 // return r;
Hagrass 2:248e3594ca61 103 //}
Hagrass 2:248e3594ca61 104
Hagrass 2:248e3594ca61 105
Hagrass 2:248e3594ca61 106 send[8] = (m >> 8) & 0xff;
Hagrass 2:248e3594ca61 107 send[9] = (m >> 0) & 0xff;
Hagrass 2:248e3594ca61 108 send[12] = (k >> 8) & 0xff;
Hagrass 2:248e3594ca61 109 send[13] = (k >> 0) & 0xff;
Hagrass 2:248e3594ca61 110 send[14] = (x >> 8) & 0xff;
Hagrass 2:248e3594ca61 111 send[15] = (x >> 0) & 0xff;
Hagrass 2:248e3594ca61 112 /*
Hagrass 2:248e3594ca61 113 * Send a command.
Hagrass 2:248e3594ca61 114 */
Hagrass 2:248e3594ca61 115 cam1.sendBytes(send, sizeof(send), 200 * 1000) ;
Hagrass 2:248e3594ca61 116 // printf("sended header");
Hagrass 2:248e3594ca61 117 // return SendError;
Hagrass 2:248e3594ca61 118
Hagrass 2:248e3594ca61 119 /*
Hagrass 2:248e3594ca61 120 * Read the header of the response.
Hagrass 2:248e3594ca61 121 */
Hagrass 2:248e3594ca61 122 uint8_t header[5];
Hagrass 2:248e3594ca61 123 cam1.recvBytes(header, sizeof(header), 2 * 1000 * 1000);
Hagrass 2:248e3594ca61 124 // return RecvError;
Hagrass 2:248e3594ca61 125 //printf("recved header");
Hagrass 2:248e3594ca61 126 /*
Hagrass 2:248e3594ca61 127 * Check the response and fetch an image data.
Hagrass 2:248e3594ca61 128 */
Hagrass 2:248e3594ca61 129
Hagrass 2:248e3594ca61 130 if ((header[0] == 0x76)
Hagrass 2:248e3594ca61 131 && (header[1] == 0x00)
Hagrass 2:248e3594ca61 132 && (header[2] == 0x32)
Hagrass 2:248e3594ca61 133 && (header[3] == 0x00)
Hagrass 2:248e3594ca61 134 && (header[4] == 0x00)) {
Hagrass 2:248e3594ca61 135
Hagrass 2:248e3594ca61 136 cam1.recvBytes(body, z, 2 * 1000 * 1000); //{
Hagrass 2:248e3594ca61 137 //return RecvError;
Hagrass 2:248e3594ca61 138 // }
Hagrass 2:248e3594ca61 139 // printf("saved");
Hagrass 2:248e3594ca61 140
Hagrass 2:248e3594ca61 141 siz_done += z;
Hagrass 2:248e3594ca61 142
Hagrass 2:248e3594ca61 143 if (func != NULL) {
Hagrass 2:248e3594ca61 144 if (siz_done > siz_total) {
Hagrass 2:248e3594ca61 145 z=sizeof(body)-siz_done+siz_total+3;
Hagrass 2:248e3594ca61 146 siz_done = siz_total;
Hagrass 2:248e3594ca61 147
Hagrass 2:248e3594ca61 148
Hagrass 2:248e3594ca61 149 }
Hagrass 2:248e3594ca61 150 func(siz_done, siz_total, body, z);
Hagrass 2:248e3594ca61 151 }
Hagrass 2:248e3594ca61 152 for (int i = 1; i < z; i++) {
Hagrass 2:248e3594ca61 153 if ((body[i - 1] == 0xFF) && (body[i - 0] == 0xD9)) {
Hagrass 2:248e3594ca61 154 end = true;
Hagrass 2:248e3594ca61 155 //printf("saved%d",i);
Hagrass 2:248e3594ca61 156 }
Hagrass 2:248e3594ca61 157 }
Hagrass 2:248e3594ca61 158 } //else {
Hagrass 2:248e3594ca61 159 // return UnexpectedReply;
Hagrass 2:248e3594ca61 160 //}
Hagrass 2:248e3594ca61 161 /*
Hagrass 2:248e3594ca61 162 * Read the footer of the response.
Hagrass 2:248e3594ca61 163 */
Hagrass 2:248e3594ca61 164 uint8_t footer[5];
Hagrass 2:248e3594ca61 165 cam1.recvBytes(footer, sizeof(footer), 2 * 1000 * 1000);// {
Hagrass 2:248e3594ca61 166 // return RecvError;
Hagrass 2:248e3594ca61 167 //}
Hagrass 2:248e3594ca61 168
Hagrass 2:248e3594ca61 169 m += z;
Hagrass 2:248e3594ca61 170
Hagrass 2:248e3594ca61 171 // return NoError;
Hagrass 2:248e3594ca61 172 }
Hagrass 2:248e3594ca61 173
Hagrass 2:248e3594ca61 174 //////////////////
Hagrass 2:248e3594ca61 175 /**
Hagrass 2:248e3594ca61 176 * Capture.
Hagrass 2:248e3594ca61 177 *
Hagrass 2:248e3594ca61 178 * @param cam A pointer to a camera object.
Hagrass 2:248e3594ca61 179 * @param filename The file name.
Hagrass 2:248e3594ca61 180 *
Hagrass 2:248e3594ca61 181 * @return Return 0 if it succeed.
Hagrass 2:248e3594ca61 182 */
Hagrass 2:248e3594ca61 183 float p=0.1;//////////////servo
Hagrass 2:248e3594ca61 184 int flag=1;///////////////servo
Hagrass 2:248e3594ca61 185 int capture(Camera_LS_Y201 *cam, char *filename) {
Hagrass 2:248e3594ca61 186 /*
Hagrass 2:248e3594ca61 187 * Take a picture.
Hagrass 2:248e3594ca61 188 */
Hagrass 2:248e3594ca61 189 if(end==true)
Hagrass 2:248e3594ca61 190 {
Hagrass 2:248e3594ca61 191 ///////////////////////////////////////////servo
Hagrass 2:248e3594ca61 192 myservo = p;
Hagrass 2:248e3594ca61 193 //pc.printf("hhhhhhh%fhhhh",p);
Hagrass 2:248e3594ca61 194 if (flag==1)
Hagrass 2:248e3594ca61 195 {
Hagrass 2:248e3594ca61 196 p+=0.1;
Hagrass 2:248e3594ca61 197 }
Hagrass 2:248e3594ca61 198 else
Hagrass 2:248e3594ca61 199 {
Hagrass 2:248e3594ca61 200 p-=0.1;
Hagrass 2:248e3594ca61 201 }
Hagrass 2:248e3594ca61 202 if(p>0.9)
Hagrass 2:248e3594ca61 203
Hagrass 2:248e3594ca61 204 {
Hagrass 2:248e3594ca61 205 flag=0;
Hagrass 2:248e3594ca61 206 }
Hagrass 2:248e3594ca61 207
Hagrass 2:248e3594ca61 208 if(p<0.1)
Hagrass 2:248e3594ca61 209 {
Hagrass 2:248e3594ca61 210 flag=1;
Hagrass 2:248e3594ca61 211 }
Hagrass 2:248e3594ca61 212
Hagrass 2:248e3594ca61 213 //////////////////////////////////////////servo
Hagrass 2:248e3594ca61 214 if (cam->takePicture() != 0) {
Hagrass 2:248e3594ca61 215 return -1;
Hagrass 2:248e3594ca61 216 }
Hagrass 2:248e3594ca61 217 cam1.readJpegFileSize(&siz_total);
Hagrass 2:248e3594ca61 218 // printf("%d",siz_total);
Hagrass 2:248e3594ca61 219 siz_done = 0;
Hagrass 2:248e3594ca61 220
Hagrass 2:248e3594ca61 221
Hagrass 2:248e3594ca61 222 DEBMSG("Captured.");
Hagrass 2:248e3594ca61 223 NEWLINE();
Hagrass 2:248e3594ca61 224 work.fp = fopen(filename, "wb");
Hagrass 2:248e3594ca61 225 //p1= fopen("/sd/IMG_11111.txt", "wb");
Hagrass 2:248e3594ca61 226 if (work.fp == NULL) {
Hagrass 2:248e3594ca61 227 return -2;
Hagrass 2:248e3594ca61 228 }
Hagrass 2:248e3594ca61 229
Hagrass 2:248e3594ca61 230 /*
Hagrass 2:248e3594ca61 231 * Read the content.
Hagrass 2:248e3594ca61 232 */
Hagrass 2:248e3594ca61 233 DEBMSG("%s", filename);
Hagrass 2:248e3594ca61 234 NEWLINE();
Hagrass 2:248e3594ca61 235 end=false;
Hagrass 2:248e3594ca61 236 }
Hagrass 2:248e3594ca61 237 /*
Hagrass 2:248e3594ca61 238 * Open file.
Hagrass 2:248e3594ca61 239 */
Hagrass 2:248e3594ca61 240
Hagrass 2:248e3594ca61 241 readdJpegFileContent(callback_func);
Hagrass 2:248e3594ca61 242 // fclose(work.fp);
Hagrass 2:248e3594ca61 243 //return -3;
Hagrass 2:248e3594ca61 244
Hagrass 2:248e3594ca61 245
Hagrass 2:248e3594ca61 246
Hagrass 2:248e3594ca61 247 /*
Hagrass 2:248e3594ca61 248 * Stop taking pictures.
Hagrass 2:248e3594ca61 249 */
Hagrass 2:248e3594ca61 250 if(end== true)
Hagrass 2:248e3594ca61 251 {
Hagrass 2:248e3594ca61 252 fclose(work.fp);
Hagrass 2:248e3594ca61 253 cam->stopTakingPictures();
Hagrass 2:248e3594ca61 254
Hagrass 2:248e3594ca61 255 DEBMSG("[%04d]:OK.", cnt);
Hagrass 2:248e3594ca61 256 NEWLINE();
Hagrass 2:248e3594ca61 257 m=0;
Hagrass 2:248e3594ca61 258 cnt++;
Hagrass 2:248e3594ca61 259 //wait(1);
Hagrass 2:248e3594ca61 260 //xbee.printf("saved");
Hagrass 2:248e3594ca61 261 }
Hagrass 2:248e3594ca61 262 return 0;
Hagrass 2:248e3594ca61 263 }
Hagrass 2:248e3594ca61 264 ////////////////////////////
Hagrass 2:248e3594ca61 265 /////////////////////////
Hagrass 2:248e3594ca61 266
Hagrass 2:248e3594ca61 267 /////////////////////////////////////////////////////
Hagrass 2:248e3594ca61 268 int main(void) {
Hagrass 2:248e3594ca61 269
Hagrass 2:248e3594ca61 270 xbee.baud(115200);
Hagrass 2:248e3594ca61 271 pc.baud(115200);
Hagrass 2:248e3594ca61 272
Hagrass 2:248e3594ca61 273 DEBMSG("Camera module");
Hagrass 2:248e3594ca61 274 NEWLINE();
Hagrass 2:248e3594ca61 275 DEBMSG("Resetting...");
Hagrass 2:248e3594ca61 276 NEWLINE();
Hagrass 2:248e3594ca61 277 wait(1);
Hagrass 2:248e3594ca61 278
Hagrass 2:248e3594ca61 279 if (cam1.reset() == 0) {
Hagrass 2:248e3594ca61 280 DEBMSG("Reset OK.");
Hagrass 2:248e3594ca61 281 NEWLINE();
Hagrass 2:248e3594ca61 282 } else {
Hagrass 2:248e3594ca61 283 DEBMSG("Reset fail.");
Hagrass 2:248e3594ca61 284 NEWLINE();
Hagrass 2:248e3594ca61 285 error("Reset fail.");
Hagrass 2:248e3594ca61 286 }
Hagrass 2:248e3594ca61 287
Hagrass 2:248e3594ca61 288
Hagrass 2:248e3594ca61 289 wait(1);
Hagrass 2:248e3594ca61 290 cam1.baud();
Hagrass 2:248e3594ca61 291 int j=1;
Hagrass 2:248e3594ca61 292 int g=1;
Hagrass 2:248e3594ca61 293 while (1) {
Hagrass 2:248e3594ca61 294
Hagrass 2:248e3594ca61 295 char fname[64];
Hagrass 2:248e3594ca61 296 // xbee.printf("hello%d\n\r",g);
Hagrass 2:248e3594ca61 297
Hagrass 2:248e3594ca61 298 if((end==true)||(j==1))
Hagrass 2:248e3594ca61 299 {
Hagrass 2:248e3594ca61 300
Hagrass 2:248e3594ca61 301 snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
Hagrass 2:248e3594ca61 302 j=0;
Hagrass 2:248e3594ca61 303 }
Hagrass 2:248e3594ca61 304 capture(&cam1, fname);
Hagrass 2:248e3594ca61 305 //if (r == 0) {
Hagrass 2:248e3594ca61 306
Hagrass 2:248e3594ca61 307
Hagrass 2:248e3594ca61 308 // }
Hagrass 2:248e3594ca61 309 //} else {
Hagrass 2:248e3594ca61 310 //DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
Hagrass 2:248e3594ca61 311 // NEWLINE();
Hagrass 2:248e3594ca61 312 // error("Failure.");
Hagrass 2:248e3594ca61 313 //}
Hagrass 2:248e3594ca61 314 g++;
Hagrass 2:248e3594ca61 315 }
Hagrass 2:248e3594ca61 316 }
Hagrass 2:248e3594ca61 317
Hagrass 2:248e3594ca61 318
Hagrass 2:248e3594ca61 319
Hagrass 2:248e3594ca61 320
Hagrass 2:248e3594ca61 321