
test
Dependents: MouseHybridSenseCode
calibrateFunc.cpp
- Committer:
- JamieBignell
- Date:
- 2018-04-28
- Revision:
- 7:11dd5581c763
- Parent:
- 6:a652deaae134
File content as of revision 7:11dd5581c763:
#include <mbed.h> #include <funcdef.h> void CalibrateFunc() { //USER PUTS MOUSE LEFT OF LINE //TAKE MIN/MAX VALUES 1ST TIME for (int i=0;i<=7;i++) { sensorValue[i]=((sensorPin[i]).read_u16()>>4); sensorMinValue1[i]=sensorValue[i]; sensorMaxValue1[i]=sensorValue[i]; } //LEFT WHEEL STARTS TURN leftMotorVal.pulsewidth_us(calibrationMotorspeed); rightMotorVal.pulsewidth_us(0); //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET AND A SMIDGE MORE ///////////////////////////////////////////// int doneYet[] = {0,0,0,0,0,0,0,0}; while(1) { for (int i=0;i<=7;i++) { sensorValue[i]=((sensorPin[i]).read_u16()>>4); if (sensorValue[i]>=sensorMaxValue1[i]) { sensorMaxValue1[i]=sensorValue[i]; } else if ((sensorValue[i]+250)<=sensorMaxValue1[i]) { 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; } } } } endOfScan1: wait_ms(50); ///////////////////////////////////////////// //STOP WHEELS leftMotorVal.pulsewidth_us(0); rightMotorVal.pulsewidth_us(0); //TURN LED OFF LEDVal=0; wait_ms(500); //TAKE MIN/MAX VALUES 2ND TIME for (int i=0;i<=7;i++) { sensorValue[i]=((sensorPin[i]).read_u16()>>4); sensorMinValue2[i]=sensorValue[i]; sensorMaxValue2[i]=sensorValue[i]; } //RIGHT WHEEL STARTS TURN leftMotorVal.pulsewidth_us(0); rightMotorVal.pulsewidth_us(calibrationMotorspeed); //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET IN NEW VARIABLES int doneYet2[] = {0,0,0,0,0,0,0,0}; while(1) { for (int i=0;i<=7;i++) { sensorValue[i]=((sensorPin[i]).read_u16()>>4); if (sensorValue[i]>=sensorMaxValue2[i]) { sensorMaxValue2[i]=sensorValue[i]; } 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; } } } } endOfScan2: wait_ms(50); ///////////////////////////////////////////// //STOP WHEELS leftMotorVal.pulsewidth_us(0); rightMotorVal.pulsewidth_us(0); wait_ms(250); //AVERAGE THESE VALUES 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=0;i<=7;i++) { sensorThreshold[i]=sensorMaxValue[i]-250; } ///////////////////////////////////////////////////////////////////////////////////////////////////////////// return; }