one lap kind of works
Dependencies: FatFileSystem MSCFileSystem btbee m3pi_ng mbed
Fork of Robot by
Diff: main.cpp
- Revision:
- 2:80a1ed62c307
- Parent:
- 1:42bba20ee253
- Child:
- 3:bae8eb81a9d7
diff -r 42bba20ee253 -r 80a1ed62c307 main.cpp --- a/main.cpp Mon May 18 12:22:57 2015 +0000 +++ b/main.cpp Thu May 21 14:38:47 2015 +0000 @@ -1,16 +1,155 @@ #include "mbed.h" #include "btbee.h" #include "m3pi_ng.h" +m3pi robot; +Timer timer; +Timer time_wait; +#define MAX 0.95 +#define MIN 0 -DigitalOut myled(LED1); +#define P_TERM 5 +#define I_TERM 0 +#define D_TERM 20 + + int main(){ - - while(1){ - myled = 1; - wait(0.2); - myled = 0; - wait(0.2); + + robot.sensor_auto_calibrate(); + wait(2.0); + + float right; + float left; + //float current_pos[5]; + float current_pos = 0.0; + float previous_pos =0.0; + float derivative, proportional, integral = 0; + float power; + float speed = MAX; + + int lap = 0; + float lap_time = 0.0; + float total_time = 0.0; + float average_time = 0.0; + int y =1; + + /* for (int i = 0; i <5; ++i) + current_pos[i] = 0.0; */ + timer.start(); + + + time_wait.start(); + while(y) + {time_wait.reset(); + //Get raw sensor values + int x [5]; + robot.calibrated_sensor(x); + + + + //Check to make sure battery isn't low + if (robot.battery() < 2.4) + {timer.stop(); + break;} + + else if( x[0] > 300 && x[2]>300 && x[4]>300) + { if (lap == 0) + { while( x[0]> 300 && x[4] > 300) + {robot.calibrated_sensor(x);} + timer.start(); + lap= lap +1; + } + + else if (lap == 5) + {lap_time = timer.read(); + total_time += lap_time; + average_time = total_time/lap; + robot.printf("%f",average_time); + y=0; + break;} + else + { while( x[0]> 300 && x[4] > 300) + {robot.calibrated_sensor(x);} + lap_time = timer.read(); + total_time += lap_time; + average_time = total_time/lap; + lap = lap +1; + timer.reset(); } + } + + + // Get the position of the line. + /* for (int i =0; i < 4; ++i) + current_pos[i] = current_pos[i+1]; + current_pos[4] = robot.line_position(); + proportional = current_pos[4]; + + // compute the derivative + derivative = 0; + for (int i =1; i<5;++i) { + if (i ==1) + derivative += 0*(current_pos[i] - current_pos[i-1]); + else if (i == 2) + derivative += 0*(current_pos[i] - current_pos[i-1]); + else if (i==3) + derivative += 0*(current_pos[i] - current_pos[i-1]); + else + derivative += (current_pos[i] - current_pos[i-1]); } -} \ No newline at end of file + derivative = derivative; */ + + + current_pos = robot.line_position(); + proportional = current_pos; + + derivative = current_pos - previous_pos; + + + //compute the integral + integral =+ proportional; + + //remember the last position. + previous_pos = current_pos; + + // compute the power + power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM)); + //computer new speeds + right = speed+power; + left = speed-power; + + //limit checks + if(right<MIN) + right = MIN; + else if (right > MAX) + right = MAX; + + if(left<MIN) + left = MIN; + else if (left>MIN) + left = MAX; + + //set speed + + robot.left_motor(left); + robot.right_motor(right); + + wait(.005-time_wait.read()); + } + + + + robot.stop(); + + char hail[]={'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8' +,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G' +,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8' +,'G','8','E','8','D','8','C','4'}; + int numb = 59; + + robot.playtune(hail,numb); + + + + + } \ No newline at end of file