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