EE149
/
FinalProject
Final Project files for mBed development.
Revision 34:3066686d5152, committed 2014-12-10
- Comitter:
- lsaristo
- Date:
- Wed Dec 10 18:49:45 2014 +0000
- Parent:
- 33:8e2e8b664938
- Child:
- 35:a1c14c6d9282
- Commit message:
- Everything done on Tuesday night. This code actually produces a closed box of side length 500!
Changed in this revision
main.c | Show annotated file Show diff for this revision Revisions of this file |
main.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.c Tue Dec 09 23:07:08 2014 +0000 +++ b/main.c Wed Dec 10 18:49:45 2014 +0000 @@ -40,10 +40,11 @@ pi.locate(0,1); pi.printf("%f mV", pi.battery()); pi.sensor_auto_calibrate(); + timerWait(.2); do { pos = pi.line_position(); - if(pos > over_thresh) { + if(pos > over_thresh) { pi.right_motor(CAL_SPEED); pi.left_motor(CAL_SPEED-correction); } else if(pos < -over_thresh) { @@ -61,7 +62,8 @@ if(pos != -1) { timer.stop(); } else { - return 1; + oled_1 = 1; + while(1); } left(180); Timer caltimer; @@ -83,26 +85,27 @@ pi.printf("Pos: %f", pos); } while(pos != -1 && pos <= 0.3); caltimer.stop(); - cal_time = caltimer.read_ms(); + cal_time = caltimer.read_ms()*2; pi.stop(); if(pos != -1) { timer.stop(); } else { - return 1; + oled_1 = 1; + while(1); } right(180); timerWait(.2); while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) { - pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED)); + pi.right((pos < 0 ? -.6*.5*CAL_SPEED : .6*.5*CAL_SPEED)); pi.cls(); pi.locate(0,0); pi.printf("O: %f", pos); } pi.stop(); timerWait(0.2); - backward(500); - + backward(400); + // --------------------------------------------------------- // DONE calibrating. Begin reading in coordinate // file and run robot. @@ -134,7 +137,7 @@ pi.cls(); pi.locate(0,0); pi.printf("sscanf1"); - return 1; + while(1); } cur = strchr(instbuf, '\n'); cur++; @@ -168,10 +171,23 @@ delta_x = x-last_x; delta_y = y-last_y; - - /* Compute turn angle and turn. */ - theta = atan(((double) delta_y)/((double) delta_x)); - theta *= 57.29; + if (delta_y == 0) { + if (delta_x < 0) { + theta = 180; + } else { + theta = 0; + } + } else if (delta_x == 0) { + if (delta_y < 0) { + theta = -90; + } else { + theta = 90; + } + } else { + /* Compute turn angle and turn. */ + theta = atan(((double) delta_y)/((double) delta_x)); + theta *= 57.29; + } if (delta_x < 0 && delta_y > 0) { theta += 180; } @@ -209,9 +225,8 @@ } pi.cls(); pi.locate(0,0); - pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x)); - forward(0.4*cal_time*(dist/(double)dim_x)); - + pi.printf("d:%f", 0.55*cal_time*(dist/(double)dim_x)); + forward(0.55*cal_time*(dist/(double)dim_x)); last_x = x; last_y = y; next = strchr(cur, '\n'); @@ -228,7 +243,8 @@ pi.cls(); pi.locate(0,0); pi.printf("Done"); - pi.stop(); // Just in case. Prevent runaway polio. That's all bad! + pi.stop(); + oled_3 = 0; while (1); } @@ -263,12 +279,25 @@ void forward(int amt) { Timer t; + float ms = 0; + float last_ms = 0; pi.locate(0,0); pi.printf("Fwd %d", amt); t.start(); - pi.left_motor(DRIVE_SPEED+.0023); + pi.left_motor(DRIVE_SPEED+.002); pi.right_motor(DRIVE_SPEED); - while(t.read_ms() < amt); + ms = t.read_ms(); + while(ms < amt) { + if (ms-last_ms > 1000) { + t.stop(); + right(3); + last_ms = ms; + t.start(); + pi.left_motor(DRIVE_SPEED+.002); + pi.right_motor(DRIVE_SPEED); + } + ms = t.read_ms(); + } pi.stop(); } @@ -286,6 +315,7 @@ if(deg < 0) { return left(-1*deg); } + deg = deg+1.5; Timer t; float amt = ((float)(deg/360)*TIME_FACT); t.start(); @@ -306,6 +336,7 @@ if(deg < 0) { return right(-1*deg); } + deg = deg + 1.5; Timer t; pi.left(TURN_SPEED); t.start();
--- a/main.h Tue Dec 09 23:07:08 2014 +0000 +++ b/main.h Wed Dec 10 18:49:45 2014 +0000 @@ -17,12 +17,12 @@ #include <stdio.h> #include <math.h> #define TURN_SPEED 0.15 -#define DRIVE_SPEED 0.2 +#define DRIVE_SPEED 0.25 #define ERR_SUCCESS 0 #define ERR_FAILURE 1 #define DRIVE_RATE 1/50 #define TIME_FACT 1780 -#define CAL_SPEED .1 +#define CAL_SPEED .25 #define CLOSE_ENOUGH .0008 /** @brief Move the robot from its current position to (x,y) */ @@ -72,19 +72,6 @@ */ void left (float deg); -/** @brief Lower pen intro drawing position */ -void pen_down(); - -/** @brief Raise pen into moveto position */ -void pen_up(); - -/** - * @brief Controller decision logic. - * - * Decide what to do next based on the status of the drawing so far. - */ -void next_action(); - /** * @brief Wait for a number of seconds, possibly fractional. *