This ILC code kinda works
Dependencies: FatFileSystem MSCFileSystem btbee m3pi_ng mbed ILCRobot
Fork of RobotB by
main.cpp
- Committer:
- charwhit
- Date:
- 2015-05-22
- Revision:
- 4:acd0f86ed832
- Parent:
- 3:bae8eb81a9d7
- Child:
- 5:c38929c0fd95
File content as of revision 4:acd0f86ed832:
#include "mbed.h" #include "btbee.h" #include "m3pi_ng.h" m3pi robot; btbee btbee; DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf Timer timer; Timer time_wait; #define MAX 0.95 #define MIN 0 //#define P_TERM 5 //#define I_TERM 0 //#define D_TERM 20 int main(){ int P_TERM = 5; int I_TERM = 0; int D_TERM = 20; btbee.reset(); 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; char arr_read[30]; // this should be long enough to store any reply coming in over bt. int chars_read; /* for (int i = 0; i <5; ++i) current_pos[i] = 0.0; */ timer.start(); time_wait.start(); wait(8); 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 (m3pi_IN [0] == 0) //{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); if (btbee.writeable()){ btbee.printf("Lap %d time: %f\n", lap, lap_time); btbee.printf("Avg Lap time: %f\n", average_time); } y=0; break;} else { while( x[0]> 300 && x[4] > 300) {robot.calibrated_sensor(x);} lap_time = timer.read(); if (btbee.writeable()){ btbee.printf("Lap %d time: %f\n", lap, lap_time); } 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]); } 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((5-time_wait.read_ms())/1000); } 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); }