Biorobotics / Mbed 2 deprecated proberen_Two_motors

klein beginnen

Dependencies:   HIDScope QEI mbed

main.cpp@0:be32746458fc, 2015-10-28 (annotated)

Committer:
yohoo15
Date:
Wed Oct 28 16:13:20 2015 +0000
Revision:
0:be32746458fc
proberen;

Who changed what in which revision?

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