Working, Clean
Dependents: ShowItv2 ShowIt_robot_control
Fork of SignalNumber by
signalnumber.cpp@8:9098c6b1897b, 2017-11-07 (annotated)
- Committer:
- peterknoben
- Date:
- Tue Nov 07 09:30:28 2017 +0000
- Revision:
- 8:9098c6b1897b
- Parent:
- 7:893503895342
Final version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
peterknoben | 0:5f8dee4d4b09 | 1 | #include "signalnumber.h" |
peterknoben | 0:5f8dee4d4b09 | 2 | #include "mbed.h" |
peterknoben | 0:5f8dee4d4b09 | 3 | |
peterknoben | 0:5f8dee4d4b09 | 4 | |
peterknoben | 7:893503895342 | 5 | //------------------------------------------------------------------------------ |
peterknoben | 0:5f8dee4d4b09 | 6 | //Constants for mean value |
peterknoben | 7:893503895342 | 7 | const int n = 500; //Window size for the mean value |
peterknoben | 7:893503895342 | 8 | float emg0_filtered[n] = {}, emg2_filtered[n] = {}; |
peterknoben | 7:893503895342 | 9 | int count1 = 0, count2 = 0; //Counters for the mean values |
peterknoben | 7:893503895342 | 10 | int countx = 0, county = 0.0; //Counters for the signalnumbers |
peterknoben | 7:893503895342 | 11 | float meanL = 0.0, meanR = 0.0; //Internal function calculation variables |
peterknoben | 0:5f8dee4d4b09 | 12 | |
peterknoben | 0:5f8dee4d4b09 | 13 | //Constants EMG switch |
peterknoben | 8:9098c6b1897b | 14 | const float MinTreshold = 0.40; |
peterknoben | 8:9098c6b1897b | 15 | const float MaxTreshold = 0.750; |
peterknoben | 7:893503895342 | 16 | int SpeedL = 0, SpeedR =0; |
peterknoben | 0:5f8dee4d4b09 | 17 | |
peterknoben | 0:5f8dee4d4b09 | 18 | |
peterknoben | 0:5f8dee4d4b09 | 19 | SignalNumber::SignalNumber(void) |
peterknoben | 0:5f8dee4d4b09 | 20 | { |
peterknoben | 0:5f8dee4d4b09 | 21 | |
peterknoben | 0:5f8dee4d4b09 | 22 | } |
peterknoben | 0:5f8dee4d4b09 | 23 | |
peterknoben | 7:893503895342 | 24 | //------------------------------------------------------------------------------ |
peterknoben | 5:f3ddc3a7b292 | 25 | //Determine the Left mean value |
peterknoben | 5:f3ddc3a7b292 | 26 | //"n" is the amount off samples over which the mean value is calculated |
peterknoben | 5:f3ddc3a7b292 | 27 | //"ipnut" is the inputsignal (filtered analog in) |
peterknoben | 4:a79f9f3a9e40 | 28 | float SignalNumber::getmeanLeft(const int n, float input){ |
peterknoben | 4:a79f9f3a9e40 | 29 | emg0_filtered[count1] = input; |
peterknoben | 4:a79f9f3a9e40 | 30 | count1++; |
peterknoben | 4:a79f9f3a9e40 | 31 | if (count1 == n){ |
peterknoben | 4:a79f9f3a9e40 | 32 | count1 = 0; |
peterknoben | 4:a79f9f3a9e40 | 33 | } |
peterknoben | 4:a79f9f3a9e40 | 34 | float sum_math = 0.0; |
peterknoben | 4:a79f9f3a9e40 | 35 | for (int m=0 ; m<n ; m++ ){ |
peterknoben | 4:a79f9f3a9e40 | 36 | sum_math = sum_math + emg0_filtered[m]; |
peterknoben | 4:a79f9f3a9e40 | 37 | } |
peterknoben | 4:a79f9f3a9e40 | 38 | float mean_math = sum_math/n; |
peterknoben | 4:a79f9f3a9e40 | 39 | return mean_math; |
peterknoben | 4:a79f9f3a9e40 | 40 | } |
peterknoben | 5:f3ddc3a7b292 | 41 | |
peterknoben | 7:893503895342 | 42 | //------------------------------------------------------------------------------ |
peterknoben | 5:f3ddc3a7b292 | 43 | //Determine the Right mean value |
peterknoben | 5:f3ddc3a7b292 | 44 | //"n" is the amount off samples over which the mean value is calculated |
peterknoben | 5:f3ddc3a7b292 | 45 | //"ipnut" is the inputsignal (filtered analog in) |
peterknoben | 4:a79f9f3a9e40 | 46 | float SignalNumber::getmeanRight(const int n, float input){ |
peterknoben | 4:a79f9f3a9e40 | 47 | emg2_filtered[count2] = input; |
peterknoben | 4:a79f9f3a9e40 | 48 | count2++; |
peterknoben | 4:a79f9f3a9e40 | 49 | if (count2 == n){ |
peterknoben | 4:a79f9f3a9e40 | 50 | count2 = 0; |
peterknoben | 4:a79f9f3a9e40 | 51 | } |
peterknoben | 4:a79f9f3a9e40 | 52 | float sum_math = 0.0; |
peterknoben | 4:a79f9f3a9e40 | 53 | for (int m=0 ; m<n ; m++ ){ |
peterknoben | 4:a79f9f3a9e40 | 54 | sum_math = sum_math + emg2_filtered[m]; |
peterknoben | 4:a79f9f3a9e40 | 55 | } |
peterknoben | 4:a79f9f3a9e40 | 56 | float mean_math = sum_math/n; |
peterknoben | 4:a79f9f3a9e40 | 57 | return mean_math; |
peterknoben | 4:a79f9f3a9e40 | 58 | } |
peterknoben | 5:f3ddc3a7b292 | 59 | |
peterknoben | 7:893503895342 | 60 | //------------------------------------------------------------------------------ |
peterknoben | 5:f3ddc3a7b292 | 61 | // Determine the offset by calculating the mean value for "n" samples |
peterknoben | 0:5f8dee4d4b09 | 62 | float SignalNumber::calibrate(const int n, float input){ |
peterknoben | 4:a79f9f3a9e40 | 63 | float offset = getmeanLeft(n, input); |
peterknoben | 0:5f8dee4d4b09 | 64 | return offset; |
peterknoben | 0:5f8dee4d4b09 | 65 | } |
peterknoben | 0:5f8dee4d4b09 | 66 | |
peterknoben | 7:893503895342 | 67 | //------------------------------------------------------------------------------ |
peterknoben | 7:893503895342 | 68 | // |
peterknoben | 7:893503895342 | 69 | int SignalNumber::getspeedLeft(const int n, const int action, float input){ |
peterknoben | 5:f3ddc3a7b292 | 70 | meanL = getmeanLeft(n, input); |
peterknoben | 5:f3ddc3a7b292 | 71 | //Check first case, standstill |
peterknoben | 7:893503895342 | 72 | if( meanL < MinTreshold ) { |
peterknoben | 5:f3ddc3a7b292 | 73 | if (county < action){ |
peterknoben | 5:f3ddc3a7b292 | 74 | meanL = getmeanLeft(n, input); |
peterknoben | 7:893503895342 | 75 | if(meanL < MinTreshold){ |
peterknoben | 4:a79f9f3a9e40 | 76 | county++; |
peterknoben | 4:a79f9f3a9e40 | 77 | } |
peterknoben | 4:a79f9f3a9e40 | 78 | else{ |
peterknoben | 5:f3ddc3a7b292 | 79 | county = 0; |
peterknoben | 7:893503895342 | 80 | SpeedL = 0; |
peterknoben | 4:a79f9f3a9e40 | 81 | } |
peterknoben | 4:a79f9f3a9e40 | 82 | } |
peterknoben | 4:a79f9f3a9e40 | 83 | else{ |
peterknoben | 7:893503895342 | 84 | SpeedL = 0; |
peterknoben | 5:f3ddc3a7b292 | 85 | county = 0; |
peterknoben | 4:a79f9f3a9e40 | 86 | } |
peterknoben | 4:a79f9f3a9e40 | 87 | } |
peterknoben | 5:f3ddc3a7b292 | 88 | //Check second case, move slow |
peterknoben | 7:893503895342 | 89 | else if(meanL >= MinTreshold and meanL < MaxTreshold){ |
peterknoben | 4:a79f9f3a9e40 | 90 | if (county <action){ |
peterknoben | 4:a79f9f3a9e40 | 91 | meanL= getmeanLeft(n, input); |
peterknoben | 7:893503895342 | 92 | if(meanL >= MinTreshold and meanL < MaxTreshold){ |
peterknoben | 4:a79f9f3a9e40 | 93 | county++; |
peterknoben | 4:a79f9f3a9e40 | 94 | } |
peterknoben | 4:a79f9f3a9e40 | 95 | else{ |
peterknoben | 4:a79f9f3a9e40 | 96 | county=0; |
peterknoben | 7:893503895342 | 97 | SpeedL=0; |
peterknoben | 4:a79f9f3a9e40 | 98 | } |
peterknoben | 4:a79f9f3a9e40 | 99 | } |
peterknoben | 4:a79f9f3a9e40 | 100 | else{ |
peterknoben | 7:893503895342 | 101 | SpeedL = 1; |
peterknoben | 4:a79f9f3a9e40 | 102 | county=0; |
peterknoben | 4:a79f9f3a9e40 | 103 | } |
peterknoben | 4:a79f9f3a9e40 | 104 | } |
peterknoben | 5:f3ddc3a7b292 | 105 | //Check third case, move fast |
peterknoben | 7:893503895342 | 106 | else if( meanL>=MaxTreshold) { |
peterknoben | 4:a79f9f3a9e40 | 107 | if (county <action){ |
peterknoben | 4:a79f9f3a9e40 | 108 | meanL= getmeanLeft(n, input); |
peterknoben | 7:893503895342 | 109 | if(meanL>=MaxTreshold){ |
peterknoben | 4:a79f9f3a9e40 | 110 | county++; |
peterknoben | 4:a79f9f3a9e40 | 111 | } |
peterknoben | 4:a79f9f3a9e40 | 112 | else{ |
peterknoben | 4:a79f9f3a9e40 | 113 | county=0; |
peterknoben | 7:893503895342 | 114 | SpeedL=0; |
peterknoben | 4:a79f9f3a9e40 | 115 | } |
peterknoben | 4:a79f9f3a9e40 | 116 | } |
peterknoben | 4:a79f9f3a9e40 | 117 | else{ |
peterknoben | 7:893503895342 | 118 | SpeedL = 2; |
peterknoben | 4:a79f9f3a9e40 | 119 | county=0; |
peterknoben | 4:a79f9f3a9e40 | 120 | } |
peterknoben | 4:a79f9f3a9e40 | 121 | } |
peterknoben | 5:f3ddc3a7b292 | 122 | //If not working output zero |
peterknoben | 4:a79f9f3a9e40 | 123 | else{ |
peterknoben | 4:a79f9f3a9e40 | 124 | county=0; |
peterknoben | 7:893503895342 | 125 | SpeedL =0; |
peterknoben | 4:a79f9f3a9e40 | 126 | } |
peterknoben | 7:893503895342 | 127 | return SpeedL; |
peterknoben | 4:a79f9f3a9e40 | 128 | } |
peterknoben | 4:a79f9f3a9e40 | 129 | |
peterknoben | 7:893503895342 | 130 | //------------------------------------------------------------------------------ |
peterknoben | 7:893503895342 | 131 | int SignalNumber::getspeedRight(const int n, const int action, float input){ |
peterknoben | 4:a79f9f3a9e40 | 132 | meanR= getmeanRight(n, input); |
peterknoben | 5:f3ddc3a7b292 | 133 | //Check first case, standstill |
peterknoben | 7:893503895342 | 134 | if( meanR< MinTreshold ) { |
peterknoben | 7:893503895342 | 135 | if (countx < action){ |
peterknoben | 4:a79f9f3a9e40 | 136 | meanR= getmeanRight(n, input); |
peterknoben | 7:893503895342 | 137 | if(meanR< MinTreshold){ |
peterknoben | 4:a79f9f3a9e40 | 138 | countx++; |
peterknoben | 4:a79f9f3a9e40 | 139 | } |
peterknoben | 4:a79f9f3a9e40 | 140 | else{ |
peterknoben | 4:a79f9f3a9e40 | 141 | countx=0; |
peterknoben | 7:893503895342 | 142 | SpeedR=0; |
peterknoben | 4:a79f9f3a9e40 | 143 | } |
peterknoben | 4:a79f9f3a9e40 | 144 | } |
peterknoben | 4:a79f9f3a9e40 | 145 | else{ |
peterknoben | 7:893503895342 | 146 | SpeedR = 0; |
peterknoben | 4:a79f9f3a9e40 | 147 | countx=0; |
peterknoben | 4:a79f9f3a9e40 | 148 | } |
peterknoben | 4:a79f9f3a9e40 | 149 | } |
peterknoben | 5:f3ddc3a7b292 | 150 | //Check second case, move slow |
peterknoben | 7:893503895342 | 151 | else if(meanR >= MinTreshold and meanR < MaxTreshold){ |
peterknoben | 4:a79f9f3a9e40 | 152 | if (countx <action){ |
peterknoben | 4:a79f9f3a9e40 | 153 | meanR= getmeanRight(n, input); |
peterknoben | 7:893503895342 | 154 | if(meanR >= MinTreshold and meanR < MaxTreshold){ |
peterknoben | 4:a79f9f3a9e40 | 155 | countx++; |
peterknoben | 4:a79f9f3a9e40 | 156 | } |
peterknoben | 4:a79f9f3a9e40 | 157 | else{ |
peterknoben | 4:a79f9f3a9e40 | 158 | countx=0; |
peterknoben | 7:893503895342 | 159 | SpeedR=0; |
peterknoben | 4:a79f9f3a9e40 | 160 | } |
peterknoben | 4:a79f9f3a9e40 | 161 | } |
peterknoben | 4:a79f9f3a9e40 | 162 | else{ |
peterknoben | 7:893503895342 | 163 | SpeedR = 1; |
peterknoben | 4:a79f9f3a9e40 | 164 | countx=0; |
peterknoben | 4:a79f9f3a9e40 | 165 | } |
peterknoben | 4:a79f9f3a9e40 | 166 | } |
peterknoben | 5:f3ddc3a7b292 | 167 | //Check third case, move fast |
peterknoben | 7:893503895342 | 168 | else if( meanR >=MaxTreshold ) { |
peterknoben | 4:a79f9f3a9e40 | 169 | if (countx <action){ |
peterknoben | 4:a79f9f3a9e40 | 170 | meanR= getmeanRight(n, input); |
peterknoben | 7:893503895342 | 171 | if( meanR >=MaxTreshold ){ |
peterknoben | 4:a79f9f3a9e40 | 172 | countx++; |
peterknoben | 4:a79f9f3a9e40 | 173 | } |
peterknoben | 4:a79f9f3a9e40 | 174 | else{ |
peterknoben | 4:a79f9f3a9e40 | 175 | countx=0; |
peterknoben | 7:893503895342 | 176 | SpeedR=0; |
peterknoben | 4:a79f9f3a9e40 | 177 | } |
peterknoben | 4:a79f9f3a9e40 | 178 | } |
peterknoben | 4:a79f9f3a9e40 | 179 | else{ |
peterknoben | 7:893503895342 | 180 | SpeedR = 2; |
peterknoben | 4:a79f9f3a9e40 | 181 | countx=0; |
peterknoben | 4:a79f9f3a9e40 | 182 | } |
peterknoben | 4:a79f9f3a9e40 | 183 | } |
peterknoben | 5:f3ddc3a7b292 | 184 | //If not working output zero |
peterknoben | 4:a79f9f3a9e40 | 185 | else{ |
peterknoben | 4:a79f9f3a9e40 | 186 | countx=0; |
peterknoben | 7:893503895342 | 187 | SpeedR =0; |
peterknoben | 4:a79f9f3a9e40 | 188 | } |
peterknoben | 7:893503895342 | 189 | return SpeedR; |
peterknoben | 7:893503895342 | 190 | } |