ksf
Dependents: kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds Robot_control
signalnumber.cpp@1:15543c143a63, 2017-10-31 (annotated)
- Committer:
- peterknoben
- Date:
- Tue Oct 31 14:34:30 2017 +0000
- Revision:
- 1:15543c143a63
- Parent:
- 0:5f8dee4d4b09
- Child:
- 2:1a677b57ce81
asfdf
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 | 0:5f8dee4d4b09 | 5 | //Constants for mean value |
peterknoben | 1:15543c143a63 | 6 | const int n = 400; //Window size for the mean value |
peterknoben | 0:5f8dee4d4b09 | 7 | float emg0_filtered[n] = {}; |
peterknoben | 0:5f8dee4d4b09 | 8 | int count = 0; |
peterknoben | 0:5f8dee4d4b09 | 9 | int count2 = 0; |
peterknoben | 0:5f8dee4d4b09 | 10 | float mean = 0.0; |
peterknoben | 0:5f8dee4d4b09 | 11 | float sum = 0.0; |
peterknoben | 0:5f8dee4d4b09 | 12 | |
peterknoben | 0:5f8dee4d4b09 | 13 | |
peterknoben | 0:5f8dee4d4b09 | 14 | //Constants EMG switch |
peterknoben | 0:5f8dee4d4b09 | 15 | const float LeftFastmin=0.075; |
peterknoben | 0:5f8dee4d4b09 | 16 | const float LeftFastmax=0.15; |
peterknoben | 0:5f8dee4d4b09 | 17 | const float LeftSlowmin=0.2; |
peterknoben | 0:5f8dee4d4b09 | 18 | const float LeftSlowmax=0.35; |
peterknoben | 0:5f8dee4d4b09 | 19 | const float RightSlowmin=0.4; |
peterknoben | 0:5f8dee4d4b09 | 20 | const float RightSlowmax=0.65; |
peterknoben | 0:5f8dee4d4b09 | 21 | const float RightFastmin=0.7; |
peterknoben | 0:5f8dee4d4b09 | 22 | const float RightFastmax=1.5; |
peterknoben | 0:5f8dee4d4b09 | 23 | int Number = 0; |
peterknoben | 0:5f8dee4d4b09 | 24 | //const int action =50; |
peterknoben | 0:5f8dee4d4b09 | 25 | |
peterknoben | 0:5f8dee4d4b09 | 26 | |
peterknoben | 0:5f8dee4d4b09 | 27 | SignalNumber::SignalNumber(void) |
peterknoben | 0:5f8dee4d4b09 | 28 | { |
peterknoben | 0:5f8dee4d4b09 | 29 | |
peterknoben | 0:5f8dee4d4b09 | 30 | } |
peterknoben | 0:5f8dee4d4b09 | 31 | |
peterknoben | 0:5f8dee4d4b09 | 32 | |
peterknoben | 0:5f8dee4d4b09 | 33 | |
peterknoben | 0:5f8dee4d4b09 | 34 | //Determine the sum of the last "n" amount of numbers. |
peterknoben | 0:5f8dee4d4b09 | 35 | float SignalNumber::getsum(float array[], const int n){ |
peterknoben | 0:5f8dee4d4b09 | 36 | float sum_math = 0.0; |
peterknoben | 0:5f8dee4d4b09 | 37 | for (int m=0 ; m<n ; m++ ){ |
peterknoben | 0:5f8dee4d4b09 | 38 | sum_math = sum_math + array[m]; |
peterknoben | 0:5f8dee4d4b09 | 39 | } |
peterknoben | 0:5f8dee4d4b09 | 40 | return sum_math; |
peterknoben | 0:5f8dee4d4b09 | 41 | } |
peterknoben | 0:5f8dee4d4b09 | 42 | |
peterknoben | 0:5f8dee4d4b09 | 43 | |
peterknoben | 0:5f8dee4d4b09 | 44 | |
peterknoben | 0:5f8dee4d4b09 | 45 | // Determine the mean value of the last "n" amount of numbers. |
peterknoben | 0:5f8dee4d4b09 | 46 | float SignalNumber::getmean(const int n, float input){ |
peterknoben | 0:5f8dee4d4b09 | 47 | emg0_filtered[count] = input; |
peterknoben | 0:5f8dee4d4b09 | 48 | count++; |
peterknoben | 0:5f8dee4d4b09 | 49 | if (count == n){ |
peterknoben | 0:5f8dee4d4b09 | 50 | count = 0; |
peterknoben | 0:5f8dee4d4b09 | 51 | } |
peterknoben | 0:5f8dee4d4b09 | 52 | float mean_math = getsum(emg0_filtered,n)/n; |
peterknoben | 0:5f8dee4d4b09 | 53 | return mean_math; |
peterknoben | 0:5f8dee4d4b09 | 54 | } |
peterknoben | 0:5f8dee4d4b09 | 55 | |
peterknoben | 0:5f8dee4d4b09 | 56 | |
peterknoben | 0:5f8dee4d4b09 | 57 | float SignalNumber::calibrate(const int n, float input){ |
peterknoben | 0:5f8dee4d4b09 | 58 | float offset = getmean(n, input); |
peterknoben | 0:5f8dee4d4b09 | 59 | return offset; |
peterknoben | 0:5f8dee4d4b09 | 60 | } |
peterknoben | 0:5f8dee4d4b09 | 61 | |
peterknoben | 0:5f8dee4d4b09 | 62 | |
peterknoben | 0:5f8dee4d4b09 | 63 | |
peterknoben | 0:5f8dee4d4b09 | 64 | //Determine the right signal number |
peterknoben | 0:5f8dee4d4b09 | 65 | /* |
peterknoben | 0:5f8dee4d4b09 | 66 | There will be checked in which range the mean value lies. If the value lies |
peterknoben | 0:5f8dee4d4b09 | 67 | within this range for "action" samples the switch will be changed to the right |
peterknoben | 0:5f8dee4d4b09 | 68 | mode. If the mean value changes outside of the region the switch will be set to |
peterknoben | 0:5f8dee4d4b09 | 69 | 0, this is the resting state. Also the counter will be reset. |
peterknoben | 0:5f8dee4d4b09 | 70 | */ |
peterknoben | 1:15543c143a63 | 71 | int SignalNumber::getnumber(const int n, const int action, double input, float k){ |
peterknoben | 1:15543c143a63 | 72 | mean = getmean(n, input)*k; |
peterknoben | 0:5f8dee4d4b09 | 73 | //Check first case |
peterknoben | 0:5f8dee4d4b09 | 74 | if( mean < LeftFastmin ) { |
peterknoben | 0:5f8dee4d4b09 | 75 | if (count2 <action){ |
peterknoben | 1:15543c143a63 | 76 | mean = getmean(n, input)*k; |
peterknoben | 0:5f8dee4d4b09 | 77 | if(mean < LeftFastmin){ |
peterknoben | 0:5f8dee4d4b09 | 78 | count2++; |
peterknoben | 0:5f8dee4d4b09 | 79 | } |
peterknoben | 0:5f8dee4d4b09 | 80 | else{ |
peterknoben | 0:5f8dee4d4b09 | 81 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 82 | Number=0; |
peterknoben | 0:5f8dee4d4b09 | 83 | } |
peterknoben | 0:5f8dee4d4b09 | 84 | } |
peterknoben | 0:5f8dee4d4b09 | 85 | else{ |
peterknoben | 0:5f8dee4d4b09 | 86 | Number = 0; |
peterknoben | 0:5f8dee4d4b09 | 87 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 88 | } |
peterknoben | 0:5f8dee4d4b09 | 89 | } |
peterknoben | 0:5f8dee4d4b09 | 90 | //Check second case |
peterknoben | 0:5f8dee4d4b09 | 91 | else if(mean <= LeftFastmax and mean > LeftFastmin){ |
peterknoben | 0:5f8dee4d4b09 | 92 | if (count2 <action){ |
peterknoben | 1:15543c143a63 | 93 | mean = getmean(n, input)*k; |
peterknoben | 0:5f8dee4d4b09 | 94 | if(mean <=LeftFastmax and mean>LeftFastmin){ |
peterknoben | 0:5f8dee4d4b09 | 95 | count2++; |
peterknoben | 0:5f8dee4d4b09 | 96 | } |
peterknoben | 0:5f8dee4d4b09 | 97 | else{ |
peterknoben | 0:5f8dee4d4b09 | 98 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 99 | Number=0; |
peterknoben | 0:5f8dee4d4b09 | 100 | } |
peterknoben | 0:5f8dee4d4b09 | 101 | } |
peterknoben | 0:5f8dee4d4b09 | 102 | else{ |
peterknoben | 0:5f8dee4d4b09 | 103 | Number = 1; |
peterknoben | 0:5f8dee4d4b09 | 104 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 105 | } |
peterknoben | 0:5f8dee4d4b09 | 106 | } |
peterknoben | 0:5f8dee4d4b09 | 107 | else if( mean <=LeftSlowmax and mean>LeftSlowmin) { |
peterknoben | 0:5f8dee4d4b09 | 108 | if (count2 <action){ |
peterknoben | 1:15543c143a63 | 109 | mean = getmean(n, input)*k; |
peterknoben | 0:5f8dee4d4b09 | 110 | if(mean <=LeftSlowmax and mean>LeftSlowmin){ |
peterknoben | 0:5f8dee4d4b09 | 111 | count2++; |
peterknoben | 0:5f8dee4d4b09 | 112 | } |
peterknoben | 0:5f8dee4d4b09 | 113 | else{ |
peterknoben | 0:5f8dee4d4b09 | 114 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 115 | Number=0; |
peterknoben | 0:5f8dee4d4b09 | 116 | } |
peterknoben | 0:5f8dee4d4b09 | 117 | } |
peterknoben | 0:5f8dee4d4b09 | 118 | else{ |
peterknoben | 0:5f8dee4d4b09 | 119 | Number = 2; |
peterknoben | 0:5f8dee4d4b09 | 120 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 121 | } |
peterknoben | 0:5f8dee4d4b09 | 122 | } |
peterknoben | 0:5f8dee4d4b09 | 123 | else if( mean <=RightSlowmax and mean>RightSlowmin) { |
peterknoben | 0:5f8dee4d4b09 | 124 | if (count2 <action){ |
peterknoben | 1:15543c143a63 | 125 | mean = getmean(n, input)*k; |
peterknoben | 0:5f8dee4d4b09 | 126 | if(mean <=RightSlowmax and mean>RightSlowmin){ |
peterknoben | 0:5f8dee4d4b09 | 127 | count2++; |
peterknoben | 0:5f8dee4d4b09 | 128 | } |
peterknoben | 0:5f8dee4d4b09 | 129 | else{ |
peterknoben | 0:5f8dee4d4b09 | 130 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 131 | Number=0; |
peterknoben | 0:5f8dee4d4b09 | 132 | } |
peterknoben | 0:5f8dee4d4b09 | 133 | } |
peterknoben | 0:5f8dee4d4b09 | 134 | else{ |
peterknoben | 0:5f8dee4d4b09 | 135 | Number = 3; |
peterknoben | 0:5f8dee4d4b09 | 136 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 137 | } |
peterknoben | 0:5f8dee4d4b09 | 138 | } |
peterknoben | 0:5f8dee4d4b09 | 139 | else if( mean <=RightFastmax and mean>RightFastmin ) { |
peterknoben | 0:5f8dee4d4b09 | 140 | if (count2 <action){ |
peterknoben | 1:15543c143a63 | 141 | mean = getmean(n, input)*k; |
peterknoben | 0:5f8dee4d4b09 | 142 | if(mean <=RightFastmax and mean>RightFastmin){ |
peterknoben | 0:5f8dee4d4b09 | 143 | count2++; |
peterknoben | 0:5f8dee4d4b09 | 144 | } |
peterknoben | 0:5f8dee4d4b09 | 145 | else{ |
peterknoben | 0:5f8dee4d4b09 | 146 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 147 | Number=0; |
peterknoben | 0:5f8dee4d4b09 | 148 | } |
peterknoben | 0:5f8dee4d4b09 | 149 | } |
peterknoben | 0:5f8dee4d4b09 | 150 | else{ |
peterknoben | 0:5f8dee4d4b09 | 151 | Number = 4; |
peterknoben | 0:5f8dee4d4b09 | 152 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 153 | } |
peterknoben | 0:5f8dee4d4b09 | 154 | } |
peterknoben | 0:5f8dee4d4b09 | 155 | else{ |
peterknoben | 0:5f8dee4d4b09 | 156 | count2=0; |
peterknoben | 0:5f8dee4d4b09 | 157 | Number =0; |
peterknoben | 0:5f8dee4d4b09 | 158 | } |
peterknoben | 0:5f8dee4d4b09 | 159 | return Number; |
peterknoben | 0:5f8dee4d4b09 | 160 | } |
peterknoben | 0:5f8dee4d4b09 | 161 | |
peterknoben | 0:5f8dee4d4b09 | 162 | |
peterknoben | 1:15543c143a63 | 163 | // In progress |
peterknoben | 1:15543c143a63 | 164 | int SignalNumber::getmode(const int n, const int action, double input, float k){ |
peterknoben | 1:15543c143a63 | 165 | int mode; |
peterknoben | 1:15543c143a63 | 166 | mean = getmean(n, input)*k; |
peterknoben | 1:15543c143a63 | 167 | //Check first case |
peterknoben | 1:15543c143a63 | 168 | if( mean < LeftFastmin ) { |
peterknoben | 1:15543c143a63 | 169 | if (count2 <action){ |
peterknoben | 1:15543c143a63 | 170 | mean = getmean(n, input)*k; |
peterknoben | 1:15543c143a63 | 171 | if(mean < LeftFastmin){ |
peterknoben | 1:15543c143a63 | 172 | count2++; |
peterknoben | 1:15543c143a63 | 173 | } |
peterknoben | 1:15543c143a63 | 174 | else{ |
peterknoben | 1:15543c143a63 | 175 | count2=0; |
peterknoben | 1:15543c143a63 | 176 | Number=0; |
peterknoben | 1:15543c143a63 | 177 | } |
peterknoben | 1:15543c143a63 | 178 | } |
peterknoben | 1:15543c143a63 | 179 | else{ |
peterknoben | 1:15543c143a63 | 180 | Number = 0; |
peterknoben | 1:15543c143a63 | 181 | count2=0; |
peterknoben | 1:15543c143a63 | 182 | } |
peterknoben | 1:15543c143a63 | 183 | } |
peterknoben | 1:15543c143a63 | 184 | return mode; |
peterknoben | 1:15543c143a63 | 185 | } |
peterknoben | 1:15543c143a63 | 186 | |
peterknoben | 1:15543c143a63 | 187 |