Peter Knoben / SignalNumber2

ksf

Dependents:   kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds Robot_control

signalnumber.cpp@3:f8d57796d69b, 2017-11-01 (annotated)

Committer:
peterknoben
Date:
Wed Nov 01 14:11:14 2017 +0000
Revision:
3:f8d57796d69b
Parent:
2:1a677b57ce81
Child:
4:a79f9f3a9e40
sef

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
peterknoben 1:15543c143a63 6 const int n = 400; //Window size for the mean value
peterknoben 2:1a677b57ce81 7 float emg_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 2:1a677b57ce81 22 const float RightFastmax=10.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 2:1a677b57ce81 47 emg_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 2:1a677b57ce81 52 float mean_math = getsum(emg_filtered,n)/n;
peterknoben 0:5f8dee4d4b09 53 return mean_math;
peterknoben 0:5f8dee4d4b09 54 }
peterknoben 2:1a677b57ce81 55 /*
peterknoben 2:1a677b57ce81 56 float SignalNumber::getmean(float array[], const int n, float input){
peterknoben 2:1a677b57ce81 57 float sum_math = 0;
peterknoben 2:1a677b57ce81 58
peterknoben 2:1a677b57ce81 59
peterknoben 2:1a677b57ce81 60 }
peterknoben 2:1a677b57ce81 61 */
peterknoben 0:5f8dee4d4b09 62 float SignalNumber::calibrate(const int n, float input){
peterknoben 0:5f8dee4d4b09 63 float offset = getmean(n, input);
peterknoben 0:5f8dee4d4b09 64 return offset;
peterknoben 0:5f8dee4d4b09 65 }
peterknoben 0:5f8dee4d4b09 66
peterknoben 0:5f8dee4d4b09 67 //Determine the right signal number
peterknoben 0:5f8dee4d4b09 68 /*
peterknoben 0:5f8dee4d4b09 69 There will be checked in which range the mean value lies. If the value lies
peterknoben 0:5f8dee4d4b09 70 within this range for "action" samples the switch will be changed to the right
peterknoben 0:5f8dee4d4b09 71 mode. If the mean value changes outside of the region the switch will be set to
peterknoben 0:5f8dee4d4b09 72 0, this is the resting state. Also the counter will be reset.
peterknoben 2:1a677b57ce81 73 n is the amount of samples in the window
peterknoben 2:1a677b57ce81 74 action is the amount of same mean values
peterknoben 0:5f8dee4d4b09 75 */
peterknoben 3:f8d57796d69b 76 int SignalNumber::getnumber(const int n, const int action, float input){
peterknoben 3:f8d57796d69b 77 mean = getmean(n, input);
peterknoben 0:5f8dee4d4b09 78 //Check first case
peterknoben 0:5f8dee4d4b09 79 if( mean < LeftFastmin ) {
peterknoben 0:5f8dee4d4b09 80 if (count2 <action){
peterknoben 3:f8d57796d69b 81 mean = getmean(n, input);
peterknoben 0:5f8dee4d4b09 82 if(mean < LeftFastmin){
peterknoben 0:5f8dee4d4b09 83 count2++;
peterknoben 0:5f8dee4d4b09 84 }
peterknoben 0:5f8dee4d4b09 85 else{
peterknoben 0:5f8dee4d4b09 86 count2=0;
peterknoben 0:5f8dee4d4b09 87 Number=0;
peterknoben 0:5f8dee4d4b09 88 }
peterknoben 0:5f8dee4d4b09 89 }
peterknoben 0:5f8dee4d4b09 90 else{
peterknoben 0:5f8dee4d4b09 91 Number = 0;
peterknoben 0:5f8dee4d4b09 92 count2=0;
peterknoben 0:5f8dee4d4b09 93 }
peterknoben 0:5f8dee4d4b09 94 }
peterknoben 0:5f8dee4d4b09 95 //Check second case
peterknoben 0:5f8dee4d4b09 96 else if(mean <= LeftFastmax and mean > LeftFastmin){
peterknoben 0:5f8dee4d4b09 97 if (count2 <action){
peterknoben 3:f8d57796d69b 98 mean = getmean(n, input);
peterknoben 0:5f8dee4d4b09 99 if(mean <=LeftFastmax and mean>LeftFastmin){
peterknoben 0:5f8dee4d4b09 100 count2++;
peterknoben 0:5f8dee4d4b09 101 }
peterknoben 0:5f8dee4d4b09 102 else{
peterknoben 0:5f8dee4d4b09 103 count2=0;
peterknoben 0:5f8dee4d4b09 104 Number=0;
peterknoben 0:5f8dee4d4b09 105 }
peterknoben 0:5f8dee4d4b09 106 }
peterknoben 0:5f8dee4d4b09 107 else{
peterknoben 0:5f8dee4d4b09 108 Number = 1;
peterknoben 0:5f8dee4d4b09 109 count2=0;
peterknoben 0:5f8dee4d4b09 110 }
peterknoben 0:5f8dee4d4b09 111 }
peterknoben 0:5f8dee4d4b09 112 else if( mean <=LeftSlowmax and mean>LeftSlowmin) {
peterknoben 0:5f8dee4d4b09 113 if (count2 <action){
peterknoben 3:f8d57796d69b 114 mean = getmean(n, input);
peterknoben 0:5f8dee4d4b09 115 if(mean <=LeftSlowmax and mean>LeftSlowmin){
peterknoben 0:5f8dee4d4b09 116 count2++;
peterknoben 0:5f8dee4d4b09 117 }
peterknoben 0:5f8dee4d4b09 118 else{
peterknoben 0:5f8dee4d4b09 119 count2=0;
peterknoben 0:5f8dee4d4b09 120 Number=0;
peterknoben 0:5f8dee4d4b09 121 }
peterknoben 0:5f8dee4d4b09 122 }
peterknoben 0:5f8dee4d4b09 123 else{
peterknoben 0:5f8dee4d4b09 124 Number = 2;
peterknoben 0:5f8dee4d4b09 125 count2=0;
peterknoben 0:5f8dee4d4b09 126 }
peterknoben 0:5f8dee4d4b09 127 }
peterknoben 0:5f8dee4d4b09 128 else if( mean <=RightSlowmax and mean>RightSlowmin) {
peterknoben 0:5f8dee4d4b09 129 if (count2 <action){
peterknoben 3:f8d57796d69b 130 mean = getmean(n, input);
peterknoben 0:5f8dee4d4b09 131 if(mean <=RightSlowmax and mean>RightSlowmin){
peterknoben 0:5f8dee4d4b09 132 count2++;
peterknoben 0:5f8dee4d4b09 133 }
peterknoben 0:5f8dee4d4b09 134 else{
peterknoben 0:5f8dee4d4b09 135 count2=0;
peterknoben 0:5f8dee4d4b09 136 Number=0;
peterknoben 0:5f8dee4d4b09 137 }
peterknoben 0:5f8dee4d4b09 138 }
peterknoben 0:5f8dee4d4b09 139 else{
peterknoben 0:5f8dee4d4b09 140 Number = 3;
peterknoben 0:5f8dee4d4b09 141 count2=0;
peterknoben 0:5f8dee4d4b09 142 }
peterknoben 0:5f8dee4d4b09 143 }
peterknoben 0:5f8dee4d4b09 144 else if( mean <=RightFastmax and mean>RightFastmin ) {
peterknoben 0:5f8dee4d4b09 145 if (count2 <action){
peterknoben 3:f8d57796d69b 146 mean = getmean(n, input);
peterknoben 0:5f8dee4d4b09 147 if(mean <=RightFastmax and mean>RightFastmin){
peterknoben 0:5f8dee4d4b09 148 count2++;
peterknoben 0:5f8dee4d4b09 149 }
peterknoben 0:5f8dee4d4b09 150 else{
peterknoben 0:5f8dee4d4b09 151 count2=0;
peterknoben 0:5f8dee4d4b09 152 Number=0;
peterknoben 0:5f8dee4d4b09 153 }
peterknoben 0:5f8dee4d4b09 154 }
peterknoben 0:5f8dee4d4b09 155 else{
peterknoben 0:5f8dee4d4b09 156 Number = 4;
peterknoben 0:5f8dee4d4b09 157 count2=0;
peterknoben 0:5f8dee4d4b09 158 }
peterknoben 0:5f8dee4d4b09 159 }
peterknoben 0:5f8dee4d4b09 160 else{
peterknoben 0:5f8dee4d4b09 161 count2=0;
peterknoben 0:5f8dee4d4b09 162 Number =0;
peterknoben 0:5f8dee4d4b09 163 }
peterknoben 0:5f8dee4d4b09 164 return Number;
peterknoben 0:5f8dee4d4b09 165 }
peterknoben 0:5f8dee4d4b09 166
peterknoben 0:5f8dee4d4b09 167
peterknoben 1:15543c143a63 168 // In progress
peterknoben 3:f8d57796d69b 169 int SignalNumber::getmode(const int n, const int action, float input){
peterknoben 1:15543c143a63 170 int mode;
peterknoben 3:f8d57796d69b 171 mean = getmean(n, input);
peterknoben 1:15543c143a63 172 //Check first case
peterknoben 1:15543c143a63 173 if( mean < LeftFastmin ) {
peterknoben 1:15543c143a63 174 if (count2 <action){
peterknoben 3:f8d57796d69b 175 mean = getmean(n, input);
peterknoben 1:15543c143a63 176 if(mean < LeftFastmin){
peterknoben 1:15543c143a63 177 count2++;
peterknoben 1:15543c143a63 178 }
peterknoben 1:15543c143a63 179 else{
peterknoben 1:15543c143a63 180 count2=0;
peterknoben 1:15543c143a63 181 Number=0;
peterknoben 1:15543c143a63 182 }
peterknoben 1:15543c143a63 183 }
peterknoben 1:15543c143a63 184 else{
peterknoben 1:15543c143a63 185 Number = 0;
peterknoben 1:15543c143a63 186 count2=0;
peterknoben 1:15543c143a63 187 }
peterknoben 1:15543c143a63 188 }
peterknoben 1:15543c143a63 189 return mode;
peterknoben 1:15543c143a63 190 }
peterknoben 1:15543c143a63 191
peterknoben 1:15543c143a63 192