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@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?
User | Revision | Line number | New 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 | } |