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: senseFunc.cpp
- Revision:
- 7:11dd5581c763
- Parent:
- 6:a652deaae134
diff -r a652deaae134 -r 11dd5581c763 senseFunc.cpp --- a/senseFunc.cpp Sun Apr 01 18:21:50 2018 +0000 +++ b/senseFunc.cpp Sat Apr 28 21:34:55 2018 +0000 @@ -14,56 +14,204 @@ } } +float interpolation(float errorVar) +{ + float distanceMm=0.0; + for (int i=1;i<=105;i++) //not looking at first values, because interpolation occurs within range of values + { + if ((errorVar>=linearOffset[i][0]) && (errorVar<linearOffset[i-1][0])) + { + //printf("linearOffset %8.4f", linearOffset[i-1][0]); + //printf("linearOffset %8.4f\r\n", linearOffset[i][0]); + distanceMm=linearOffset[i][1]+(errorVar-linearOffset[i][0])*((linearOffset[i-1][1]-linearOffset[i][1])/(linearOffset[i-1][0]-linearOffset[i][0])); + } + } + return distanceMm; +} + void senseFunc() { //READ ALL SENSORS float totalError = 0.0; - // for (int i=0;i<=7;i++) - for (int i=7;i>=0;i--) + //for (int i=0;i<=7;i++) + for (int i=7;i>=0;i--) { sensorValue[i]=((sensorPin[i]).read_u16()>>4); sensorErrors[i]=((1/(float)(sensorMaxValue[i]-sensorMinValue[i]))*sensorValue[i])+(1-(sensorMaxValue[i]/(float)(sensorMaxValue[i]-sensorMinValue[i]))); - } - /* - printf("%f ",sensorErrors[7]); - printf("%f ",sensorErrors[5]); - printf("%f ",sensorErrors[3]); - printf("%f ",sensorErrors[1]); - printf("%f ",sensorErrors[0]); - printf("%f ",sensorErrors[2]); - printf("%f ",sensorErrors[4]); - printf("%f\r\n ",sensorErrors[6]); - */ - // cout <<" " << sensorErrors[0] << endl; - // printf("%d ",sensorMaxValue1[0]); -// printf("%d ",sensorMaxValue2[0]); + } - // printf("%d ",sensorMaxValue[0]); -// printf("%d ",sensorMinValue[0]); -// printf("%d ",sensorValue[0]); -// printf("%f\r\n ",sensorErrors[0]); - // printf("%6.2f\r\n ",fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2+fmax(sensorErrors[4],0.0)*3+fmax(sensorErrors[6], 0.0)*4); + if ((sensorErrors[6]<=(float)0.7) && (sensorErrors[7]<=(float)0.7)) { - totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2+fmax(sensorErrors[4],0.0)*3+fmax(sensorErrors[6], 0.0)*5) - - (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2+fmax(sensorErrors[5],0.0)*3+fmax(sensorErrors[7], 0.0)*5); + totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2+fmax(sensorErrors[4],0.0)*3+fmax(sensorErrors[6], 0.0)*5) - + (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2+fmax(sensorErrors[5],0.0)*3+fmax(sensorErrors[7], 0.0)*5); + } + else + { + //To compensate for the black lines on the ramp + totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2) - + (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2); + accumulator=0; + errorDetectthres=100; + LEDVal=0; + } + + errorVar = -totalError; + errorVar=interpolation(errorVar)/-3.81; + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + for (int i=0;i<=7;i++) + { + if ((sensorValue[i])>=(sensorThreshold[i])) + { + sensorErrorsdiscrete[i] = 1; + } + else + { + sensorErrorsdiscrete[i] = 0; + } + } + + if ((sensorErrorsdiscrete[0]) == 1) + { + if ((sensorErrorsdiscrete[1]) == 1) + { + errorVardiscrete=0; + } + else if ((sensorErrorsdiscrete[2]) == 1) + { + errorVardiscrete=-1; + } + else + { + errorVardiscrete=-0.5; + } + } + else if ((sensorErrorsdiscrete[1]) == 1) + { + if ((sensorErrorsdiscrete[0]) == 1) + { + errorVardiscrete=0; + } + else if ((sensorErrorsdiscrete[3]) == 1) + { + errorVardiscrete=1; + } + else + { + errorVardiscrete=0.5; + } + } + else if ((sensorErrorsdiscrete[2]) == 1) + { + if ((sensorErrorsdiscrete[0]) == 1) + { + errorVardiscrete=-1; + } + else if ((sensorErrorsdiscrete[4]) == 1) + { + errorVardiscrete=-2; + } + else + { + errorVardiscrete=-1.5; + } + } + else if ((sensorErrorsdiscrete[3]) == 1) + { + if ((sensorErrorsdiscrete[1]) == 1) + { + errorVardiscrete=1; + } + else if ((sensorErrorsdiscrete[5]) == 1) + { + errorVardiscrete=2; + } + else + { + errorVardiscrete=1.5; + } + } + else if ((sensorErrorsdiscrete[4]) == 1) + { + if ((sensorErrorsdiscrete[2]) == 1) + { + errorVardiscrete=-2; + } + else if ((sensorErrorsdiscrete[6]) == 1) + { + errorVardiscrete=-3.5; + } + else + { + errorVardiscrete=-2.5; + } + } + else if ((sensorErrorsdiscrete[5]) == 1) + { + if ((sensorErrorsdiscrete[3]) == 1) + { + errorVardiscrete=2; + } + else if ((sensorErrorsdiscrete[7]) == 1) + { + errorVardiscrete=3.5; + } + else + { + errorVardiscrete=2.5; + } + } + else if ((sensorErrorsdiscrete[6]) == 1) + { + if ((sensorErrorsdiscrete[4]) == 1) + { + errorVardiscrete=-3.5; + } + else + { + //errorVardiscrete=-4; + errorVardiscrete=-5; + } + } + else if ((sensorErrorsdiscrete[7]) == 1) + { + if ((sensorErrorsdiscrete[5]) == 1) + { + errorVardiscrete=3.5; + } + else + { + //errorVardiscrete=4; + errorVardiscrete=5; + } } else { - //To compensate for the black lines on the ramp - totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2) - - (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2); + errorVardiscrete=previousErrordiscrete; } - //printf("%6.3f\t%6.3f\t%6.3f\t%6.3f\r\n", sensorErrors[0],sensorErrors[2],sensorErrors[4],sensorErrors[6]); - //printf("Total error %6.3f\r\n", totalError); - errorVar = -totalError; - //pc.printf("$%d %d %d;", (int)(100*(sensorErrors[0])),(int)(100*(sensorErrors[2])),(int)(100*(sensorErrors[4]))); - //wait_ms(1); - if((sensorErrors[0] < 0.1f) && (sensorErrors[1] < 0.1f) && (sensorErrors[2] < 0.1f) && (sensorErrors[3] < 0.1) && (sensorErrors[4] < 0.1f) && (sensorErrors[5] < 0.1f) && (sensorErrors[6] < 0.1) && (sensorErrors[7] < 0.1f)) + if (errorVar>largestpositiveerrorvar) + { + largestpositiveerrorvar=errorVar; + } + if (errorVar<largestnegativeerrorvar) + { + largestnegativeerrorvar=errorVar; + } + + if (((errorVar)<0.2) && ((errorVardiscrete)>2)) { - stopMotors=true; - } + errorVar=largestpositiveerrorvar; + } + if (((errorVar)<0.2) && ((errorVardiscrete)<-2)) + { + errorVar=largestnegativeerrorvar; + } + + //printf("errorVardiscrete: %d ",errorVardiscrete); + //printf("errorVar: %f\r\n",errorVar); + //wait_ms(200); return; }