Peter Knoben / Mbed 2 deprecated Signalnumber

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder HIDScope biquadFilter mbed

main.cpp@0:9c203fdb48e0, 2017-10-26 (annotated)

Committer:
peterknoben
Date:
Thu Oct 26 09:01:33 2017 +0000
Revision:
0:9c203fdb48e0
Mean value en signalnumber bepalen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peterknoben 0:9c203fdb48e0 1 #include "mbed.h"
peterknoben 0:9c203fdb48e0 2 #include "encoder.h"
peterknoben 0:9c203fdb48e0 3 #include "BiQuad.h"
peterknoben 0:9c203fdb48e0 4 #include "HIDScope.h"
peterknoben 0:9c203fdb48e0 5 #include <complex>
peterknoben 0:9c203fdb48e0 6 #include <iostream>
peterknoben 0:9c203fdb48e0 7
peterknoben 0:9c203fdb48e0 8
peterknoben 0:9c203fdb48e0 9 AnalogIn potmeter1(A3);
peterknoben 0:9c203fdb48e0 10 AnalogIn emg0( A0 );
peterknoben 0:9c203fdb48e0 11 AnalogIn emg1( A1 );
peterknoben 0:9c203fdb48e0 12
peterknoben 0:9c203fdb48e0 13 HIDScope scope( 3 );
peterknoben 0:9c203fdb48e0 14 DigitalOut led(LED1);
peterknoben 0:9c203fdb48e0 15
peterknoben 0:9c203fdb48e0 16 Ticker sample_timer;
peterknoben 0:9c203fdb48e0 17 Ticker MyTickerMean;
peterknoben 0:9c203fdb48e0 18
peterknoben 0:9c203fdb48e0 19 //Constants------------------------------------
peterknoben 0:9c203fdb48e0 20 const int n = 10;
peterknoben 0:9c203fdb48e0 21 //const int n2 = 10;
peterknoben 0:9c203fdb48e0 22 float emg0_filtered[n] = {};
peterknoben 0:9c203fdb48e0 23 //float mean_filtered[n2] = {};
peterknoben 0:9c203fdb48e0 24 int count = 0;
peterknoben 0:9c203fdb48e0 25 int count2 = 0;
peterknoben 0:9c203fdb48e0 26 const float controller_TS = 0.1;
peterknoben 0:9c203fdb48e0 27 float mean = 0.0;
peterknoben 0:9c203fdb48e0 28 float sum = 0.0;
peterknoben 0:9c203fdb48e0 29
peterknoben 0:9c203fdb48e0 30
peterknoben 0:9c203fdb48e0 31 //Constants EMG switch
peterknoben 0:9c203fdb48e0 32 const float LeftFastmin=0.075;
peterknoben 0:9c203fdb48e0 33 const float LeftFastmax=0.15;
peterknoben 0:9c203fdb48e0 34 const float LeftSlowmin=0.2;
peterknoben 0:9c203fdb48e0 35 const float LeftSlowmax=0.35;
peterknoben 0:9c203fdb48e0 36 const float RightSlowmin=0.4;
peterknoben 0:9c203fdb48e0 37 const float RightSlowmax=0.65;
peterknoben 0:9c203fdb48e0 38 const float RightFastmin=0.7;
peterknoben 0:9c203fdb48e0 39 const float RightFastmax=1.5;
peterknoben 0:9c203fdb48e0 40 int SignalNumber = 0;
peterknoben 0:9c203fdb48e0 41 int numberOfSet = 0;
peterknoben 0:9c203fdb48e0 42 const int action =50;
peterknoben 0:9c203fdb48e0 43
peterknoben 0:9c203fdb48e0 44
peterknoben 0:9c203fdb48e0 45 //Functions---------------------------------------
peterknoben 0:9c203fdb48e0 46 float read_pot(double potmeter){
peterknoben 0:9c203fdb48e0 47 float pot_value = potmeter;
peterknoben 0:9c203fdb48e0 48 return pot_value;
peterknoben 0:9c203fdb48e0 49 }
peterknoben 0:9c203fdb48e0 50 /*
peterknoben 0:9c203fdb48e0 51 float get_sum(float array[], const int n){
peterknoben 0:9c203fdb48e0 52 float sum_math = 0.0;
peterknoben 0:9c203fdb48e0 53 for (int m=0 ; m<n ; m++ ){
peterknoben 0:9c203fdb48e0 54 sum_math = sum_math + array[m];
peterknoben 0:9c203fdb48e0 55 }
peterknoben 0:9c203fdb48e0 56 return sum_math;
peterknoben 0:9c203fdb48e0 57 }*/
peterknoben 0:9c203fdb48e0 58
peterknoben 0:9c203fdb48e0 59 float get_sum(float array[], const int n){
peterknoben 0:9c203fdb48e0 60 float sum_math = 0.0;
peterknoben 0:9c203fdb48e0 61 for (int m=0 ; m<n ; m++ ){
peterknoben 0:9c203fdb48e0 62 sum_math = sum_math + array[m];
peterknoben 0:9c203fdb48e0 63 }
peterknoben 0:9c203fdb48e0 64 return sum_math;
peterknoben 0:9c203fdb48e0 65 }
peterknoben 0:9c203fdb48e0 66
peterknoben 0:9c203fdb48e0 67
peterknoben 0:9c203fdb48e0 68
peterknoben 0:9c203fdb48e0 69 float get_mean_value(){
peterknoben 0:9c203fdb48e0 70 emg0_filtered[count] = read_pot(potmeter1);
peterknoben 0:9c203fdb48e0 71 count++;
peterknoben 0:9c203fdb48e0 72 if (count == n){
peterknoben 0:9c203fdb48e0 73 count = 0;
peterknoben 0:9c203fdb48e0 74 }
peterknoben 0:9c203fdb48e0 75 float mean_math = get_sum(emg0_filtered,n)/n;
peterknoben 0:9c203fdb48e0 76 return mean_math;
peterknoben 0:9c203fdb48e0 77 }
peterknoben 0:9c203fdb48e0 78
peterknoben 0:9c203fdb48e0 79 //----------------------------------------------------
peterknoben 0:9c203fdb48e0 80
peterknoben 0:9c203fdb48e0 81
peterknoben 0:9c203fdb48e0 82 void get_Signal_number(){
peterknoben 0:9c203fdb48e0 83 mean = get_mean_value();
peterknoben 0:9c203fdb48e0 84 //Check first case
peterknoben 0:9c203fdb48e0 85 if( mean < LeftFastmin ) {
peterknoben 0:9c203fdb48e0 86 if (count2 <action){
peterknoben 0:9c203fdb48e0 87 mean = get_mean_value();
peterknoben 0:9c203fdb48e0 88 if(mean < LeftFastmin){
peterknoben 0:9c203fdb48e0 89 count2++;
peterknoben 0:9c203fdb48e0 90 }
peterknoben 0:9c203fdb48e0 91 else{
peterknoben 0:9c203fdb48e0 92 count2=0;
peterknoben 0:9c203fdb48e0 93 SignalNumber=0;
peterknoben 0:9c203fdb48e0 94 }
peterknoben 0:9c203fdb48e0 95 }
peterknoben 0:9c203fdb48e0 96 else{
peterknoben 0:9c203fdb48e0 97 SignalNumber = 0;
peterknoben 0:9c203fdb48e0 98 count2=0;
peterknoben 0:9c203fdb48e0 99 }
peterknoben 0:9c203fdb48e0 100 }
peterknoben 0:9c203fdb48e0 101 //Check second case
peterknoben 0:9c203fdb48e0 102 else if(mean <= LeftFastmax and mean > LeftFastmin){
peterknoben 0:9c203fdb48e0 103 if (count2 <action){
peterknoben 0:9c203fdb48e0 104 mean = get_mean_value();
peterknoben 0:9c203fdb48e0 105 if(mean <=LeftFastmax and mean>LeftFastmin){
peterknoben 0:9c203fdb48e0 106 count2++;
peterknoben 0:9c203fdb48e0 107 }
peterknoben 0:9c203fdb48e0 108 else{
peterknoben 0:9c203fdb48e0 109 count2=0;
peterknoben 0:9c203fdb48e0 110 SignalNumber=0;
peterknoben 0:9c203fdb48e0 111 }
peterknoben 0:9c203fdb48e0 112 }
peterknoben 0:9c203fdb48e0 113 else{
peterknoben 0:9c203fdb48e0 114 SignalNumber = 1;
peterknoben 0:9c203fdb48e0 115 count2=0;
peterknoben 0:9c203fdb48e0 116 }
peterknoben 0:9c203fdb48e0 117 }
peterknoben 0:9c203fdb48e0 118 else if( mean <=LeftSlowmax and mean>LeftSlowmin) {
peterknoben 0:9c203fdb48e0 119 if (count2 <action){
peterknoben 0:9c203fdb48e0 120 mean = get_mean_value();
peterknoben 0:9c203fdb48e0 121 if(mean <=LeftSlowmax and mean>LeftSlowmin){
peterknoben 0:9c203fdb48e0 122 count2++;
peterknoben 0:9c203fdb48e0 123 }
peterknoben 0:9c203fdb48e0 124 else{
peterknoben 0:9c203fdb48e0 125 count2=0;
peterknoben 0:9c203fdb48e0 126 SignalNumber=0;
peterknoben 0:9c203fdb48e0 127 }
peterknoben 0:9c203fdb48e0 128 }
peterknoben 0:9c203fdb48e0 129 else{
peterknoben 0:9c203fdb48e0 130 SignalNumber = 2;
peterknoben 0:9c203fdb48e0 131 count2=0;
peterknoben 0:9c203fdb48e0 132 }
peterknoben 0:9c203fdb48e0 133 }
peterknoben 0:9c203fdb48e0 134 else if( mean <=RightSlowmax and mean>RightSlowmin) {
peterknoben 0:9c203fdb48e0 135 if (count2 <action){
peterknoben 0:9c203fdb48e0 136 mean = get_mean_value();
peterknoben 0:9c203fdb48e0 137 if(mean <=RightSlowmax and mean>RightSlowmin){
peterknoben 0:9c203fdb48e0 138 count2++;
peterknoben 0:9c203fdb48e0 139 }
peterknoben 0:9c203fdb48e0 140 else{
peterknoben 0:9c203fdb48e0 141 count2=0;
peterknoben 0:9c203fdb48e0 142 SignalNumber=0;
peterknoben 0:9c203fdb48e0 143 }
peterknoben 0:9c203fdb48e0 144 }
peterknoben 0:9c203fdb48e0 145 else{
peterknoben 0:9c203fdb48e0 146 SignalNumber = 3;
peterknoben 0:9c203fdb48e0 147 count2=0;
peterknoben 0:9c203fdb48e0 148 }
peterknoben 0:9c203fdb48e0 149 }
peterknoben 0:9c203fdb48e0 150 else if( mean <=RightFastmax and mean>RightFastmin ) {
peterknoben 0:9c203fdb48e0 151 if (count2 <action){
peterknoben 0:9c203fdb48e0 152 mean = get_mean_value();
peterknoben 0:9c203fdb48e0 153 if(mean <=RightFastmax and mean>RightFastmin){
peterknoben 0:9c203fdb48e0 154 count2++;
peterknoben 0:9c203fdb48e0 155 }
peterknoben 0:9c203fdb48e0 156 else{
peterknoben 0:9c203fdb48e0 157 count2=0;
peterknoben 0:9c203fdb48e0 158 SignalNumber=0;
peterknoben 0:9c203fdb48e0 159 }
peterknoben 0:9c203fdb48e0 160 }
peterknoben 0:9c203fdb48e0 161 else{
peterknoben 0:9c203fdb48e0 162 SignalNumber = 4;
peterknoben 0:9c203fdb48e0 163 count2=0;
peterknoben 0:9c203fdb48e0 164 }
peterknoben 0:9c203fdb48e0 165 }
peterknoben 0:9c203fdb48e0 166 else{
peterknoben 0:9c203fdb48e0 167 count2=0;
peterknoben 0:9c203fdb48e0 168 SignalNumber =0;
peterknoben 0:9c203fdb48e0 169 }
peterknoben 0:9c203fdb48e0 170 }
peterknoben 0:9c203fdb48e0 171
peterknoben 0:9c203fdb48e0 172 /*switch(SignalNumber)
peterknoben 0:9c203fdb48e0 173 {
peterknoben 0:9c203fdb48e0 174 case 0: //Standstill
peterknoben 0:9c203fdb48e0 175 motor1Direction=1;
peterknoben 0:9c203fdb48e0 176 motor1Speed=0;
peterknoben 0:9c203fdb48e0 177 printf("Motor 1 is standing still\n");
peterknoben 0:9c203fdb48e0 178 break;
peterknoben 0:9c203fdb48e0 179 case 1: //Move Left fast
peterknoben 0:9c203fdb48e0 180 motor1Direction=1;
peterknoben 0:9c203fdb48e0 181 motor1Speed=1;
peterknoben 0:9c203fdb48e0 182 printf("Motor 1 is moving left fast\n");
peterknoben 0:9c203fdb48e0 183 break;
peterknoben 0:9c203fdb48e0 184 case 2: //Move Left slow (movement speed is half of that of fast movement)
peterknoben 0:9c203fdb48e0 185 motor1Direction=1;
peterknoben 0:9c203fdb48e0 186 motor1Speed=0.5;
peterknoben 0:9c203fdb48e0 187 printf("Motor 1 is moving left slow\n");
peterknoben 0:9c203fdb48e0 188 break;
peterknoben 0:9c203fdb48e0 189 case 3: //Move right slow
peterknoben 0:9c203fdb48e0 190 motor1Direction=0;
peterknoben 0:9c203fdb48e0 191 motor1Speed=0.5;
peterknoben 0:9c203fdb48e0 192 printf("Motor 1 is moving right slow\n");
peterknoben 0:9c203fdb48e0 193 break;
peterknoben 0:9c203fdb48e0 194 case 4: //Move right fast
peterknoben 0:9c203fdb48e0 195 motor1Direction=0;
peterknoben 0:9c203fdb48e0 196 motor1Speed=1;
peterknoben 0:9c203fdb48e0 197 printf("Motor 1 is moving right fast\n");
peterknoben 0:9c203fdb48e0 198 break;
peterknoben 0:9c203fdb48e0 199 default : //if something is wrong, standstill
peterknoben 0:9c203fdb48e0 200 motor1Direction=1;
peterknoben 0:9c203fdb48e0 201 motor1Speed=0;
peterknoben 0:9c203fdb48e0 202 printf("Motor 1 is standing still, something went wrong\n")
peterknoben 0:9c203fdb48e0 203
peterknoben 0:9c203fdb48e0 204 }*/
peterknoben 0:9c203fdb48e0 205
peterknoben 0:9c203fdb48e0 206
peterknoben 0:9c203fdb48e0 207
peterknoben 0:9c203fdb48e0 208
peterknoben 0:9c203fdb48e0 209 ///-----------------------------------------------------------------------
peterknoben 0:9c203fdb48e0 210 /** Sample function
peterknoben 0:9c203fdb48e0 211 * this function samples the emg and sends it to HIDScope
peterknoben 0:9c203fdb48e0 212 **/
peterknoben 0:9c203fdb48e0 213
peterknoben 0:9c203fdb48e0 214 // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
peterknoben 0:9c203fdb48e0 215 BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
peterknoben 0:9c203fdb48e0 216 BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
peterknoben 0:9c203fdb48e0 217 BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
peterknoben 0:9c203fdb48e0 218 BiQuadChain BiQuad_filter;
peterknoben 0:9c203fdb48e0 219 float Signal;
peterknoben 0:9c203fdb48e0 220 float Signal_filtered;
peterknoben 0:9c203fdb48e0 221
peterknoben 0:9c203fdb48e0 222 // Add the biquads to the chain
peterknoben 0:9c203fdb48e0 223
peterknoben 0:9c203fdb48e0 224
peterknoben 0:9c203fdb48e0 225 void sample()
peterknoben 0:9c203fdb48e0 226 {
peterknoben 0:9c203fdb48e0 227 Signal=(emg0+emg1)/2;
peterknoben 0:9c203fdb48e0 228 Signal_filtered= BiQuad_filter.step(Signal);
peterknoben 0:9c203fdb48e0 229 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
peterknoben 0:9c203fdb48e0 230 scope.set(0, emg0.read() );
peterknoben 0:9c203fdb48e0 231 scope.set(1, emg1.read() );
peterknoben 0:9c203fdb48e0 232 scope.set(2, Signal_filtered);
peterknoben 0:9c203fdb48e0 233 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
peterknoben 0:9c203fdb48e0 234 * Ensure that enough channels are available (HIDScope scope( 2 ))
peterknoben 0:9c203fdb48e0 235 * Finally, send all channels to the PC at once */
peterknoben 0:9c203fdb48e0 236 scope.send();
peterknoben 0:9c203fdb48e0 237 /* To indicate that the function is working, the LED is toggled */
peterknoben 0:9c203fdb48e0 238 led = !led;
peterknoben 0:9c203fdb48e0 239 }
peterknoben 0:9c203fdb48e0 240
peterknoben 0:9c203fdb48e0 241
peterknoben 0:9c203fdb48e0 242
peterknoben 0:9c203fdb48e0 243
peterknoben 0:9c203fdb48e0 244
peterknoben 0:9c203fdb48e0 245
peterknoben 0:9c203fdb48e0 246
peterknoben 0:9c203fdb48e0 247
peterknoben 0:9c203fdb48e0 248 int main()
peterknoben 0:9c203fdb48e0 249 {
peterknoben 0:9c203fdb48e0 250 BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
peterknoben 0:9c203fdb48e0 251
peterknoben 0:9c203fdb48e0 252 MyTickerMean.attach(&get_Signal_number, controller_TS);
peterknoben 0:9c203fdb48e0 253 sample_timer.attach(&sample, 0.02);
peterknoben 0:9c203fdb48e0 254
peterknoben 0:9c203fdb48e0 255 while (true) {}
peterknoben 0:9c203fdb48e0 256
peterknoben 0:9c203fdb48e0 257 }
peterknoben 0:9c203fdb48e0 258