Nick Tavis
basic code
Dependencies: FatFileSystem MSCFileSystem btbee m3pi_ng mbed
Diff: main.cpp
- Revision:
- 6:c38929c0fd95
- Parent:
- 5:acd0f86ed832
- Child:
- 7:99d09e88924b
--- a/main.cpp Fri May 22 14:21:22 2015 +0000
+++ b/main.cpp Fri May 22 15:07:30 2015 +0000
@@ -7,169 +7,191 @@
Timer timer;
Timer time_wait;
#define MAX 0.95
-#define MIN 0
+#define MIN 0
//#define P_TERM 5
//#define I_TERM 0
-//#define D_TERM 20
+//#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;
-
+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;
+ int count = 0;
+ int paramChange[3];
+
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();
-
+ /* 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 == 2) {
+ 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);
+ }
+ robot.stop();
+ while (count < 3){
+ //btbee.printf("Input parameter\n");
+ btbee.read_line(arr_read, 30, &chars_read);
+ paramChange[count] = atoi(arr_read);
+ btbee.printf("%d", arr_read);
+ count++;
+ }
+ P_TERM = paramChange[0];
+ I_TERM = paramChange[1];
+ D_TERM = paramChange[2];
+ lap = 0;
+ total_time = 0;
+ count = 0;
+ continue;
+
+ } 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();
+ }
+ }
+
- 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]);
+ // 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);
}
-
- 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'};
+
+
+
+ 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
+
+
+
+
+}
\ No newline at end of file