EE149
/
FinalProject
Final Project files for mBed development.
Diff: main.c
- Revision:
- 15:14d4e7021125
- Parent:
- 13:070846d87d4a
- Child:
- 16:f1beef7beeb9
--- a/main.c Sun Nov 16 09:17:20 2014 +0000 +++ b/main.c Sun Nov 30 21:49:46 2014 +0000 @@ -37,6 +37,8 @@ DigitalOut oled_3(LED3); DigitalOut oled_4(LED4); +/* Local File System */ +LocalFileSystem local("local"); /** * @brief Entry point. Main loop. @@ -50,72 +52,114 @@ // // Drawing environment calibration. pi.sensor_auto_calibrate(); - pi.backward(DRIVE_SPEED); - float pos; - float over_thresh = 0.5; - float correction = 0.1; + //pi.backward(DRIVE_SPEED); + float pos = 0; + float over_thresh = 0.05; + float correction = 0.2*DRIVE_SPEED; + + wait(1); + + do { + pos = pi.line_position(); + if(pos > over_thresh) { + pi.right_motor(-DRIVE_SPEED); + pi.left_motor(-DRIVE_SPEED+correction); + } else if(pos < -over_thresh) { + pi.left_motor(-DRIVE_SPEED); + pi.right_motor(-DRIVE_SPEED+correction); + } else { + pi.right_motor(-DRIVE_SPEED); + pi.left_motor(-DRIVE_SPEED); + } + /* + pi.cls(); + pi.locate(0,0); + pi.printf("P: %f", pos); + */ + } while (pos != -1 && pos < 0.2); + pi.stop(); + wait(1); + if (pos != -1) { + timer.start(); + pi.forward(DRIVE_SPEED); + } else { + pi.cls(); + pi.locate(0,0); + pi.printf("LP: %f",pos); + return 1; + } + + wait(1); do { pos = pi.line_position(); if(pos > over_thresh) { pi.right_motor(DRIVE_SPEED); - pi.left_motor(DRIVE_SPEED - correction); + pi.left_motor(DRIVE_SPEED-correction); } else if(pos < -over_thresh) { pi.left_motor(DRIVE_SPEED); - pi.right_motor(DRIVE_SPEED - correction); + pi.right_motor(DRIVE_SPEED-correction); } else { - pi.forward(DRIVE_SPEED); + pi.right_motor(DRIVE_SPEED); + pi.left_motor(DRIVE_SPEED); + } + pi.cls(); + pi.locate(0,0); + pi.printf("Pos: %f", pos); + } while(pos != -1 && pos <= 0.3); + pi.stop(); + if(pos != -1) { + oled_1 = 1; + timer.stop(); + pi.printf("T: %d", timer.read_ms()); + } else { + pi.cls(); + pi.locate(0,0); + pi.printf("lP:%f", pos); + return 1; + } + pi.right(TURN_SPEED); + wait(0.5); + pi.stop(); + do { + pos = pi.line_position(); + if(pos > over_thresh) { + pi.right_motor(-DRIVE_SPEED); + pi.left_motor(-DRIVE_SPEED+correction); + } else if(pos < -over_thresh) { + pi.left_motor(-DRIVE_SPEED); + pi.right_motor(-DRIVE_SPEED+correction); + } else { + pi.right_motor(-DRIVE_SPEED); + pi.left_motor(-DRIVE_SPEED); } pi.cls(); pi.locate(0,0); pi.printf("P: %f", pos); - } while(pos != -1 && pos != 1); - if(pos == 1) { - timer.start(); - pi.forward(DRIVE_SPEED); + } while (pos != -1 && pos > -0.3); + pi.stop(); + if(pos != -1) { + oled_1 = 1; + timer.stop(); + pi.printf("T: %d", timer.read_ms()); } else { - pi.stop(); pi.cls(); pi.locate(0,0); - pi.printf("LP: %f",pos); - while(1); + pi.printf("lP:%f", pos); + return 1; } - - while(pi.line_position() == 1); - do { - pos = pi.line_position(); - if(pos > over_thresh) { - pi.right_motor(DRIVE_SPEED); - pi.left_motor(DRIVE_SPEED - correction); - } else if(pos < -over_thresh) { - pi.left_motor(DRIVE_SPEED); - pi.right_motor(DRIVE_SPEED - correction); - } else { - pi.forward(DRIVE_SPEED); - } - pi.cls(); - pi.locate(0,0); - pi.printf("Pos: %f", pos); - } while(pos != -1 && pos != 1); - if(pos == 1) { - oled_1 = 1; - timer.stop(); - pi.stop(); - } else { - pi.stop(); - pi.cls(); - pi.locate(0,0); - pi.printf("LP:%f", pos); - while(1); - } - // // Main program loop. // robot_loop(); + ps_file = fopen("/local/test.ps", "r"); + while () { + memset(instbuf, 0, instbuflen); + fread(instbuf, sizeof(char), instbuflen, ps_file); + } // // We should never reach this point! - while(1); + return 0; } int forward(int amt)