Jamie Bignell / Mbed 2 deprecated Nucleo_blink_led

test

Dependencies:   mbed

Dependents:   MouseHybridSenseCode

senseFunc.cpp@7:11dd5581c763, 2018-04-28 (annotated)

Committer:
JamieBignell
Date:
Sat Apr 28 21:34:55 2018 +0000
Revision:
7:11dd5581c763
Parent:
6:a652deaae134 
Hybrid Mouse Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JamieBignell 5:dfea493f7a12 1 #include <mbed.h>
JamieBignell 5:dfea493f7a12 2 #include <funcdef.h>
JamieBignell 6:a652deaae134 3 //#include <iostream>
JamieBignell 6:a652deaae134 4 #include <math.h>
JamieBignell 6:a652deaae134 5
JamieBignell 6:a652deaae134 6 float fmax(float x, float y)
JamieBignell 6:a652deaae134 7 {
JamieBignell 6:a652deaae134 8 if(x > y)
JamieBignell 6:a652deaae134 9 {
JamieBignell 6:a652deaae134 10 return x;
JamieBignell 6:a652deaae134 11 }else
JamieBignell 6:a652deaae134 12 {
JamieBignell 6:a652deaae134 13 return y;
JamieBignell 6:a652deaae134 14 }
JamieBignell 6:a652deaae134 15 }
JamieBignell 6:a652deaae134 16
JamieBignell 7:11dd5581c763 17 float interpolation(float errorVar)
JamieBignell 7:11dd5581c763 18 {
JamieBignell 7:11dd5581c763 19 float distanceMm=0.0;
JamieBignell 7:11dd5581c763 20 for (int i=1;i<=105;i++) //not looking at first values, because interpolation occurs within range of values
JamieBignell 7:11dd5581c763 21 {
JamieBignell 7:11dd5581c763 22 if ((errorVar>=linearOffset[i][0]) && (errorVar<linearOffset[i-1][0]))
JamieBignell 7:11dd5581c763 23 {
JamieBignell 7:11dd5581c763 24 //printf("linearOffset %8.4f", linearOffset[i-1][0]);
JamieBignell 7:11dd5581c763 25 //printf("linearOffset %8.4f\r\n", linearOffset[i][0]);
JamieBignell 7:11dd5581c763 26 distanceMm=linearOffset[i][1]+(errorVar-linearOffset[i][0])*((linearOffset[i-1][1]-linearOffset[i][1])/(linearOffset[i-1][0]-linearOffset[i][0]));
JamieBignell 7:11dd5581c763 27 }
JamieBignell 7:11dd5581c763 28 }
JamieBignell 7:11dd5581c763 29 return distanceMm;
JamieBignell 7:11dd5581c763 30 }
JamieBignell 7:11dd5581c763 31
JamieBignell 5:dfea493f7a12 32 void senseFunc()
JamieBignell 5:dfea493f7a12 33 {
JamieBignell 5:dfea493f7a12 34 //READ ALL SENSORS
JamieBignell 6:a652deaae134 35 float totalError = 0.0;
JamieBignell 6:a652deaae134 36
JamieBignell 7:11dd5581c763 37 //for (int i=0;i<=7;i++)
JamieBignell 7:11dd5581c763 38 for (int i=7;i>=0;i--)
JamieBignell 5:dfea493f7a12 39 {
JamieBignell 5:dfea493f7a12 40 sensorValue[i]=((sensorPin[i]).read_u16()>>4);
JamieBignell 6:a652deaae134 41 sensorErrors[i]=((1/(float)(sensorMaxValue[i]-sensorMinValue[i]))*sensorValue[i])+(1-(sensorMaxValue[i]/(float)(sensorMaxValue[i]-sensorMinValue[i])));
JamieBignell 7:11dd5581c763 42 }
JamieBignell 5:dfea493f7a12 43
JamieBignell 7:11dd5581c763 44
JamieBignell 6:a652deaae134 45 if ((sensorErrors[6]<=(float)0.7) && (sensorErrors[7]<=(float)0.7))
JamieBignell 5:dfea493f7a12 46 {
JamieBignell 7:11dd5581c763 47 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) -
JamieBignell 7:11dd5581c763 48 (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2+fmax(sensorErrors[5],0.0)*3+fmax(sensorErrors[7], 0.0)*5);
JamieBignell 7:11dd5581c763 49 }
JamieBignell 7:11dd5581c763 50 else
JamieBignell 7:11dd5581c763 51 {
JamieBignell 7:11dd5581c763 52 //To compensate for the black lines on the ramp
JamieBignell 7:11dd5581c763 53 totalError = (fmax(sensorErrors[0], 0.0)*1+fmax(sensorErrors[2],0.0)*2) -
JamieBignell 7:11dd5581c763 54 (fmax(sensorErrors[1], 0.0)*1+fmax(sensorErrors[3],0.0)*2);
JamieBignell 7:11dd5581c763 55 accumulator=0;
JamieBignell 7:11dd5581c763 56 errorDetectthres=100;
JamieBignell 7:11dd5581c763 57 LEDVal=0;
JamieBignell 7:11dd5581c763 58 }
JamieBignell 7:11dd5581c763 59
JamieBignell 7:11dd5581c763 60 errorVar = -totalError;
JamieBignell 7:11dd5581c763 61 errorVar=interpolation(errorVar)/-3.81;
JamieBignell 7:11dd5581c763 62
JamieBignell 7:11dd5581c763 63 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
JamieBignell 7:11dd5581c763 64 for (int i=0;i<=7;i++)
JamieBignell 7:11dd5581c763 65 {
JamieBignell 7:11dd5581c763 66 if ((sensorValue[i])>=(sensorThreshold[i]))
JamieBignell 7:11dd5581c763 67 {
JamieBignell 7:11dd5581c763 68 sensorErrorsdiscrete[i] = 1;
JamieBignell 7:11dd5581c763 69 }
JamieBignell 7:11dd5581c763 70 else
JamieBignell 7:11dd5581c763 71 {
JamieBignell 7:11dd5581c763 72 sensorErrorsdiscrete[i] = 0;
JamieBignell 7:11dd5581c763 73 }
JamieBignell 7:11dd5581c763 74 }
JamieBignell 7:11dd5581c763 75
JamieBignell 7:11dd5581c763 76 if ((sensorErrorsdiscrete[0]) == 1)
JamieBignell 7:11dd5581c763 77 {
JamieBignell 7:11dd5581c763 78 if ((sensorErrorsdiscrete[1]) == 1)
JamieBignell 7:11dd5581c763 79 {
JamieBignell 7:11dd5581c763 80 errorVardiscrete=0;
JamieBignell 7:11dd5581c763 81 }
JamieBignell 7:11dd5581c763 82 else if ((sensorErrorsdiscrete[2]) == 1)
JamieBignell 7:11dd5581c763 83 {
JamieBignell 7:11dd5581c763 84 errorVardiscrete=-1;
JamieBignell 7:11dd5581c763 85 }
JamieBignell 7:11dd5581c763 86 else
JamieBignell 7:11dd5581c763 87 {
JamieBignell 7:11dd5581c763 88 errorVardiscrete=-0.5;
JamieBignell 7:11dd5581c763 89 }
JamieBignell 7:11dd5581c763 90 }
JamieBignell 7:11dd5581c763 91 else if ((sensorErrorsdiscrete[1]) == 1)
JamieBignell 7:11dd5581c763 92 {
JamieBignell 7:11dd5581c763 93 if ((sensorErrorsdiscrete[0]) == 1)
JamieBignell 7:11dd5581c763 94 {
JamieBignell 7:11dd5581c763 95 errorVardiscrete=0;
JamieBignell 7:11dd5581c763 96 }
JamieBignell 7:11dd5581c763 97 else if ((sensorErrorsdiscrete[3]) == 1)
JamieBignell 7:11dd5581c763 98 {
JamieBignell 7:11dd5581c763 99 errorVardiscrete=1;
JamieBignell 7:11dd5581c763 100 }
JamieBignell 7:11dd5581c763 101 else
JamieBignell 7:11dd5581c763 102 {
JamieBignell 7:11dd5581c763 103 errorVardiscrete=0.5;
JamieBignell 7:11dd5581c763 104 }
JamieBignell 7:11dd5581c763 105 }
JamieBignell 7:11dd5581c763 106 else if ((sensorErrorsdiscrete[2]) == 1)
JamieBignell 7:11dd5581c763 107 {
JamieBignell 7:11dd5581c763 108 if ((sensorErrorsdiscrete[0]) == 1)
JamieBignell 7:11dd5581c763 109 {
JamieBignell 7:11dd5581c763 110 errorVardiscrete=-1;
JamieBignell 7:11dd5581c763 111 }
JamieBignell 7:11dd5581c763 112 else if ((sensorErrorsdiscrete[4]) == 1)
JamieBignell 7:11dd5581c763 113 {
JamieBignell 7:11dd5581c763 114 errorVardiscrete=-2;
JamieBignell 7:11dd5581c763 115 }
JamieBignell 7:11dd5581c763 116 else
JamieBignell 7:11dd5581c763 117 {
JamieBignell 7:11dd5581c763 118 errorVardiscrete=-1.5;
JamieBignell 7:11dd5581c763 119 }
JamieBignell 7:11dd5581c763 120 }
JamieBignell 7:11dd5581c763 121 else if ((sensorErrorsdiscrete[3]) == 1)
JamieBignell 7:11dd5581c763 122 {
JamieBignell 7:11dd5581c763 123 if ((sensorErrorsdiscrete[1]) == 1)
JamieBignell 7:11dd5581c763 124 {
JamieBignell 7:11dd5581c763 125 errorVardiscrete=1;
JamieBignell 7:11dd5581c763 126 }
JamieBignell 7:11dd5581c763 127 else if ((sensorErrorsdiscrete[5]) == 1)
JamieBignell 7:11dd5581c763 128 {
JamieBignell 7:11dd5581c763 129 errorVardiscrete=2;
JamieBignell 7:11dd5581c763 130 }
JamieBignell 7:11dd5581c763 131 else
JamieBignell 7:11dd5581c763 132 {
JamieBignell 7:11dd5581c763 133 errorVardiscrete=1.5;
JamieBignell 7:11dd5581c763 134 }
JamieBignell 7:11dd5581c763 135 }
JamieBignell 7:11dd5581c763 136 else if ((sensorErrorsdiscrete[4]) == 1)
JamieBignell 7:11dd5581c763 137 {
JamieBignell 7:11dd5581c763 138 if ((sensorErrorsdiscrete[2]) == 1)
JamieBignell 7:11dd5581c763 139 {
JamieBignell 7:11dd5581c763 140 errorVardiscrete=-2;
JamieBignell 7:11dd5581c763 141 }
JamieBignell 7:11dd5581c763 142 else if ((sensorErrorsdiscrete[6]) == 1)
JamieBignell 7:11dd5581c763 143 {
JamieBignell 7:11dd5581c763 144 errorVardiscrete=-3.5;
JamieBignell 7:11dd5581c763 145 }
JamieBignell 7:11dd5581c763 146 else
JamieBignell 7:11dd5581c763 147 {
JamieBignell 7:11dd5581c763 148 errorVardiscrete=-2.5;
JamieBignell 7:11dd5581c763 149 }
JamieBignell 7:11dd5581c763 150 }
JamieBignell 7:11dd5581c763 151 else if ((sensorErrorsdiscrete[5]) == 1)
JamieBignell 7:11dd5581c763 152 {
JamieBignell 7:11dd5581c763 153 if ((sensorErrorsdiscrete[3]) == 1)
JamieBignell 7:11dd5581c763 154 {
JamieBignell 7:11dd5581c763 155 errorVardiscrete=2;
JamieBignell 7:11dd5581c763 156 }
JamieBignell 7:11dd5581c763 157 else if ((sensorErrorsdiscrete[7]) == 1)
JamieBignell 7:11dd5581c763 158 {
JamieBignell 7:11dd5581c763 159 errorVardiscrete=3.5;
JamieBignell 7:11dd5581c763 160 }
JamieBignell 7:11dd5581c763 161 else
JamieBignell 7:11dd5581c763 162 {
JamieBignell 7:11dd5581c763 163 errorVardiscrete=2.5;
JamieBignell 7:11dd5581c763 164 }
JamieBignell 7:11dd5581c763 165 }
JamieBignell 7:11dd5581c763 166 else if ((sensorErrorsdiscrete[6]) == 1)
JamieBignell 7:11dd5581c763 167 {
JamieBignell 7:11dd5581c763 168 if ((sensorErrorsdiscrete[4]) == 1)
JamieBignell 7:11dd5581c763 169 {
JamieBignell 7:11dd5581c763 170 errorVardiscrete=-3.5;
JamieBignell 7:11dd5581c763 171 }
JamieBignell 7:11dd5581c763 172 else
JamieBignell 7:11dd5581c763 173 {
JamieBignell 7:11dd5581c763 174 //errorVardiscrete=-4;
JamieBignell 7:11dd5581c763 175 errorVardiscrete=-5;
JamieBignell 7:11dd5581c763 176 }
JamieBignell 7:11dd5581c763 177 }
JamieBignell 7:11dd5581c763 178 else if ((sensorErrorsdiscrete[7]) == 1)
JamieBignell 7:11dd5581c763 179 {
JamieBignell 7:11dd5581c763 180 if ((sensorErrorsdiscrete[5]) == 1)
JamieBignell 7:11dd5581c763 181 {
JamieBignell 7:11dd5581c763 182 errorVardiscrete=3.5;
JamieBignell 7:11dd5581c763 183 }
JamieBignell 7:11dd5581c763 184 else
JamieBignell 7:11dd5581c763 185 {
JamieBignell 7:11dd5581c763 186 //errorVardiscrete=4;
JamieBignell 7:11dd5581c763 187 errorVardiscrete=5;
JamieBignell 7:11dd5581c763 188 }
JamieBignell 5:dfea493f7a12 189 }
JamieBignell 5:dfea493f7a12 190 else
JamieBignell 5:dfea493f7a12 191 {
JamieBignell 7:11dd5581c763 192 errorVardiscrete=previousErrordiscrete;
JamieBignell 5:dfea493f7a12 193 }
JamieBignell 6:a652deaae134 194
JamieBignell 7:11dd5581c763 195 if (errorVar>largestpositiveerrorvar)
JamieBignell 7:11dd5581c763 196 {
JamieBignell 7:11dd5581c763 197 largestpositiveerrorvar=errorVar;
JamieBignell 7:11dd5581c763 198 }
JamieBignell 7:11dd5581c763 199 if (errorVar<largestnegativeerrorvar)
JamieBignell 7:11dd5581c763 200 {
JamieBignell 7:11dd5581c763 201 largestnegativeerrorvar=errorVar;
JamieBignell 7:11dd5581c763 202 }
JamieBignell 7:11dd5581c763 203
JamieBignell 7:11dd5581c763 204 if (((errorVar)<0.2) && ((errorVardiscrete)>2))
JamieBignell 6:a652deaae134 205 {
JamieBignell 7:11dd5581c763 206 errorVar=largestpositiveerrorvar;
JamieBignell 7:11dd5581c763 207 }
JamieBignell 7:11dd5581c763 208 if (((errorVar)<0.2) && ((errorVardiscrete)<-2))
JamieBignell 7:11dd5581c763 209 {
JamieBignell 7:11dd5581c763 210 errorVar=largestnegativeerrorvar;
JamieBignell 7:11dd5581c763 211 }
JamieBignell 7:11dd5581c763 212
JamieBignell 7:11dd5581c763 213 //printf("errorVardiscrete: %d ",errorVardiscrete);
JamieBignell 7:11dd5581c763 214 //printf("errorVar: %f\r\n",errorVar);
JamieBignell 7:11dd5581c763 215 //wait_ms(200);
JamieBignell 5:dfea493f7a12 216 return;
JamieBignell 6:a652deaae134 217 }