main.cpp

00001 #include "mbed.h"
00002 #include "Camera_LS_Y201.h"
00003 #include "SDFileSystem.h"
00004 #include "motordriver.h"
00005 #include "HMC6352.h"
00006 #define BUFFER_SIZE 10
00007 #define USE_SDCARD 0
00008  
00009 #if USE_SDCARD
00010 #define FILENAME    "/sd/IMG_%04d.jpg"
00011 SDFileSystem fs(p5, p6, p7, p8, "sd");
00012 #else
00013 #define FILENAME    "/local/IMG_%04d.jpg"
00014 LocalFileSystem fs("local");
00015 #endif
00016 Camera_LS_Y201 cam1(p13, p14);
00017 Serial rn42(p9,p10);
00018 Motor right(p21, p22, p23, 1);
00019 Motor left(p26, p25, p24, 1);
00020 DigitalOut myled(LED1);
00021 HMC6352 compass(p28, p27);
00022 char sendRcvBuf[BUFFER_SIZE];
00023 typedef struct work {
00024     FILE *fp;
00025 } work_t;
00026  
00027 work_t work;
00028 
00029 void callback_func(int done, int total, uint8_t *buf, size_t siz) {
00030     fwrite(buf, siz, 1, work.fp);
00031  
00032     static int n = 0;
00033     int tmp = done * 100 / total;
00034     if (n != tmp) {
00035         n = tmp;
00036     }
00037 }
00038  
00039 
00040 int capture(Camera_LS_Y201 *cam, char *filename) {
00041     if (cam->takePicture() != 0) {
00042         return -1;
00043     }
00044     work.fp = fopen(filename, "wb");
00045     if (work.fp == NULL) {
00046         return -2;
00047     }
00048     if (cam->readJpegFileContent(callback_func) != 0) {
00049         fclose(work.fp);
00050         return -3;
00051     }
00052     fclose(work.fp);
00053     cam->stopTakingPictures();
00054     return 0;
00055 }
00056 
00057 void clearBuffer(char* arr, int size) {
00058     int i = 0;
00059     while(i < size) {
00060         arr[i++] = 0;
00061     }
00062 }
00063 
00064 float nextFloat() {
00065     char curr = rn42.getc();
00066     int i = 0;
00067     while(curr != '#') {
00068         sendRcvBuf[i++] = curr;
00069         while(!rn42.readable());
00070         curr = rn42.getc();
00071     }
00072     float ret = atof(sendRcvBuf);
00073     clearBuffer(sendRcvBuf, BUFFER_SIZE);
00074     return ret;
00075 }
00076 
00077 void sendFloat(float reading) {
00078     char * out = (char*)malloc(sizeof(float));
00079     sprintf(out, "%.2f", reading);
00080     int i = 0;
00081     while(i < strlen(out)) {
00082         rn42.putc(out[i++]);
00083     }
00084     rn42.putc('#');
00085     free(out);   
00086 }
00087 
00088 void sendInt(int reading) {
00089     char * out = (char*)malloc(sizeof(int));
00090     sprintf(out, "%d", reading);
00091     int i = 0;
00092     while(i < strlen(out)) {
00093         rn42.putc(out[i++]);
00094     }
00095     rn42.putc('#');
00096     free(out);   
00097 }
00098 const int SPEED_THRESH = 2.5;
00099 const int TURN_THRESH = 2.5;
00100 const int MAX_ANGLE_SPEED = 8;
00101 const int MAX_ANGLE_TURN = 8;
00102 void updateMotor(float speed, float turn) {
00103   //printf("speed: %.2f\n", speed);
00104   //printf("turn: %.2f\n", turn);
00105     float leftSpeed = 0.0;
00106     float rightSpeed = 0.0;
00107     
00108     if(speed < SPEED_THRESH && speed > -1*SPEED_THRESH) { //STAY STATIONARY
00109         if(turn > TURN_THRESH) { //SPIN LEFT
00110             leftSpeed = -1*(turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
00111             rightSpeed = (turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
00112         } else if(turn < -1 * TURN_THRESH) { //SPIN RIGHT
00113             leftSpeed = (turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
00114             rightSpeed = -1*(turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
00115         } else {
00116             left.stop(1);
00117             right.stop(1);
00118             leftSpeed = 0;
00119             rightSpeed = 0;
00120         }
00121     } else {
00122         if(speed > 0) { //GO FORWARD
00123             leftSpeed = (speed - SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
00124             rightSpeed = (speed - SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
00125         } else { //GO BACKWARDS
00126             leftSpeed = (speed + SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
00127             rightSpeed = (speed + SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
00128         }
00129         if(turn > TURN_THRESH) { //TURN LEFT
00130             leftSpeed = leftSpeed*.75;
00131             rightSpeed = rightSpeed*1.25;
00132         } else if (turn < -1*TURN_THRESH) { //TURN RIGHT
00133             leftSpeed = leftSpeed*1.25;
00134             rightSpeed = rightSpeed*.75;
00135         }
00136     }
00137     leftSpeed = leftSpeed > 1.0 ? 1.0 : leftSpeed;
00138     leftSpeed = leftSpeed < -1.0 ? -1.0 : leftSpeed;
00139     rightSpeed = rightSpeed > 1.0 ? 1.0 : rightSpeed;
00140     rightSpeed = rightSpeed < -1.0 ? -1.0 : rightSpeed;
00141 //    sendFloat(leftSpeed);
00142 //    sendFloat(rightSpeed);
00143     left.speed(leftSpeed);
00144     right.speed(rightSpeed);
00145 }
00146 
00147 void brakeMotor() {
00148     left.stop(1);
00149     right.stop(1);
00150 }
00151 int main(void) {
00152     rn42.baud(115200);
00153     rn42.putc('S');
00154     rn42.putc('#');
00155 //    char sendRcvBuf[BUFFER_SIZE];
00156     int cnt = 0;
00157     
00158   //printf("Camera module\n");
00159   //printf("Resetting...\n");
00160     wait(1);
00161     if (cam1.reset() == 0) {
00162       //printf("Reset OK.\n");
00163     } else {
00164       //printf("Reset fail.\n");
00165       //error("Reset fail.");
00166     }
00167     wait(1);
00168     rn42.putc('R');
00169     rn42.putc('#');
00170     float speed = 0.0;
00171     float turn = 0.0;
00172     while(1) {
00173         if(rn42.readable()) {
00174             char byte = rn42.getc();
00175           //printf("Received: %c \n",byte);
00176             switch(byte) {
00177                 case 'M': //Move command received; numbers to follow
00178                     while(!rn42.readable());
00179                     speed = nextFloat();
00180                     while(!rn42.readable());
00181                     turn = nextFloat(); 
00182                     updateMotor(speed, turn);
00183                     rn42.putc('D'); //precursor for Direction
00184                     sendFloat(compass.sample() / 10.0); //send compass reading             
00185                     break;
00186                 case 'C':
00187                     brakeMotor();
00188                     char fname[64];
00189                     snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
00190                     int r = capture(&cam1, fname);
00191                     if (r == 0) {
00192                       //printf("[%04d]:OK.\n", cnt);
00193                     } else {
00194                       //printf("[%04d]:NG. (code=%d)\n", cnt, r);
00195                       //error("Failure.");
00196                     }
00197                     updateMotor(speed, turn);
00198                     
00199                     //char img[1024];
00200 //                    FILE *f = NULL;
00201 //                    f = fopen(fname, "r");
00202 //                    if(f==NULL) {
00203 //                     //printf("Problem opening file for reading\r\n");
00204 //                    }
00205 //                  //printf("about to send image");
00206 //                    // read the file and dump it as hex
00207 //                    size_t br;
00208 //                    int n = 0;
00209 //                    bool flag = false;
00210 //                    fseek(f, 0, SEEK_END); // seek to end of file
00211 //                    int size = ftell(f); // get current file pointer
00212 //                    fseek(f, 0, SEEK_SET); // seek back to beginning of file
00213                     rn42.putc('P');
00214                     sendInt(0);
00215 //                  //printf("size: %d\n", size);
00216 //                    while((br=fread(img,1,1024,f))!=0) {
00217 //                      //printf("Read %d bytes\r\n",br);
00218 //                        for(size_t i=0; i<br; i++) {
00219 //                            if(n==0) printf("start: %x %x\n",img[i],img[i+1]);
00220 //                            rn42.putc(img[i]);
00221 //                            if(i > 0 && img[i-1] == 0xFF && img[i] == 0xD9) {
00222 //                            //printf("spot of last byte: %d\n",n);
00223 //                              flag = true;
00224 //                            }
00225 //                            if(i < 1024 && img[i] == 0xFF && img[i+1] == 0xD8) {
00226 //                            //printf("spot of first byte: %d\n",n);
00227 //                            }
00228 //                            n++;
00229 //                            if(flag) {
00230 //                              //printf("byte after: %x\n",img[i]);
00231 //                            }
00232 //                        }
00233 //                    }
00234 //                    
00235 //                    // determine whether we reached EOF or not
00236 //                    if(!feof(f)&&ferror(f)) {
00237 //                     //printf("Did not complete reading file, file error\r\n");
00238 //                    } else {
00239 //                     //printf("File read successfully\r\n");
00240 //                    }   
00241 //                    fclose(f);
00242                     cnt++;
00243                     break;
00244                     
00245             }
00246         }
00247     }
00248 }