Peter Knoben / SignalNumberClean

Working, Clean

Dependents:   ShowItv2 ShowIt_robot_control

Fork of SignalNumber by Dustin Berendsen

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?

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 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 }