Robot controlled using Google Glass. Communication over RN-42 Bluetooth module.
Dependencies: Camera_LS_Y201 HMC6352 SDFileSystem mbed
Fork of Camera_LS_Y201_TestProgram_2014 by
main.cpp
- Committer:
- reinaldo422
- Date:
- 2014-12-10
- Revision:
- 1:1a1bdef49fb0
- Parent:
- 0:31be9011f67e
File content as of revision 1:1a1bdef49fb0:
#include "mbed.h" #include "Camera_LS_Y201.h" #include "SDFileSystem.h" #include "motordriver.h" #include "HMC6352.h" #define BUFFER_SIZE 10 #define USE_SDCARD 0 #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); Serial rn42(p9,p10); Motor right(p21, p22, p23, 1); Motor left(p26, p25, p24, 1); DigitalOut myled(LED1); HMC6352 compass(p28, p27); char sendRcvBuf[BUFFER_SIZE]; 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; } void clearBuffer(char* arr, int size) { int i = 0; while(i < size) { arr[i++] = 0; } } float nextFloat() { char curr = rn42.getc(); int i = 0; while(curr != '#') { sendRcvBuf[i++] = curr; while(!rn42.readable()); curr = rn42.getc(); } float ret = atof(sendRcvBuf); clearBuffer(sendRcvBuf, BUFFER_SIZE); return ret; } void sendFloat(float reading) { char * out = (char*)malloc(sizeof(float)); sprintf(out, "%.2f", reading); int i = 0; while(i < strlen(out)) { rn42.putc(out[i++]); } rn42.putc('#'); free(out); } void sendInt(int reading) { char * out = (char*)malloc(sizeof(int)); sprintf(out, "%d", reading); int i = 0; while(i < strlen(out)) { rn42.putc(out[i++]); } rn42.putc('#'); free(out); } const int SPEED_THRESH = 2.5; const int TURN_THRESH = 2.5; const int MAX_ANGLE_SPEED = 8; const int MAX_ANGLE_TURN = 8; void updateMotor(float speed, float turn) { //printf("speed: %.2f\n", speed); //printf("turn: %.2f\n", turn); float leftSpeed = 0.0; float rightSpeed = 0.0; if(speed < SPEED_THRESH && speed > -1*SPEED_THRESH) { //STAY STATIONARY if(turn > TURN_THRESH) { //SPIN LEFT leftSpeed = -1*(turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH); rightSpeed = (turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH); } else if(turn < -1 * TURN_THRESH) { //SPIN RIGHT leftSpeed = (turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH); rightSpeed = -1*(turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH); } else { left.stop(1); right.stop(1); leftSpeed = 0; rightSpeed = 0; } } else { if(speed > 0) { //GO FORWARD leftSpeed = (speed - SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH); rightSpeed = (speed - SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH); } else { //GO BACKWARDS leftSpeed = (speed + SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH); rightSpeed = (speed + SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH); } if(turn > TURN_THRESH) { //TURN LEFT leftSpeed = leftSpeed*.75; rightSpeed = rightSpeed*1.25; } else if (turn < -1*TURN_THRESH) { //TURN RIGHT leftSpeed = leftSpeed*1.25; rightSpeed = rightSpeed*.75; } } leftSpeed = leftSpeed > 1.0 ? 1.0 : leftSpeed; leftSpeed = leftSpeed < -1.0 ? -1.0 : leftSpeed; rightSpeed = rightSpeed > 1.0 ? 1.0 : rightSpeed; rightSpeed = rightSpeed < -1.0 ? -1.0 : rightSpeed; // sendFloat(leftSpeed); // sendFloat(rightSpeed); left.speed(leftSpeed); right.speed(rightSpeed); } void brakeMotor() { left.stop(1); right.stop(1); } int main(void) { rn42.baud(115200); rn42.putc('S'); rn42.putc('#'); // char sendRcvBuf[BUFFER_SIZE]; int cnt = 0; //printf("Camera module\n"); //printf("Resetting...\n"); wait(1); if (cam1.reset() == 0) { //printf("Reset OK.\n"); } else { //printf("Reset fail.\n"); //error("Reset fail."); } wait(1); rn42.putc('R'); rn42.putc('#'); float speed = 0.0; float turn = 0.0; while(1) { if(rn42.readable()) { char byte = rn42.getc(); //printf("Received: %c \n",byte); switch(byte) { case 'M': //Move command received; numbers to follow while(!rn42.readable()); speed = nextFloat(); while(!rn42.readable()); turn = nextFloat(); updateMotor(speed, turn); rn42.putc('D'); //precursor for Direction sendFloat(compass.sample() / 10.0); //send compass reading break; case 'C': brakeMotor(); char fname[64]; snprintf(fname, sizeof(fname) - 1, FILENAME, cnt); int r = capture(&cam1, fname); if (r == 0) { //printf("[%04d]:OK.\n", cnt); } else { //printf("[%04d]:NG. (code=%d)\n", cnt, r); //error("Failure."); } updateMotor(speed, turn); //char img[1024]; // FILE *f = NULL; // f = fopen(fname, "r"); // if(f==NULL) { // //printf("Problem opening file for reading\r\n"); // } // //printf("about to send image"); // // read the file and dump it as hex // size_t br; // int n = 0; // bool flag = false; // fseek(f, 0, SEEK_END); // seek to end of file // int size = ftell(f); // get current file pointer // fseek(f, 0, SEEK_SET); // seek back to beginning of file rn42.putc('P'); sendInt(0); // //printf("size: %d\n", size); // while((br=fread(img,1,1024,f))!=0) { // //printf("Read %d bytes\r\n",br); // for(size_t i=0; i<br; i++) { // if(n==0) printf("start: %x %x\n",img[i],img[i+1]); // rn42.putc(img[i]); // if(i > 0 && img[i-1] == 0xFF && img[i] == 0xD9) { // //printf("spot of last byte: %d\n",n); // flag = true; // } // if(i < 1024 && img[i] == 0xFF && img[i+1] == 0xD8) { // //printf("spot of first byte: %d\n",n); // } // n++; // if(flag) { // //printf("byte after: %x\n",img[i]); // } // } // } // // // determine whether we reached EOF or not // if(!feof(f)&&ferror(f)) { // //printf("Did not complete reading file, file error\r\n"); // } else { // //printf("File read successfully\r\n"); // } // fclose(f); cnt++; break; } } } }