This code enables our mouse to go round the track using the two error methods.
Dependencies: Nucleo_blink_led mbed
Fork of Nucleo_blink_led by
Diff: calibrateFunc.cpp
- Revision:
- 5:dfea493f7a12
- Parent:
- 4:f1670eec4681
- Child:
- 6:a652deaae134
--- a/calibrateFunc.cpp Wed Mar 07 19:27:13 2018 +0000 +++ b/calibrateFunc.cpp Sun Mar 11 13:15:34 2018 +0000 @@ -1,5 +1,5 @@ -#include "mbed.h" -#include "funcdef.h" +#include <mbed.h> +#include <funcdef.h> void CalibrateFunc() { @@ -13,14 +13,13 @@ sensorMaxValue1[i]=sensorValue[i]; } - //LEFT WHEEL STARTS TURN - leftMotorVal.write(0.00125*100); - rightMotorVal.write(0.00125*0); + //LEFT WHEEL STARTS TURN + leftMotorVal.pulsewidth_us(300); + rightMotorVal.pulsewidth_us(0); //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET AND A SMIDGE MORE - ///////////////////////////////////////////// - - int doneYet=0; + ///////////////////////////////////////////// + int doneYet[] = {0,0,0,0,0,0,0,0}; while(1) { for (int i=0;i<=7;i++) @@ -30,23 +29,31 @@ { sensorMaxValue1[i]=sensorValue[i]; } - else if ((sensorValue[i]+70)<=sensorMaxValue1[i]) + else if ((sensorValue[i]+250)<=sensorMaxValue1[i]) { - doneYet++; - if (doneYet==8) + doneYet[i]=1; + if (((doneYet[6]) == 1) && ((doneYet[4]) == 1) && ((doneYet[2]) == 1) && ((doneYet[0]) == 1) && ((doneYet[1]) == 1) && ((doneYet[3]) == 1) && ((doneYet[5]) == 1) && ((doneYet[7]) == 1)) { - goto endOfScan1; + goto endOfScan1; } } } } endOfScan1: - wait(0.1); + wait_ms(50); ///////////////////////////////////////////// //STOP WHEELS - leftMotorVal.write(0.00125*0); - rightMotorVal.write(0.00125*0); + leftMotorVal.pulsewidth_us(0); + rightMotorVal.pulsewidth_us(0); + + //SLOW BEEPER TO 4HZ + counter=0; + delayBetweenPulses_ms=250; + //beeper.attach(&beep, 0.01); + LEDVal=0; + + wait_ms(500); //TAKE MIN/MAX VALUES 2ND TIME for (int i=0;i<=7;i++) @@ -57,11 +64,11 @@ } //RIGHT WHEEL STARTS TURN - leftMotorVal.write(0.00125*0); - rightMotorVal.write(0.00125*100); + leftMotorVal.pulsewidth_us(0); + rightMotorVal.pulsewidth_us(300); //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET IN NEW VARIABLES - doneYet=0; + int doneYet2[] = {0,0,0,0,0,0,0,0}; while(1) { for (int i=0;i<=7;i++) @@ -71,10 +78,10 @@ { sensorMaxValue2[i]=sensorValue[i]; } - else if ((sensorValue[i]+70)<=sensorMaxValue2[i]) - { - doneYet++; - if (doneYet==8) + else if ((sensorValue[i]+250)<=sensorMaxValue2[i]) + { + doneYet2[i]=1; + if (((doneYet2[6]) == 1) && ((doneYet2[4]) == 1) && ((doneYet2[2]) == 1) && ((doneYet2[0]) == 1) && ((doneYet2[1]) == 1) && ((doneYet2[3]) == 1) && ((doneYet2[5]) == 1) && ((doneYet2[7]) == 1)) { goto endOfScan2; } @@ -83,25 +90,27 @@ } endOfScan2: - wait(0.1); + wait_ms(150); ///////////////////////////////////////////// //STOP WHEELS - leftMotorVal.write(0.00125*0); - rightMotorVal.write(0.00125*0); + leftMotorVal.pulsewidth_us(0); + rightMotorVal.pulsewidth_us(0); + + wait_ms(5); //AVERAGE THESE VALUES - for (int i=1;i<=8;i++) + for (int i=0;i<=7;i++) { sensorMinValue[i]=(sensorMinValue1[i]+sensorMinValue2[i])/2; sensorMaxValue[i]=(sensorMaxValue1[i]+sensorMaxValue2[i])/2; } //SET THRESHOLDS FOR ERROR OF 0 - for (int i=1;i<=8;i++) + for (int i=0;i<=7;i++) { - sensorThreshold[i]=sensorMaxValue[i]-100; + sensorThreshold[i]=sensorMaxValue[i]-250; } - + ///////////////////////////////////////////////////////////////////////////////////////////////////////////// return; } \ No newline at end of file