Peter Knoben / SignalNumberClean

Working, Clean

Dependents:   ShowItv2 ShowIt_robot_control

Fork of SignalNumber by Dustin Berendsen

signalnumber.cpp@6:7f6e2d293154, 2017-11-02 (annotated)

Committer:
DBerendsen
Date:
Thu Nov 02 10:17:26 2017 +0000
Revision:
6:7f6e2d293154
Parent:
5:f3ddc3a7b292
Child:
7:893503895342
Eerste keer super mooi werkend;

Who changed what in which revision?

UserRevisionLine numberNew 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
DBerendsen 6:7f6e2d293154 6 const int n = 500; //Window size for the mean value
peterknoben 4:a79f9f3a9e40 7 float emg0_filtered[n] = {}, emg2_filtered[n] = {}, emg4_filtered[n]={};
peterknoben 5:f3ddc3a7b292 8 int count1 = 0, count2 = 0, count3 = 0; //Counters for the mean values
peterknoben 5:f3ddc3a7b292 9 int countx = 0, county = 0.0, countz = 0.0; //Counters for the signalnumbers
peterknoben 5:f3ddc3a7b292 10 float meanL = 0.0, meanR = 0.0, meanM = 0.0; //Internal function calculation variables
peterknoben 0:5f8dee4d4b09 11
peterknoben 0:5f8dee4d4b09 12 //Constants EMG switch
peterknoben 4:a79f9f3a9e40 13 const float LeftMin = 0.20;
peterknoben 4:a79f9f3a9e40 14 const float LeftMax = 0.50;
peterknoben 4:a79f9f3a9e40 15 int NumberL = 0, NumberR =0;
peterknoben 0:5f8dee4d4b09 16
peterknoben 0:5f8dee4d4b09 17
peterknoben 0:5f8dee4d4b09 18 SignalNumber::SignalNumber(void)
peterknoben 0:5f8dee4d4b09 19 {
peterknoben 0:5f8dee4d4b09 20
peterknoben 0:5f8dee4d4b09 21 }
peterknoben 0:5f8dee4d4b09 22
peterknoben 5:f3ddc3a7b292 23 //Determine the Left mean value
peterknoben 5:f3ddc3a7b292 24 //"n" is the amount off samples over which the mean value is calculated
peterknoben 5:f3ddc3a7b292 25 //"ipnut" is the inputsignal (filtered analog in)
peterknoben 4:a79f9f3a9e40 26 float SignalNumber::getmeanLeft(const int n, float input){
peterknoben 4:a79f9f3a9e40 27 emg0_filtered[count1] = input;
peterknoben 4:a79f9f3a9e40 28 count1++;
peterknoben 4:a79f9f3a9e40 29 if (count1 == n){
peterknoben 4:a79f9f3a9e40 30 count1 = 0;
peterknoben 4:a79f9f3a9e40 31 }
peterknoben 4:a79f9f3a9e40 32 float sum_math = 0.0;
peterknoben 4:a79f9f3a9e40 33 for (int m=0 ; m<n ; m++ ){
peterknoben 4:a79f9f3a9e40 34 sum_math = sum_math + emg0_filtered[m];
peterknoben 4:a79f9f3a9e40 35 }
peterknoben 4:a79f9f3a9e40 36 float mean_math = sum_math/n;
peterknoben 4:a79f9f3a9e40 37 return mean_math;
peterknoben 4:a79f9f3a9e40 38 }
peterknoben 5:f3ddc3a7b292 39
peterknoben 5:f3ddc3a7b292 40 //Determine the Right mean value
peterknoben 5:f3ddc3a7b292 41 //"n" is the amount off samples over which the mean value is calculated
peterknoben 5:f3ddc3a7b292 42 //"ipnut" is the inputsignal (filtered analog in)
peterknoben 4:a79f9f3a9e40 43 float SignalNumber::getmeanRight(const int n, float input){
peterknoben 4:a79f9f3a9e40 44 emg2_filtered[count2] = input;
peterknoben 4:a79f9f3a9e40 45 count2++;
peterknoben 4:a79f9f3a9e40 46 if (count2 == n){
peterknoben 4:a79f9f3a9e40 47 count2 = 0;
peterknoben 4:a79f9f3a9e40 48 }
peterknoben 4:a79f9f3a9e40 49 float sum_math = 0.0;
peterknoben 4:a79f9f3a9e40 50 for (int m=0 ; m<n ; m++ ){
peterknoben 4:a79f9f3a9e40 51 sum_math = sum_math + emg2_filtered[m];
peterknoben 4:a79f9f3a9e40 52 }
peterknoben 4:a79f9f3a9e40 53 float mean_math = sum_math/n;
peterknoben 4:a79f9f3a9e40 54 return mean_math;
peterknoben 4:a79f9f3a9e40 55 }
peterknoben 5:f3ddc3a7b292 56
peterknoben 5:f3ddc3a7b292 57 /*
peterknoben 5:f3ddc3a7b292 58 //Determine the Mode mean value
peterknoben 5:f3ddc3a7b292 59 //"n" is the amount off samples over which the mean value is calculated
peterknoben 5:f3ddc3a7b292 60 //"ipnut" is the inputsignal (filtered analog in)
peterknoben 4:a79f9f3a9e40 61 float SignalNumber::getmeanMode(const int n, float input){
peterknoben 4:a79f9f3a9e40 62 emg4_filtered[count3] = input;
peterknoben 4:a79f9f3a9e40 63 count3++;
peterknoben 4:a79f9f3a9e40 64 if (count3 == n){
peterknoben 4:a79f9f3a9e40 65 count3 = 0;
peterknoben 4:a79f9f3a9e40 66 }
peterknoben 4:a79f9f3a9e40 67 float sum_math = 0.0;
peterknoben 4:a79f9f3a9e40 68 for (int m=0 ; m<n ; m++ ){
peterknoben 4:a79f9f3a9e40 69 sum_math = sum_math + emg4_filtered[m];
peterknoben 4:a79f9f3a9e40 70 }
peterknoben 4:a79f9f3a9e40 71 float mean_math = sum_math/n;
peterknoben 0:5f8dee4d4b09 72 return mean_math;
peterknoben 0:5f8dee4d4b09 73 }
peterknoben 5:f3ddc3a7b292 74 */
peterknoben 5:f3ddc3a7b292 75
peterknoben 5:f3ddc3a7b292 76 // Determine the offset by calculating the mean value for "n" samples
peterknoben 0:5f8dee4d4b09 77 float SignalNumber::calibrate(const int n, float input){
peterknoben 4:a79f9f3a9e40 78 float offset = getmeanLeft(n, input);
peterknoben 0:5f8dee4d4b09 79 return offset;
peterknoben 0:5f8dee4d4b09 80 }
peterknoben 0:5f8dee4d4b09 81
peterknoben 4:a79f9f3a9e40 82 int SignalNumber::getnumberLeft(const int n, const int action, float input){
peterknoben 5:f3ddc3a7b292 83 meanL = getmeanLeft(n, input);
peterknoben 5:f3ddc3a7b292 84 //Check first case, standstill
peterknoben 5:f3ddc3a7b292 85 if( meanL < LeftMin ) {
peterknoben 5:f3ddc3a7b292 86 if (county < action){
peterknoben 5:f3ddc3a7b292 87 meanL = getmeanLeft(n, input);
peterknoben 5:f3ddc3a7b292 88 if(meanL < LeftMin){
peterknoben 4:a79f9f3a9e40 89 county++;
peterknoben 4:a79f9f3a9e40 90 }
peterknoben 4:a79f9f3a9e40 91 else{
peterknoben 5:f3ddc3a7b292 92 county = 0;
peterknoben 5:f3ddc3a7b292 93 NumberL = 0;
peterknoben 4:a79f9f3a9e40 94 }
peterknoben 4:a79f9f3a9e40 95 }
peterknoben 4:a79f9f3a9e40 96 else{
peterknoben 4:a79f9f3a9e40 97 NumberL = 0;
peterknoben 5:f3ddc3a7b292 98 county = 0;
peterknoben 4:a79f9f3a9e40 99 }
peterknoben 4:a79f9f3a9e40 100 }
peterknoben 5:f3ddc3a7b292 101 //Check second case, move slow
peterknoben 4:a79f9f3a9e40 102 else if(meanL >= LeftMin and meanL < LeftMax){
peterknoben 4:a79f9f3a9e40 103 if (county <action){
peterknoben 4:a79f9f3a9e40 104 meanL= getmeanLeft(n, input);
peterknoben 4:a79f9f3a9e40 105 if(meanL >= LeftMin and meanL < LeftMax){
peterknoben 4:a79f9f3a9e40 106 county++;
peterknoben 4:a79f9f3a9e40 107 }
peterknoben 4:a79f9f3a9e40 108 else{
peterknoben 4:a79f9f3a9e40 109 county=0;
peterknoben 4:a79f9f3a9e40 110 NumberL=0;
peterknoben 4:a79f9f3a9e40 111 }
peterknoben 4:a79f9f3a9e40 112 }
peterknoben 4:a79f9f3a9e40 113 else{
peterknoben 4:a79f9f3a9e40 114 NumberL = 1;
peterknoben 4:a79f9f3a9e40 115 county=0;
peterknoben 4:a79f9f3a9e40 116 }
peterknoben 4:a79f9f3a9e40 117 }
peterknoben 5:f3ddc3a7b292 118 //Check third case, move fast
peterknoben 4:a79f9f3a9e40 119 else if( meanL>=LeftMax) {
peterknoben 4:a79f9f3a9e40 120 if (county <action){
peterknoben 4:a79f9f3a9e40 121 meanL= getmeanLeft(n, input);
peterknoben 4:a79f9f3a9e40 122 if(meanL>=LeftMax){
peterknoben 4:a79f9f3a9e40 123 county++;
peterknoben 4:a79f9f3a9e40 124 }
peterknoben 4:a79f9f3a9e40 125 else{
peterknoben 4:a79f9f3a9e40 126 county=0;
peterknoben 4:a79f9f3a9e40 127 NumberL=0;
peterknoben 4:a79f9f3a9e40 128 }
peterknoben 4:a79f9f3a9e40 129 }
peterknoben 4:a79f9f3a9e40 130 else{
peterknoben 4:a79f9f3a9e40 131 NumberL = 2;
peterknoben 4:a79f9f3a9e40 132 county=0;
peterknoben 4:a79f9f3a9e40 133 }
peterknoben 4:a79f9f3a9e40 134 }
peterknoben 5:f3ddc3a7b292 135 //If not working output zero
peterknoben 4:a79f9f3a9e40 136 else{
peterknoben 4:a79f9f3a9e40 137 county=0;
peterknoben 4:a79f9f3a9e40 138 NumberL =0;
peterknoben 4:a79f9f3a9e40 139 }
peterknoben 4:a79f9f3a9e40 140 return NumberL;
peterknoben 4:a79f9f3a9e40 141 }
peterknoben 4:a79f9f3a9e40 142
peterknoben 4:a79f9f3a9e40 143 int SignalNumber::getnumberRight(const int n, const int action, float input){
peterknoben 4:a79f9f3a9e40 144 meanR= getmeanRight(n, input);
peterknoben 5:f3ddc3a7b292 145 //Check first case, standstill
peterknoben 4:a79f9f3a9e40 146 if( meanR< LeftMin ) {
peterknoben 4:a79f9f3a9e40 147 if (countx <action){
peterknoben 4:a79f9f3a9e40 148 meanR= getmeanRight(n, input);
peterknoben 4:a79f9f3a9e40 149 if(meanR< LeftMin){
peterknoben 4:a79f9f3a9e40 150 countx++;
peterknoben 4:a79f9f3a9e40 151 }
peterknoben 4:a79f9f3a9e40 152 else{
peterknoben 4:a79f9f3a9e40 153 countx=0;
peterknoben 4:a79f9f3a9e40 154 NumberR=0;
peterknoben 4:a79f9f3a9e40 155 }
peterknoben 4:a79f9f3a9e40 156 }
peterknoben 4:a79f9f3a9e40 157 else{
peterknoben 4:a79f9f3a9e40 158 NumberR = 0;
peterknoben 4:a79f9f3a9e40 159 countx=0;
peterknoben 4:a79f9f3a9e40 160 }
peterknoben 4:a79f9f3a9e40 161 }
peterknoben 5:f3ddc3a7b292 162 //Check second case, move slow
peterknoben 4:a79f9f3a9e40 163 else if(meanR >= LeftMin and meanR < LeftMax){
peterknoben 4:a79f9f3a9e40 164 if (countx <action){
peterknoben 4:a79f9f3a9e40 165 meanR= getmeanRight(n, input);
peterknoben 4:a79f9f3a9e40 166 if(meanR >= LeftMin and meanR < LeftMax){
peterknoben 4:a79f9f3a9e40 167 countx++;
peterknoben 4:a79f9f3a9e40 168 }
peterknoben 4:a79f9f3a9e40 169 else{
peterknoben 4:a79f9f3a9e40 170 countx=0;
peterknoben 4:a79f9f3a9e40 171 NumberR=0;
peterknoben 4:a79f9f3a9e40 172 }
peterknoben 4:a79f9f3a9e40 173 }
peterknoben 4:a79f9f3a9e40 174 else{
peterknoben 4:a79f9f3a9e40 175 NumberR = 1;
peterknoben 4:a79f9f3a9e40 176 countx=0;
peterknoben 4:a79f9f3a9e40 177 }
peterknoben 4:a79f9f3a9e40 178 }
peterknoben 5:f3ddc3a7b292 179 //Check third case, move fast
peterknoben 4:a79f9f3a9e40 180 else if( meanR >=LeftMax ) {
peterknoben 4:a79f9f3a9e40 181 if (countx <action){
peterknoben 4:a79f9f3a9e40 182 meanR= getmeanRight(n, input);
peterknoben 4:a79f9f3a9e40 183 if( meanR >=LeftMax ){
peterknoben 4:a79f9f3a9e40 184 countx++;
peterknoben 4:a79f9f3a9e40 185 }
peterknoben 4:a79f9f3a9e40 186 else{
peterknoben 4:a79f9f3a9e40 187 countx=0;
peterknoben 4:a79f9f3a9e40 188 NumberR=0;
peterknoben 4:a79f9f3a9e40 189 }
peterknoben 4:a79f9f3a9e40 190 }
peterknoben 4:a79f9f3a9e40 191 else{
peterknoben 4:a79f9f3a9e40 192 NumberR = 2;
peterknoben 4:a79f9f3a9e40 193 countx=0;
peterknoben 4:a79f9f3a9e40 194 }
peterknoben 4:a79f9f3a9e40 195 }
peterknoben 5:f3ddc3a7b292 196 //If not working output zero
peterknoben 4:a79f9f3a9e40 197 else{
peterknoben 4:a79f9f3a9e40 198 countx=0;
peterknoben 4:a79f9f3a9e40 199 NumberR =0;
peterknoben 4:a79f9f3a9e40 200 }
peterknoben 4:a79f9f3a9e40 201 return NumberR;
peterknoben 4:a79f9f3a9e40 202 }
peterknoben 4:a79f9f3a9e40 203
peterknoben 4:a79f9f3a9e40 204