Peter Knoben / SignalNumber2

ksf

Dependents:   kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds Robot_control

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?

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