Biorobotics / Mbed 2 deprecated Biorobotics_testing

Yea it is working. done programinging for a while!

Dependencies:   HIDScope QEI mbed

main.cpp@0:0f08f98ae7f8, 2015-10-29 (annotated)

Committer:
yohoo15
Date:
Thu Oct 29 10:42:15 2015 +0000
Revision:
0:0f08f98ae7f8
Child:
1:267ff9a39ee4
Yea it is working. done programinging for a while!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yohoo15 0:0f08f98ae7f8 1 #include "mbed.h"
yohoo15 0:0f08f98ae7f8 2 //#include "read_filter_emg.h"
yohoo15 0:0f08f98ae7f8 3 //included for fabs() function
yohoo15 0:0f08f98ae7f8 4 #include <math.h>
yohoo15 0:0f08f98ae7f8 5 #include "HIDScope.h"
yohoo15 0:0f08f98ae7f8 6 #include <iostream>
yohoo15 0:0f08f98ae7f8 7 #include "QEI.h"
yohoo15 0:0f08f98ae7f8 8
yohoo15 0:0f08f98ae7f8 9 //DigitalOut led_green(LED_GREEN);
yohoo15 0:0f08f98ae7f8 10 //DigitalOut led_red(LED_RED);
yohoo15 0:0f08f98ae7f8 11
yohoo15 0:0f08f98ae7f8 12 Serial pc(USBTX, USBRX);
yohoo15 0:0f08f98ae7f8 13 AnalogIn analog_emg_left(A0);
yohoo15 0:0f08f98ae7f8 14 AnalogIn analog_emg_right(A1);
yohoo15 0:0f08f98ae7f8 15
yohoo15 0:0f08f98ae7f8 16 Ticker HIDScope_timer;
yohoo15 0:0f08f98ae7f8 17 Ticker Filteren_timer;
yohoo15 0:0f08f98ae7f8 18 Ticker aansturen;
yohoo15 0:0f08f98ae7f8 19
yohoo15 0:0f08f98ae7f8 20 HIDScope scope(4);
yohoo15 0:0f08f98ae7f8 21
yohoo15 0:0f08f98ae7f8 22 // defining flags
yohoo15 0:0f08f98ae7f8 23 volatile bool Flag_filteren = false;
yohoo15 0:0f08f98ae7f8 24 volatile bool Flag_HIDScope = false;
yohoo15 0:0f08f98ae7f8 25 volatile bool left_movement = false;
yohoo15 0:0f08f98ae7f8 26 volatile bool right_movement = false;
yohoo15 0:0f08f98ae7f8 27
yohoo15 0:0f08f98ae7f8 28 // making function flags.
yohoo15 0:0f08f98ae7f8 29 void Go_flag_filteren()
yohoo15 0:0f08f98ae7f8 30 {
yohoo15 0:0f08f98ae7f8 31 Flag_filteren = true;
yohoo15 0:0f08f98ae7f8 32 }
yohoo15 0:0f08f98ae7f8 33
yohoo15 0:0f08f98ae7f8 34 void Go_flag_HIDScope()
yohoo15 0:0f08f98ae7f8 35 {
yohoo15 0:0f08f98ae7f8 36 Flag_HIDScope = true;
yohoo15 0:0f08f98ae7f8 37 }
yohoo15 0:0f08f98ae7f8 38
yohoo15 0:0f08f98ae7f8 39
yohoo15 0:0f08f98ae7f8 40
yohoo15 0:0f08f98ae7f8 41 double input_left = 0 ;
yohoo15 0:0f08f98ae7f8 42 double input_right = 0 ;
yohoo15 0:0f08f98ae7f8 43 //double input = 0;
yohoo15 0:0f08f98ae7f8 44 double filter_signal_hid_left = 0;
yohoo15 0:0f08f98ae7f8 45 double filter_signal_hid_right = 0;
yohoo15 0:0f08f98ae7f8 46 //double input_right = 0;
yohoo15 0:0f08f98ae7f8 47 // defining threshold
yohoo15 0:0f08f98ae7f8 48 double high_threshold = 500;
yohoo15 0:0f08f98ae7f8 49 double low_threshold = 125;
yohoo15 0:0f08f98ae7f8 50
yohoo15 0:0f08f98ae7f8 51 //*** making the v for everything and conquer the world***
yohoo15 0:0f08f98ae7f8 52
yohoo15 0:0f08f98ae7f8 53 //for left
yohoo15 0:0f08f98ae7f8 54 //for Notchfilter
yohoo15 0:0f08f98ae7f8 55 double notch_v11=0;
yohoo15 0:0f08f98ae7f8 56 double notch_v21=0;
yohoo15 0:0f08f98ae7f8 57 double notch_v12=0;
yohoo15 0:0f08f98ae7f8 58 double notch_v22=0;
yohoo15 0:0f08f98ae7f8 59 double notch_v13=0;
yohoo15 0:0f08f98ae7f8 60 double notch_v23=0;
yohoo15 0:0f08f98ae7f8 61
yohoo15 0:0f08f98ae7f8 62 //for highpass filter
yohoo15 0:0f08f98ae7f8 63 double high_v11=0;
yohoo15 0:0f08f98ae7f8 64 double high_v21=0;
yohoo15 0:0f08f98ae7f8 65 double high_v12=0;
yohoo15 0:0f08f98ae7f8 66 double high_v22=0;
yohoo15 0:0f08f98ae7f8 67 double high_v13=0;
yohoo15 0:0f08f98ae7f8 68 double high_v23=0;
yohoo15 0:0f08f98ae7f8 69
yohoo15 0:0f08f98ae7f8 70 // for lowpasfilter
yohoo15 0:0f08f98ae7f8 71 double low_v11=0;
yohoo15 0:0f08f98ae7f8 72 double low_v21=0;
yohoo15 0:0f08f98ae7f8 73 double low_v12=0;
yohoo15 0:0f08f98ae7f8 74 double low_v22=0;
yohoo15 0:0f08f98ae7f8 75 double low_v13=0;
yohoo15 0:0f08f98ae7f8 76 double low_v23=0;
yohoo15 0:0f08f98ae7f8 77
yohoo15 0:0f08f98ae7f8 78 // for moving average
yohoo15 0:0f08f98ae7f8 79 double n1 = 0;
yohoo15 0:0f08f98ae7f8 80 double n2 = 0;
yohoo15 0:0f08f98ae7f8 81 double n3 = 0;
yohoo15 0:0f08f98ae7f8 82 double n4 = 0;
yohoo15 0:0f08f98ae7f8 83 double n5 = 0;
yohoo15 0:0f08f98ae7f8 84
yohoo15 0:0f08f98ae7f8 85 // for right
yohoo15 0:0f08f98ae7f8 86 //for Notchfilter
yohoo15 0:0f08f98ae7f8 87 double notch_v11_b=0;
yohoo15 0:0f08f98ae7f8 88 double notch_v21_b=0;
yohoo15 0:0f08f98ae7f8 89 double notch_v12_b=0;
yohoo15 0:0f08f98ae7f8 90 double notch_v22_b=0;
yohoo15 0:0f08f98ae7f8 91 double notch_v13_b=0;
yohoo15 0:0f08f98ae7f8 92 double notch_v23_b=0;
yohoo15 0:0f08f98ae7f8 93
yohoo15 0:0f08f98ae7f8 94 //for highpass filter
yohoo15 0:0f08f98ae7f8 95 double high_v11_b=0;
yohoo15 0:0f08f98ae7f8 96 double high_v21_b=0;
yohoo15 0:0f08f98ae7f8 97 double high_v12_b=0;
yohoo15 0:0f08f98ae7f8 98 double high_v22_b=0;
yohoo15 0:0f08f98ae7f8 99 double high_v13_b=0;
yohoo15 0:0f08f98ae7f8 100 double high_v23_b=0;
yohoo15 0:0f08f98ae7f8 101
yohoo15 0:0f08f98ae7f8 102 // for lowpasfilter
yohoo15 0:0f08f98ae7f8 103 double low_v11_b=0;
yohoo15 0:0f08f98ae7f8 104 double low_v21_b=0;
yohoo15 0:0f08f98ae7f8 105 double low_v12_b=0;
yohoo15 0:0f08f98ae7f8 106 double low_v22_b=0;
yohoo15 0:0f08f98ae7f8 107 double low_v13_b=0;
yohoo15 0:0f08f98ae7f8 108 double low_v23_b=0;
yohoo15 0:0f08f98ae7f8 109
yohoo15 0:0f08f98ae7f8 110 // for moving average
yohoo15 0:0f08f98ae7f8 111 double n1_b = 0;
yohoo15 0:0f08f98ae7f8 112 double n2_b = 0;
yohoo15 0:0f08f98ae7f8 113 double n3_b = 0;
yohoo15 0:0f08f98ae7f8 114 double n4_b = 0;
yohoo15 0:0f08f98ae7f8 115 double n5_b = 0;
yohoo15 0:0f08f98ae7f8 116
yohoo15 0:0f08f98ae7f8 117
yohoo15 0:0f08f98ae7f8 118 double filter_left;
yohoo15 0:0f08f98ae7f8 119 double filter_right;
yohoo15 0:0f08f98ae7f8 120
yohoo15 0:0f08f98ae7f8 121 //general biquad filter that can be called in all the filter functions
yohoo15 0:0f08f98ae7f8 122 double biquad(double u, double &v1, double &v2, const double a1,
yohoo15 0:0f08f98ae7f8 123 const double a2, const double b0, const double b1, const double b2)
yohoo15 0:0f08f98ae7f8 124 {
yohoo15 0:0f08f98ae7f8 125 double v = u - a1*v1 - a2*v2;
yohoo15 0:0f08f98ae7f8 126 double y = b0*v + b1*v1 + b2*v2;
yohoo15 0:0f08f98ae7f8 127 //values of v2 and v1 are updated, as they are passed by reference
yohoo15 0:0f08f98ae7f8 128 //they update globally
yohoo15 0:0f08f98ae7f8 129 v2 = v1;
yohoo15 0:0f08f98ae7f8 130 v1 = v;
yohoo15 0:0f08f98ae7f8 131 return y;
yohoo15 0:0f08f98ae7f8 132 }
yohoo15 0:0f08f98ae7f8 133
yohoo15 0:0f08f98ae7f8 134 double moving_average(double y, double &n1, double &n2, double &n3, double &n4, double &n5)
yohoo15 0:0f08f98ae7f8 135 {
yohoo15 0:0f08f98ae7f8 136 double average = (y + n1 + n2 +n3 + n4 + n5)/5;
yohoo15 0:0f08f98ae7f8 137 n5 = n4;
yohoo15 0:0f08f98ae7f8 138 n4 = n3;
yohoo15 0:0f08f98ae7f8 139 n3 = n2;
yohoo15 0:0f08f98ae7f8 140 n2 = n1;
yohoo15 0:0f08f98ae7f8 141 n1 = y;
yohoo15 0:0f08f98ae7f8 142
yohoo15 0:0f08f98ae7f8 143 return average;
yohoo15 0:0f08f98ae7f8 144 }
yohoo15 0:0f08f98ae7f8 145 /*
yohoo15 0:0f08f98ae7f8 146 double threshold(double signal, const double lowtreshold, const double hightreshold)
yohoo15 0:0f08f98ae7f8 147 {
yohoo15 0:0f08f98ae7f8 148 if (signal > hightreshold)
yohoo15 0:0f08f98ae7f8 149 left = true;
yohoo15 0:0f08f98ae7f8 150 else if (signal <lowtreshold)
yohoo15 0:0f08f98ae7f8 151 left = false;
yohoo15 0:0f08f98ae7f8 152 }
yohoo15 0:0f08f98ae7f8 153 */
yohoo15 0:0f08f98ae7f8 154 //Specifying filter coefficients highpass
yohoo15 0:0f08f98ae7f8 155
yohoo15 0:0f08f98ae7f8 156 /* notch filter with 3 cascaded biquads*/
yohoo15 0:0f08f98ae7f8 157 //first notch biquad
yohoo15 0:0f08f98ae7f8 158 const double notch1_a1 = -1.55951422433;
yohoo15 0:0f08f98ae7f8 159 const double notch1_a2 = 0.92705680308;
yohoo15 0:0f08f98ae7f8 160 const double notch1_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 161 const double notch1_b1 = -1.61854515325;
yohoo15 0:0f08f98ae7f8 162 const double notch1_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 163
yohoo15 0:0f08f98ae7f8 164 //second notch biquad
yohoo15 0:0f08f98ae7f8 165 const double notch2_a1 = -1.54767435801;
yohoo15 0:0f08f98ae7f8 166 const double notch2_a2 = 0.96124842048;
yohoo15 0:0f08f98ae7f8 167 const double notch2_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 168 const double notch2_b1 = -1.61854515325;
yohoo15 0:0f08f98ae7f8 169 const double notch2_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 170
yohoo15 0:0f08f98ae7f8 171 //third notch biquad
yohoo15 0:0f08f98ae7f8 172 const double notch3_a1 = -1.62600366964;
yohoo15 0:0f08f98ae7f8 173 const double notch3_a2 = 0.96453460373;
yohoo15 0:0f08f98ae7f8 174 const double notch3_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 175 const double notch3_b1 = -1.61854515325;
yohoo15 0:0f08f98ae7f8 176 const double notch3_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 177
yohoo15 0:0f08f98ae7f8 178 /* high pass filter consists of three cascaded biquads
yohoo15 0:0f08f98ae7f8 179 blow coefficients for those three biquads */
yohoo15 0:0f08f98ae7f8 180 //first high pass biquad
yohoo15 0:0f08f98ae7f8 181 const double highp1_a1 = -0.67538034389;
yohoo15 0:0f08f98ae7f8 182 const double highp1_a2 = 0.12769255668;
yohoo15 0:0f08f98ae7f8 183 const double highp1_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 184 const double highp1_b1 = -2.00000000000;
yohoo15 0:0f08f98ae7f8 185 const double highp1_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 186
yohoo15 0:0f08f98ae7f8 187 //second high pass biquad
yohoo15 0:0f08f98ae7f8 188 const double highp2_a1 = -0.76475499450;
yohoo15 0:0f08f98ae7f8 189 const double highp2_a2 = 0.27692273367;
yohoo15 0:0f08f98ae7f8 190 const double highp2_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 191 const double highp2_b1 = -2.00000000000;
yohoo15 0:0f08f98ae7f8 192 const double highp2_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 193
yohoo15 0:0f08f98ae7f8 194 //third high pass biquad
yohoo15 0:0f08f98ae7f8 195 const double highp3_a1 = -0.99216561242;
yohoo15 0:0f08f98ae7f8 196 const double highp3_a2 = 0.65663360837;
yohoo15 0:0f08f98ae7f8 197 const double highp3_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 198 const double highp3_b1 = -2.00000000000;
yohoo15 0:0f08f98ae7f8 199 const double highp3_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 200
yohoo15 0:0f08f98ae7f8 201 /* lowpass filter consists of three cascaded biquads
yohoo15 0:0f08f98ae7f8 202 below the coefficients for those three biquads */
yohoo15 0:0f08f98ae7f8 203 //first low pass biquad
yohoo15 0:0f08f98ae7f8 204 const double lowp1_a1 = -1.05207469728;
yohoo15 0:0f08f98ae7f8 205 const double lowp1_a2 = 0.28586907478;
yohoo15 0:0f08f98ae7f8 206 const double lowp1_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 207 const double lowp1_b1 = 2.00000000000;
yohoo15 0:0f08f98ae7f8 208 const double lowp1_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 209
yohoo15 0:0f08f98ae7f8 210 //second low pass biquad
yohoo15 0:0f08f98ae7f8 211 const double lowp2_a1 = -1.16338171052;
yohoo15 0:0f08f98ae7f8 212 const double lowp2_a2 = 0.42191097989;
yohoo15 0:0f08f98ae7f8 213 const double lowp2_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 214 const double lowp2_b1 = 2.00000000000;
yohoo15 0:0f08f98ae7f8 215 const double lowp2_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 216
yohoo15 0:0f08f98ae7f8 217 //third low pass biquad
yohoo15 0:0f08f98ae7f8 218 const double lowp3_a1 = -1.42439823874;
yohoo15 0:0f08f98ae7f8 219 const double lowp3_a2 = 0.74093118112;
yohoo15 0:0f08f98ae7f8 220 const double lowp3_b0 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 221 const double lowp3_b1 = 2.00000000000;
yohoo15 0:0f08f98ae7f8 222 const double lowp3_b2 = 1.00000000000;
yohoo15 0:0f08f98ae7f8 223
yohoo15 0:0f08f98ae7f8 224
yohoo15 0:0f08f98ae7f8 225
yohoo15 0:0f08f98ae7f8 226 double Filteren_left(double input)
yohoo15 0:0f08f98ae7f8 227 {
yohoo15 0:0f08f98ae7f8 228
yohoo15 0:0f08f98ae7f8 229 input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER
yohoo15 0:0f08f98ae7f8 230 //input_right = analog_emg_right.read();
yohoo15 0:0f08f98ae7f8 231
yohoo15 0:0f08f98ae7f8 232 // notch filter
yohoo15 0:0f08f98ae7f8 233 double y1 = biquad(input, notch_v11, notch_v21, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
yohoo15 0:0f08f98ae7f8 234 double y2 = biquad(y1, notch_v12, notch_v22, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
yohoo15 0:0f08f98ae7f8 235 double y3 = biquad(y2, notch_v13, notch_v23, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2);
yohoo15 0:0f08f98ae7f8 236
yohoo15 0:0f08f98ae7f8 237 //higpass filter
yohoo15 0:0f08f98ae7f8 238 double y4 = biquad(y3, high_v11, high_v21, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
yohoo15 0:0f08f98ae7f8 239 double y5 = biquad(y4, high_v12, high_v22, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
yohoo15 0:0f08f98ae7f8 240 double y6 = biquad(y5, high_v13, high_v23, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
yohoo15 0:0f08f98ae7f8 241
yohoo15 0:0f08f98ae7f8 242 //rectivier
yohoo15 0:0f08f98ae7f8 243 double y7 = fabs(y6);
yohoo15 0:0f08f98ae7f8 244
yohoo15 0:0f08f98ae7f8 245 //lowpas filter cascade
yohoo15 0:0f08f98ae7f8 246 double y8 = biquad(y7, low_v11, low_v21, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
yohoo15 0:0f08f98ae7f8 247 double y9 = biquad(y8, low_v12, low_v22, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
yohoo15 0:0f08f98ae7f8 248 double y10= biquad(y9, low_v13, low_v23, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
yohoo15 0:0f08f98ae7f8 249
yohoo15 0:0f08f98ae7f8 250 // moving average
yohoo15 0:0f08f98ae7f8 251 double filter_signal = moving_average(y10,n1,n2,n3,n4,n5);
yohoo15 0:0f08f98ae7f8 252
yohoo15 0:0f08f98ae7f8 253 return(filter_signal);
yohoo15 0:0f08f98ae7f8 254 }
yohoo15 0:0f08f98ae7f8 255
yohoo15 0:0f08f98ae7f8 256 double Filteren_right(double input_b)
yohoo15 0:0f08f98ae7f8 257 {
yohoo15 0:0f08f98ae7f8 258
yohoo15 0:0f08f98ae7f8 259 input_b = input_b-0.45; //FIRST SUBTRACT MEAN THEN FILTER
yohoo15 0:0f08f98ae7f8 260 //input_right = analog_emg_right.read();
yohoo15 0:0f08f98ae7f8 261
yohoo15 0:0f08f98ae7f8 262 // notch filter
yohoo15 0:0f08f98ae7f8 263 double y1_b = biquad(input_b, notch_v11_b, notch_v21_b, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
yohoo15 0:0f08f98ae7f8 264 double y2_b = biquad(y1_b, notch_v12_b, notch_v22_b, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
yohoo15 0:0f08f98ae7f8 265 double y3_b = biquad(y2_b, notch_v13_b, notch_v23_b, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2);
yohoo15 0:0f08f98ae7f8 266
yohoo15 0:0f08f98ae7f8 267 //higpass filter
yohoo15 0:0f08f98ae7f8 268 double y4_b = biquad(y3_b, high_v11_b, high_v21_b, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
yohoo15 0:0f08f98ae7f8 269 double y5_b = biquad(y4_b, high_v12_b, high_v22_b, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
yohoo15 0:0f08f98ae7f8 270 double y6_b = biquad(y5_b, high_v13_b, high_v23_b, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
yohoo15 0:0f08f98ae7f8 271
yohoo15 0:0f08f98ae7f8 272 //rectivier
yohoo15 0:0f08f98ae7f8 273 double y7_b = fabs(y6_b);
yohoo15 0:0f08f98ae7f8 274
yohoo15 0:0f08f98ae7f8 275 //lowpas filter cascade
yohoo15 0:0f08f98ae7f8 276 double y8_b = biquad(y7_b, low_v11_b, low_v21_b, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
yohoo15 0:0f08f98ae7f8 277 double y9_b = biquad(y8_b, low_v12_b, low_v22_b, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
yohoo15 0:0f08f98ae7f8 278 double y10_b= biquad(y9_b, low_v13_b, low_v23_b, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
yohoo15 0:0f08f98ae7f8 279
yohoo15 0:0f08f98ae7f8 280 // moving average
yohoo15 0:0f08f98ae7f8 281 double filter_signal_b = moving_average(y10_b,n1_b,n2_b,n3_b,n4_b,n5_b);
yohoo15 0:0f08f98ae7f8 282
yohoo15 0:0f08f98ae7f8 283 return(filter_signal_b);
yohoo15 0:0f08f98ae7f8 284 }
yohoo15 0:0f08f98ae7f8 285
yohoo15 0:0f08f98ae7f8 286 /*************************************************************BEGIN motor control******************************************************************************************************/
yohoo15 0:0f08f98ae7f8 287 // Define pin for motor control
yohoo15 0:0f08f98ae7f8 288 DigitalOut directionPin(D4);
yohoo15 0:0f08f98ae7f8 289 PwmOut PWM(D5);
yohoo15 0:0f08f98ae7f8 290 DigitalOut directionPin_key(D7);
yohoo15 0:0f08f98ae7f8 291 PwmOut PWM_key(D6);
yohoo15 0:0f08f98ae7f8 292 Ticker both_timer;
yohoo15 0:0f08f98ae7f8 293 // define rotation direction and begin possition
yohoo15 0:0f08f98ae7f8 294 const int cw = 1;
yohoo15 0:0f08f98ae7f8 295 const int ccw = 0;
yohoo15 0:0f08f98ae7f8 296
yohoo15 0:0f08f98ae7f8 297 volatile bool flag_both = false;
yohoo15 0:0f08f98ae7f8 298
yohoo15 0:0f08f98ae7f8 299 // making function flags.
yohoo15 0:0f08f98ae7f8 300 void Go_flag_both()
yohoo15 0:0f08f98ae7f8 301 {
yohoo15 0:0f08f98ae7f8 302 flag_both = true;
yohoo15 0:0f08f98ae7f8 303 }
yohoo15 0:0f08f98ae7f8 304
yohoo15 0:0f08f98ae7f8 305
yohoo15 0:0f08f98ae7f8 306 /*
yohoo15 0:0f08f98ae7f8 307 double Rotation = -2; // rotation
yohoo15 0:0f08f98ae7f8 308 double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
yohoo15 0:0f08f98ae7f8 309 */
yohoo15 0:0f08f98ae7f8 310
yohoo15 0:0f08f98ae7f8 311
yohoo15 0:0f08f98ae7f8 312 /*
yohoo15 0:0f08f98ae7f8 313
yohoo15 0:0f08f98ae7f8 314 // Reusable P controller
yohoo15 0:0f08f98ae7f8 315 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
yohoo15 0:0f08f98ae7f8 316 {
yohoo15 0:0f08f98ae7f8 317
yohoo15 0:0f08f98ae7f8 318 e_int = e_int + Ts * error;
yohoo15 0:0f08f98ae7f8 319
yohoo15 0:0f08f98ae7f8 320 double PI_output = (Kp * error) + (Ki * e_int);
yohoo15 0:0f08f98ae7f8 321 return PI_output;
yohoo15 0:0f08f98ae7f8 322 }
yohoo15 0:0f08f98ae7f8 323 // Next task, measure the error and apply the output to the plant
yohoo15 0:0f08f98ae7f8 324
yohoo15 0:0f08f98ae7f8 325 // control for movement left right
yohoo15 0:0f08f98ae7f8 326 void motor1_Controller()
yohoo15 0:0f08f98ae7f8 327 {
yohoo15 0:0f08f98ae7f8 328 double reference = setpoint; // setpoint is in pulses
yohoo15 0:0f08f98ae7f8 329 double position = wheel.getPulses();
yohoo15 0:0f08f98ae7f8 330 double error_pulses = (reference - position); // calculate the error in pulses
yohoo15 0:0f08f98ae7f8 331 double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
yohoo15 0:0f08f98ae7f8 332
yohoo15 0:0f08f98ae7f8 333 double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
yohoo15 0:0f08f98ae7f8 334
yohoo15 0:0f08f98ae7f8 335 if(error_pulses > 0) {
yohoo15 0:0f08f98ae7f8 336 directionPin.write(cw);
yohoo15 0:0f08f98ae7f8 337
yohoo15 0:0f08f98ae7f8 338 } else if(error_pulses < 0) {
yohoo15 0:0f08f98ae7f8 339 directionPin.write(ccw);
yohoo15 0:0f08f98ae7f8 340 } else {
yohoo15 0:0f08f98ae7f8 341 output = 0;
yohoo15 0:0f08f98ae7f8 342 }
yohoo15 0:0f08f98ae7f8 343 PWM.write(output); // out of the if loop due to abs output
yohoo15 0:0f08f98ae7f8 344
yohoo15 0:0f08f98ae7f8 345
yohoo15 0:0f08f98ae7f8 346 }*/
yohoo15 0:0f08f98ae7f8 347
yohoo15 0:0f08f98ae7f8 348 /*************************************************************END motor control******************************************************************************************************/
yohoo15 0:0f08f98ae7f8 349
yohoo15 0:0f08f98ae7f8 350 void HIDScope_kijken()
yohoo15 0:0f08f98ae7f8 351 {
yohoo15 0:0f08f98ae7f8 352 scope.set(0, analog_emg_left.read());
yohoo15 0:0f08f98ae7f8 353 scope.set(1, filter_signal_hid_left);
yohoo15 0:0f08f98ae7f8 354 scope.set(2, analog_emg_right.read());
yohoo15 0:0f08f98ae7f8 355 scope.set(3, filter_signal_hid_right);
yohoo15 0:0f08f98ae7f8 356 scope.send();
yohoo15 0:0f08f98ae7f8 357 }
yohoo15 0:0f08f98ae7f8 358
yohoo15 0:0f08f98ae7f8 359
yohoo15 0:0f08f98ae7f8 360 int main()
yohoo15 0:0f08f98ae7f8 361 {
yohoo15 0:0f08f98ae7f8 362 pc.printf("at the begin, \n");
yohoo15 0:0f08f98ae7f8 363
yohoo15 0:0f08f98ae7f8 364 HIDScope_timer.attach(&Go_flag_HIDScope, 0.02);//0.02 had worked
yohoo15 0:0f08f98ae7f8 365 Filteren_timer.attach(&Go_flag_filteren,0.04);// 0.04 had worked
yohoo15 0:0f08f98ae7f8 366 both_timer.attach(&Go_flag_both, 2);
yohoo15 0:0f08f98ae7f8 367 while(1) {
yohoo15 0:0f08f98ae7f8 368
yohoo15 0:0f08f98ae7f8 369
yohoo15 0:0f08f98ae7f8 370 if(Flag_filteren) {
yohoo15 0:0f08f98ae7f8 371 Flag_filteren = false;
yohoo15 0:0f08f98ae7f8 372 // filter left and set bool
yohoo15 0:0f08f98ae7f8 373 filter_signal_hid_left = Filteren_left(analog_emg_left.read());
yohoo15 0:0f08f98ae7f8 374
yohoo15 0:0f08f98ae7f8 375 if (filter_signal_hid_left > high_threshold) {
yohoo15 0:0f08f98ae7f8 376 left_movement = true;
yohoo15 0:0f08f98ae7f8 377 } else if (filter_signal_hid_left < low_threshold) {
yohoo15 0:0f08f98ae7f8 378 left_movement = false;
yohoo15 0:0f08f98ae7f8 379 }
yohoo15 0:0f08f98ae7f8 380 // make filter right and set bool
yohoo15 0:0f08f98ae7f8 381 filter_signal_hid_right = Filteren_right(analog_emg_right.read());
yohoo15 0:0f08f98ae7f8 382
yohoo15 0:0f08f98ae7f8 383 if (filter_signal_hid_right > high_threshold) {
yohoo15 0:0f08f98ae7f8 384 right_movement = true;
yohoo15 0:0f08f98ae7f8 385 } else if (filter_signal_hid_right < low_threshold) {
yohoo15 0:0f08f98ae7f8 386 right_movement = false;
yohoo15 0:0f08f98ae7f8 387 }
yohoo15 0:0f08f98ae7f8 388
yohoo15 0:0f08f98ae7f8 389
yohoo15 0:0f08f98ae7f8 390 }
yohoo15 0:0f08f98ae7f8 391
yohoo15 0:0f08f98ae7f8 392
yohoo15 0:0f08f98ae7f8 393
yohoo15 0:0f08f98ae7f8 394 if(Flag_HIDScope) {
yohoo15 0:0f08f98ae7f8 395 Flag_HIDScope = false;
yohoo15 0:0f08f98ae7f8 396 HIDScope_kijken();
yohoo15 0:0f08f98ae7f8 397
yohoo15 0:0f08f98ae7f8 398 }
yohoo15 0:0f08f98ae7f8 399
yohoo15 0:0f08f98ae7f8 400
yohoo15 0:0f08f98ae7f8 401
yohoo15 0:0f08f98ae7f8 402
yohoo15 0:0f08f98ae7f8 403 // Pussing buttons to get input signal
yohoo15 0:0f08f98ae7f8 404 if( left_movement and right_movement == false) {
yohoo15 0:0f08f98ae7f8 405 directionPin.write(cw);
yohoo15 0:0f08f98ae7f8 406 PWM.write(1);
yohoo15 0:0f08f98ae7f8 407
yohoo15 0:0f08f98ae7f8 408 } else if(right_movement and left_movement == false ) {
yohoo15 0:0f08f98ae7f8 409 directionPin.write(ccw);
yohoo15 0:0f08f98ae7f8 410 PWM.write(1);
yohoo15 0:0f08f98ae7f8 411 } else {
yohoo15 0:0f08f98ae7f8 412 PWM.write(0);
yohoo15 0:0f08f98ae7f8 413 }
yohoo15 0:0f08f98ae7f8 414
yohoo15 0:0f08f98ae7f8 415 if( left_movement and right_movement and flag_both){
yohoo15 0:0f08f98ae7f8 416 directionPin_key.write(cw);
yohoo15 0:0f08f98ae7f8 417 PWM_key.write(1);
yohoo15 0:0f08f98ae7f8 418 wait(0.14);
yohoo15 0:0f08f98ae7f8 419 PWM_key.write(0);
yohoo15 0:0f08f98ae7f8 420 wait(1);
yohoo15 0:0f08f98ae7f8 421 directionPin_key.write(ccw);
yohoo15 0:0f08f98ae7f8 422 PWM_key.write(1);
yohoo15 0:0f08f98ae7f8 423 wait(0.165f);
yohoo15 0:0f08f98ae7f8 424 PWM_key.write(0);
yohoo15 0:0f08f98ae7f8 425 flag_both = false;
yohoo15 0:0f08f98ae7f8 426 }
yohoo15 0:0f08f98ae7f8 427 else{
yohoo15 0:0f08f98ae7f8 428 PWM_key.write(0);
yohoo15 0:0f08f98ae7f8 429 }
yohoo15 0:0f08f98ae7f8 430 }
yohoo15 0:0f08f98ae7f8 431 }
yohoo15 0:0f08f98ae7f8 432