Working, Clean
Dependents: ShowItv2 ShowIt_robot_control
Fork of SignalNumber by
signalnumber.cpp
- Committer:
- peterknoben
- Date:
- 2017-11-02
- Revision:
- 7:893503895342
- Parent:
- 6:7f6e2d293154
- Child:
- 8:9098c6b1897b
File content as of revision 7:893503895342:
#include "signalnumber.h" #include "mbed.h" //------------------------------------------------------------------------------ //Constants for mean value const int n = 500; //Window size for the mean value float emg0_filtered[n] = {}, emg2_filtered[n] = {}; int count1 = 0, count2 = 0; //Counters for the mean values int countx = 0, county = 0.0; //Counters for the signalnumbers float meanL = 0.0, meanR = 0.0; //Internal function calculation variables //Constants EMG switch const float MinTreshold = 0.20; const float MaxTreshold = 0.50; int SpeedL = 0, SpeedR =0; SignalNumber::SignalNumber(void) { } //------------------------------------------------------------------------------ //Determine the Left mean value //"n" is the amount off samples over which the mean value is calculated //"ipnut" is the inputsignal (filtered analog in) float SignalNumber::getmeanLeft(const int n, float input){ emg0_filtered[count1] = input; count1++; if (count1 == n){ count1 = 0; } float sum_math = 0.0; for (int m=0 ; m<n ; m++ ){ sum_math = sum_math + emg0_filtered[m]; } float mean_math = sum_math/n; return mean_math; } //------------------------------------------------------------------------------ //Determine the Right mean value //"n" is the amount off samples over which the mean value is calculated //"ipnut" is the inputsignal (filtered analog in) float SignalNumber::getmeanRight(const int n, float input){ emg2_filtered[count2] = input; count2++; if (count2 == n){ count2 = 0; } float sum_math = 0.0; for (int m=0 ; m<n ; m++ ){ sum_math = sum_math + emg2_filtered[m]; } float mean_math = sum_math/n; return mean_math; } //------------------------------------------------------------------------------ // Determine the offset by calculating the mean value for "n" samples float SignalNumber::calibrate(const int n, float input){ float offset = getmeanLeft(n, input); return offset; } //------------------------------------------------------------------------------ // int SignalNumber::getspeedLeft(const int n, const int action, float input){ meanL = getmeanLeft(n, input); //Check first case, standstill if( meanL < MinTreshold ) { if (county < action){ meanL = getmeanLeft(n, input); if(meanL < MinTreshold){ county++; } else{ county = 0; SpeedL = 0; } } else{ SpeedL = 0; county = 0; } } //Check second case, move slow else if(meanL >= MinTreshold and meanL < MaxTreshold){ if (county <action){ meanL= getmeanLeft(n, input); if(meanL >= MinTreshold and meanL < MaxTreshold){ county++; } else{ county=0; SpeedL=0; } } else{ SpeedL = 1; county=0; } } //Check third case, move fast else if( meanL>=MaxTreshold) { if (county <action){ meanL= getmeanLeft(n, input); if(meanL>=MaxTreshold){ county++; } else{ county=0; SpeedL=0; } } else{ SpeedL = 2; county=0; } } //If not working output zero else{ county=0; SpeedL =0; } return SpeedL; } //------------------------------------------------------------------------------ int SignalNumber::getspeedRight(const int n, const int action, float input){ meanR= getmeanRight(n, input); //Check first case, standstill if( meanR< MinTreshold ) { if (countx < action){ meanR= getmeanRight(n, input); if(meanR< MinTreshold){ countx++; } else{ countx=0; SpeedR=0; } } else{ SpeedR = 0; countx=0; } } //Check second case, move slow else if(meanR >= MinTreshold and meanR < MaxTreshold){ if (countx <action){ meanR= getmeanRight(n, input); if(meanR >= MinTreshold and meanR < MaxTreshold){ countx++; } else{ countx=0; SpeedR=0; } } else{ SpeedR = 1; countx=0; } } //Check third case, move fast else if( meanR >=MaxTreshold ) { if (countx <action){ meanR= getmeanRight(n, input); if( meanR >=MaxTreshold ){ countx++; } else{ countx=0; SpeedR=0; } } else{ SpeedR = 2; countx=0; } } //If not working output zero else{ countx=0; SpeedR =0; } return SpeedR; }