Reinaldo Cruz / Mbed 2 deprecated GlassWareRobot

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 Shinichiro Nakamura

main.cpp@1:1a1bdef49fb0, 2014-12-10 (annotated)

Committer:
reinaldo422
Date:
Wed Dec 10 03:01:47 2014 +0000
Revision:
1:1a1bdef49fb0
Parent:
0:31be9011f67e 
Driver for robot used in Google Glass project. RN-42 bluetooth module used to communicate with Google Glass.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shintamainjp 0:31be9011f67e 1 #include "mbed.h"
shintamainjp 0:31be9011f67e 2 #include "Camera_LS_Y201.h"
shintamainjp 0:31be9011f67e 3 #include "SDFileSystem.h"
reinaldo422 1:1a1bdef49fb0 4 #include "motordriver.h"
reinaldo422 1:1a1bdef49fb0 5 #include "HMC6352.h"
reinaldo422 1:1a1bdef49fb0 6 #define BUFFER_SIZE 10
shintamainjp 0:31be9011f67e 7 #define USE_SDCARD 0
shintamainjp 0:31be9011f67e 8
shintamainjp 0:31be9011f67e 9 #if USE_SDCARD
shintamainjp 0:31be9011f67e 10 #define FILENAME "/sd/IMG_%04d.jpg"
shintamainjp 0:31be9011f67e 11 SDFileSystem fs(p5, p6, p7, p8, "sd");
shintamainjp 0:31be9011f67e 12 #else
shintamainjp 0:31be9011f67e 13 #define FILENAME "/local/IMG_%04d.jpg"
shintamainjp 0:31be9011f67e 14 LocalFileSystem fs("local");
shintamainjp 0:31be9011f67e 15 #endif
shintamainjp 0:31be9011f67e 16 Camera_LS_Y201 cam1(p13, p14);
reinaldo422 1:1a1bdef49fb0 17 Serial rn42(p9,p10);
reinaldo422 1:1a1bdef49fb0 18 Motor right(p21, p22, p23, 1);
reinaldo422 1:1a1bdef49fb0 19 Motor left(p26, p25, p24, 1);
reinaldo422 1:1a1bdef49fb0 20 DigitalOut myled(LED1);
reinaldo422 1:1a1bdef49fb0 21 HMC6352 compass(p28, p27);
reinaldo422 1:1a1bdef49fb0 22 char sendRcvBuf[BUFFER_SIZE];
shintamainjp 0:31be9011f67e 23 typedef struct work {
shintamainjp 0:31be9011f67e 24 FILE *fp;
shintamainjp 0:31be9011f67e 25 } work_t;
shintamainjp 0:31be9011f67e 26
shintamainjp 0:31be9011f67e 27 work_t work;
reinaldo422 1:1a1bdef49fb0 28
shintamainjp 0:31be9011f67e 29 void callback_func(int done, int total, uint8_t *buf, size_t siz) {
shintamainjp 0:31be9011f67e 30 fwrite(buf, siz, 1, work.fp);
shintamainjp 0:31be9011f67e 31
shintamainjp 0:31be9011f67e 32 static int n = 0;
shintamainjp 0:31be9011f67e 33 int tmp = done * 100 / total;
shintamainjp 0:31be9011f67e 34 if (n != tmp) {
shintamainjp 0:31be9011f67e 35 n = tmp;
shintamainjp 0:31be9011f67e 36 }
shintamainjp 0:31be9011f67e 37 }
shintamainjp 0:31be9011f67e 38
reinaldo422 1:1a1bdef49fb0 39
shintamainjp 0:31be9011f67e 40 int capture(Camera_LS_Y201 *cam, char *filename) {
shintamainjp 0:31be9011f67e 41 if (cam->takePicture() != 0) {
shintamainjp 0:31be9011f67e 42 return -1;
shintamainjp 0:31be9011f67e 43 }
shintamainjp 0:31be9011f67e 44 work.fp = fopen(filename, "wb");
shintamainjp 0:31be9011f67e 45 if (work.fp == NULL) {
shintamainjp 0:31be9011f67e 46 return -2;
shintamainjp 0:31be9011f67e 47 }
shintamainjp 0:31be9011f67e 48 if (cam->readJpegFileContent(callback_func) != 0) {
shintamainjp 0:31be9011f67e 49 fclose(work.fp);
shintamainjp 0:31be9011f67e 50 return -3;
shintamainjp 0:31be9011f67e 51 }
shintamainjp 0:31be9011f67e 52 fclose(work.fp);
shintamainjp 0:31be9011f67e 53 cam->stopTakingPictures();
shintamainjp 0:31be9011f67e 54 return 0;
shintamainjp 0:31be9011f67e 55 }
reinaldo422 1:1a1bdef49fb0 56
reinaldo422 1:1a1bdef49fb0 57 void clearBuffer(char* arr, int size) {
reinaldo422 1:1a1bdef49fb0 58 int i = 0;
reinaldo422 1:1a1bdef49fb0 59 while(i < size) {
reinaldo422 1:1a1bdef49fb0 60 arr[i++] = 0;
reinaldo422 1:1a1bdef49fb0 61 }
reinaldo422 1:1a1bdef49fb0 62 }
reinaldo422 1:1a1bdef49fb0 63
reinaldo422 1:1a1bdef49fb0 64 float nextFloat() {
reinaldo422 1:1a1bdef49fb0 65 char curr = rn42.getc();
reinaldo422 1:1a1bdef49fb0 66 int i = 0;
reinaldo422 1:1a1bdef49fb0 67 while(curr != '#') {
reinaldo422 1:1a1bdef49fb0 68 sendRcvBuf[i++] = curr;
reinaldo422 1:1a1bdef49fb0 69 while(!rn42.readable());
reinaldo422 1:1a1bdef49fb0 70 curr = rn42.getc();
reinaldo422 1:1a1bdef49fb0 71 }
reinaldo422 1:1a1bdef49fb0 72 float ret = atof(sendRcvBuf);
reinaldo422 1:1a1bdef49fb0 73 clearBuffer(sendRcvBuf, BUFFER_SIZE);
reinaldo422 1:1a1bdef49fb0 74 return ret;
reinaldo422 1:1a1bdef49fb0 75 }
reinaldo422 1:1a1bdef49fb0 76
reinaldo422 1:1a1bdef49fb0 77 void sendFloat(float reading) {
reinaldo422 1:1a1bdef49fb0 78 char * out = (char*)malloc(sizeof(float));
reinaldo422 1:1a1bdef49fb0 79 sprintf(out, "%.2f", reading);
reinaldo422 1:1a1bdef49fb0 80 int i = 0;
reinaldo422 1:1a1bdef49fb0 81 while(i < strlen(out)) {
reinaldo422 1:1a1bdef49fb0 82 rn42.putc(out[i++]);
reinaldo422 1:1a1bdef49fb0 83 }
reinaldo422 1:1a1bdef49fb0 84 rn42.putc('#');
reinaldo422 1:1a1bdef49fb0 85 free(out);
reinaldo422 1:1a1bdef49fb0 86 }
reinaldo422 1:1a1bdef49fb0 87
reinaldo422 1:1a1bdef49fb0 88 void sendInt(int reading) {
reinaldo422 1:1a1bdef49fb0 89 char * out = (char*)malloc(sizeof(int));
reinaldo422 1:1a1bdef49fb0 90 sprintf(out, "%d", reading);
reinaldo422 1:1a1bdef49fb0 91 int i = 0;
reinaldo422 1:1a1bdef49fb0 92 while(i < strlen(out)) {
reinaldo422 1:1a1bdef49fb0 93 rn42.putc(out[i++]);
reinaldo422 1:1a1bdef49fb0 94 }
reinaldo422 1:1a1bdef49fb0 95 rn42.putc('#');
reinaldo422 1:1a1bdef49fb0 96 free(out);
reinaldo422 1:1a1bdef49fb0 97 }
reinaldo422 1:1a1bdef49fb0 98 const int SPEED_THRESH = 2.5;
reinaldo422 1:1a1bdef49fb0 99 const int TURN_THRESH = 2.5;
reinaldo422 1:1a1bdef49fb0 100 const int MAX_ANGLE_SPEED = 8;
reinaldo422 1:1a1bdef49fb0 101 const int MAX_ANGLE_TURN = 8;
reinaldo422 1:1a1bdef49fb0 102 void updateMotor(float speed, float turn) {
reinaldo422 1:1a1bdef49fb0 103 //printf("speed: %.2f\n", speed);
reinaldo422 1:1a1bdef49fb0 104 //printf("turn: %.2f\n", turn);
reinaldo422 1:1a1bdef49fb0 105 float leftSpeed = 0.0;
reinaldo422 1:1a1bdef49fb0 106 float rightSpeed = 0.0;
reinaldo422 1:1a1bdef49fb0 107
reinaldo422 1:1a1bdef49fb0 108 if(speed < SPEED_THRESH && speed > -1*SPEED_THRESH) { //STAY STATIONARY
reinaldo422 1:1a1bdef49fb0 109 if(turn > TURN_THRESH) { //SPIN LEFT
reinaldo422 1:1a1bdef49fb0 110 leftSpeed = -1*(turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
reinaldo422 1:1a1bdef49fb0 111 rightSpeed = (turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
reinaldo422 1:1a1bdef49fb0 112 } else if(turn < -1 * TURN_THRESH) { //SPIN RIGHT
reinaldo422 1:1a1bdef49fb0 113 leftSpeed = (turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
reinaldo422 1:1a1bdef49fb0 114 rightSpeed = -1*(turn - TURN_THRESH)/(MAX_ANGLE_TURN-TURN_THRESH);
reinaldo422 1:1a1bdef49fb0 115 } else {
reinaldo422 1:1a1bdef49fb0 116 left.stop(1);
reinaldo422 1:1a1bdef49fb0 117 right.stop(1);
reinaldo422 1:1a1bdef49fb0 118 leftSpeed = 0;
reinaldo422 1:1a1bdef49fb0 119 rightSpeed = 0;
reinaldo422 1:1a1bdef49fb0 120 }
reinaldo422 1:1a1bdef49fb0 121 } else {
reinaldo422 1:1a1bdef49fb0 122 if(speed > 0) { //GO FORWARD
reinaldo422 1:1a1bdef49fb0 123 leftSpeed = (speed - SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
reinaldo422 1:1a1bdef49fb0 124 rightSpeed = (speed - SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
reinaldo422 1:1a1bdef49fb0 125 } else { //GO BACKWARDS
reinaldo422 1:1a1bdef49fb0 126 leftSpeed = (speed + SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
reinaldo422 1:1a1bdef49fb0 127 rightSpeed = (speed + SPEED_THRESH)/(MAX_ANGLE_SPEED-SPEED_THRESH);
reinaldo422 1:1a1bdef49fb0 128 }
reinaldo422 1:1a1bdef49fb0 129 if(turn > TURN_THRESH) { //TURN LEFT
reinaldo422 1:1a1bdef49fb0 130 leftSpeed = leftSpeed*.75;
reinaldo422 1:1a1bdef49fb0 131 rightSpeed = rightSpeed*1.25;
reinaldo422 1:1a1bdef49fb0 132 } else if (turn < -1*TURN_THRESH) { //TURN RIGHT
reinaldo422 1:1a1bdef49fb0 133 leftSpeed = leftSpeed*1.25;
reinaldo422 1:1a1bdef49fb0 134 rightSpeed = rightSpeed*.75;
reinaldo422 1:1a1bdef49fb0 135 }
reinaldo422 1:1a1bdef49fb0 136 }
reinaldo422 1:1a1bdef49fb0 137 leftSpeed = leftSpeed > 1.0 ? 1.0 : leftSpeed;
reinaldo422 1:1a1bdef49fb0 138 leftSpeed = leftSpeed < -1.0 ? -1.0 : leftSpeed;
reinaldo422 1:1a1bdef49fb0 139 rightSpeed = rightSpeed > 1.0 ? 1.0 : rightSpeed;
reinaldo422 1:1a1bdef49fb0 140 rightSpeed = rightSpeed < -1.0 ? -1.0 : rightSpeed;
reinaldo422 1:1a1bdef49fb0 141 // sendFloat(leftSpeed);
reinaldo422 1:1a1bdef49fb0 142 // sendFloat(rightSpeed);
reinaldo422 1:1a1bdef49fb0 143 left.speed(leftSpeed);
reinaldo422 1:1a1bdef49fb0 144 right.speed(rightSpeed);
reinaldo422 1:1a1bdef49fb0 145 }
reinaldo422 1:1a1bdef49fb0 146
reinaldo422 1:1a1bdef49fb0 147 void brakeMotor() {
reinaldo422 1:1a1bdef49fb0 148 left.stop(1);
reinaldo422 1:1a1bdef49fb0 149 right.stop(1);
reinaldo422 1:1a1bdef49fb0 150 }
shintamainjp 0:31be9011f67e 151 int main(void) {
reinaldo422 1:1a1bdef49fb0 152 rn42.baud(115200);
reinaldo422 1:1a1bdef49fb0 153 rn42.putc('S');
reinaldo422 1:1a1bdef49fb0 154 rn42.putc('#');
reinaldo422 1:1a1bdef49fb0 155 // char sendRcvBuf[BUFFER_SIZE];
reinaldo422 1:1a1bdef49fb0 156 int cnt = 0;
reinaldo422 1:1a1bdef49fb0 157
reinaldo422 1:1a1bdef49fb0 158 //printf("Camera module\n");
reinaldo422 1:1a1bdef49fb0 159 //printf("Resetting...\n");
shintamainjp 0:31be9011f67e 160 wait(1);
shintamainjp 0:31be9011f67e 161 if (cam1.reset() == 0) {
reinaldo422 1:1a1bdef49fb0 162 //printf("Reset OK.\n");
shintamainjp 0:31be9011f67e 163 } else {
reinaldo422 1:1a1bdef49fb0 164 //printf("Reset fail.\n");
reinaldo422 1:1a1bdef49fb0 165 //error("Reset fail.");
shintamainjp 0:31be9011f67e 166 }
shintamainjp 0:31be9011f67e 167 wait(1);
reinaldo422 1:1a1bdef49fb0 168 rn42.putc('R');
reinaldo422 1:1a1bdef49fb0 169 rn42.putc('#');
reinaldo422 1:1a1bdef49fb0 170 float speed = 0.0;
reinaldo422 1:1a1bdef49fb0 171 float turn = 0.0;
reinaldo422 1:1a1bdef49fb0 172 while(1) {
reinaldo422 1:1a1bdef49fb0 173 if(rn42.readable()) {
reinaldo422 1:1a1bdef49fb0 174 char byte = rn42.getc();
reinaldo422 1:1a1bdef49fb0 175 //printf("Received: %c \n",byte);
reinaldo422 1:1a1bdef49fb0 176 switch(byte) {
reinaldo422 1:1a1bdef49fb0 177 case 'M': //Move command received; numbers to follow
reinaldo422 1:1a1bdef49fb0 178 while(!rn42.readable());
reinaldo422 1:1a1bdef49fb0 179 speed = nextFloat();
reinaldo422 1:1a1bdef49fb0 180 while(!rn42.readable());
reinaldo422 1:1a1bdef49fb0 181 turn = nextFloat();
reinaldo422 1:1a1bdef49fb0 182 updateMotor(speed, turn);
reinaldo422 1:1a1bdef49fb0 183 rn42.putc('D'); //precursor for Direction
reinaldo422 1:1a1bdef49fb0 184 sendFloat(compass.sample() / 10.0); //send compass reading
reinaldo422 1:1a1bdef49fb0 185 break;
reinaldo422 1:1a1bdef49fb0 186 case 'C':
reinaldo422 1:1a1bdef49fb0 187 brakeMotor();
reinaldo422 1:1a1bdef49fb0 188 char fname[64];
reinaldo422 1:1a1bdef49fb0 189 snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
reinaldo422 1:1a1bdef49fb0 190 int r = capture(&cam1, fname);
reinaldo422 1:1a1bdef49fb0 191 if (r == 0) {
reinaldo422 1:1a1bdef49fb0 192 //printf("[%04d]:OK.\n", cnt);
reinaldo422 1:1a1bdef49fb0 193 } else {
reinaldo422 1:1a1bdef49fb0 194 //printf("[%04d]:NG. (code=%d)\n", cnt, r);
reinaldo422 1:1a1bdef49fb0 195 //error("Failure.");
reinaldo422 1:1a1bdef49fb0 196 }
reinaldo422 1:1a1bdef49fb0 197 updateMotor(speed, turn);
reinaldo422 1:1a1bdef49fb0 198
reinaldo422 1:1a1bdef49fb0 199 //char img[1024];
reinaldo422 1:1a1bdef49fb0 200 // FILE *f = NULL;
reinaldo422 1:1a1bdef49fb0 201 // f = fopen(fname, "r");
reinaldo422 1:1a1bdef49fb0 202 // if(f==NULL) {
reinaldo422 1:1a1bdef49fb0 203 // //printf("Problem opening file for reading\r\n");
reinaldo422 1:1a1bdef49fb0 204 // }
reinaldo422 1:1a1bdef49fb0 205 // //printf("about to send image");
reinaldo422 1:1a1bdef49fb0 206 // // read the file and dump it as hex
reinaldo422 1:1a1bdef49fb0 207 // size_t br;
reinaldo422 1:1a1bdef49fb0 208 // int n = 0;
reinaldo422 1:1a1bdef49fb0 209 // bool flag = false;
reinaldo422 1:1a1bdef49fb0 210 // fseek(f, 0, SEEK_END); // seek to end of file
reinaldo422 1:1a1bdef49fb0 211 // int size = ftell(f); // get current file pointer
reinaldo422 1:1a1bdef49fb0 212 // fseek(f, 0, SEEK_SET); // seek back to beginning of file
reinaldo422 1:1a1bdef49fb0 213 rn42.putc('P');
reinaldo422 1:1a1bdef49fb0 214 sendInt(0);
reinaldo422 1:1a1bdef49fb0 215 // //printf("size: %d\n", size);
reinaldo422 1:1a1bdef49fb0 216 // while((br=fread(img,1,1024,f))!=0) {
reinaldo422 1:1a1bdef49fb0 217 // //printf("Read %d bytes\r\n",br);
reinaldo422 1:1a1bdef49fb0 218 // for(size_t i=0; i<br; i++) {
reinaldo422 1:1a1bdef49fb0 219 // if(n==0) printf("start: %x %x\n",img[i],img[i+1]);
reinaldo422 1:1a1bdef49fb0 220 // rn42.putc(img[i]);
reinaldo422 1:1a1bdef49fb0 221 // if(i > 0 && img[i-1] == 0xFF && img[i] == 0xD9) {
reinaldo422 1:1a1bdef49fb0 222 // //printf("spot of last byte: %d\n",n);
reinaldo422 1:1a1bdef49fb0 223 // flag = true;
reinaldo422 1:1a1bdef49fb0 224 // }
reinaldo422 1:1a1bdef49fb0 225 // if(i < 1024 && img[i] == 0xFF && img[i+1] == 0xD8) {
reinaldo422 1:1a1bdef49fb0 226 // //printf("spot of first byte: %d\n",n);
reinaldo422 1:1a1bdef49fb0 227 // }
reinaldo422 1:1a1bdef49fb0 228 // n++;
reinaldo422 1:1a1bdef49fb0 229 // if(flag) {
reinaldo422 1:1a1bdef49fb0 230 // //printf("byte after: %x\n",img[i]);
reinaldo422 1:1a1bdef49fb0 231 // }
reinaldo422 1:1a1bdef49fb0 232 // }
reinaldo422 1:1a1bdef49fb0 233 // }
reinaldo422 1:1a1bdef49fb0 234 //
reinaldo422 1:1a1bdef49fb0 235 // // determine whether we reached EOF or not
reinaldo422 1:1a1bdef49fb0 236 // if(!feof(f)&&ferror(f)) {
reinaldo422 1:1a1bdef49fb0 237 // //printf("Did not complete reading file, file error\r\n");
reinaldo422 1:1a1bdef49fb0 238 // } else {
reinaldo422 1:1a1bdef49fb0 239 // //printf("File read successfully\r\n");
reinaldo422 1:1a1bdef49fb0 240 // }
reinaldo422 1:1a1bdef49fb0 241 // fclose(f);
reinaldo422 1:1a1bdef49fb0 242 cnt++;
reinaldo422 1:1a1bdef49fb0 243 break;
reinaldo422 1:1a1bdef49fb0 244
reinaldo422 1:1a1bdef49fb0 245 }
shintamainjp 0:31be9011f67e 246 }
shintamainjp 0:31be9011f67e 247 }
shintamainjp 0:31be9011f67e 248 }