Iris van Leeuwen / Mbed 2 deprecated Lampje_EMG_Gr6

2 losse EMG signalen van de biceps en deltoid

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed Encoder

Fork of Lampje_EMG_Gr6 by Jesse Kaiser

main.cpp@28:c33a0658605e, 2014-11-01 (annotated)

Committer:
irisl
Date:
Sat Nov 01 16:19:53 2014 +0000
Revision:
28:c33a0658605e
Parent:
27:691779624530
Child:
29:d0dab8921e9d
Commentaar bijgevoegd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jessekaiser 0:db396b9f4b4c 1 #include "mbed.h"
jessekaiser 0:db396b9f4b4c 2 #include "HIDScope.h"
jessekaiser 0:db396b9f4b4c 3 #include "arm_math.h"
jessekaiser 0:db396b9f4b4c 4 #include "MODSERIAL.h"
irisl 20:d40b6cba4280 5 #include "encoder.h"
irisl 21:674fafb6301d 6 #include "PwmOut.h"
jessekaiser 0:db396b9f4b4c 7
irisl 21:674fafb6301d 8 #define TSAMP 0.005
irisl 21:674fafb6301d 9 #define K_P1 (3.5) //voor motor1 is het 3.5
irisl 21:674fafb6301d 10 #define K_I1 (0.01 *TSAMP) //voor motor1 is het 0.01
irisl 23:94b5746e52f5 11 #define K_P2 (0.7)
irisl 21:674fafb6301d 12 #define K_I2 (0.01 *TSAMP)
irisl 20:d40b6cba4280 13 #define I_LIMIT 1.
irisl 21:674fafb6301d 14 //#define PI 3.14159265
irisl 21:674fafb6301d 15 #define l_arm 0.5
irisl 20:d40b6cba4280 16
irisl 20:d40b6cba4280 17 #define M1_PWM PTC8
irisl 20:d40b6cba4280 18 #define M1_DIR PTC9
irisl 20:d40b6cba4280 19 #define M2_PWM PTA5
irisl 20:d40b6cba4280 20 #define M2_DIR PTA4
irisl 20:d40b6cba4280 21
irisl 20:d40b6cba4280 22 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
irisl 20:d40b6cba4280 23
irisl 28:c33a0658605e 24 // Define objects
irisl 20:d40b6cba4280 25 Serial pc(USBTX, USBRX);
irisl 20:d40b6cba4280 26
irisl 28:c33a0658605e 27 // LED
irisl 21:674fafb6301d 28 DigitalOut myledred(PTB3);
irisl 21:674fafb6301d 29 DigitalOut myledgreen(PTB1);
irisl 21:674fafb6301d 30 DigitalOut myledblue(PTB2);
jessekaiser 0:db396b9f4b4c 31
irisl 28:c33a0658605e 32 //EMG
irisl 21:674fafb6301d 33 AnalogIn emg0(PTB0); //Analog input
irisl 21:674fafb6301d 34 AnalogIn emg1(PTC2); //Analog input
irisl 25:a12d51345aa4 35 HIDScope scope(4);
jessekaiser 0:db396b9f4b4c 36
irisl 20:d40b6cba4280 37 //motor 25D
irisl 21:674fafb6301d 38 Encoder motor1(PTD3,PTD5); //wit, geel
irisl 20:d40b6cba4280 39 PwmOut pwm_motor1(M2_PWM);
irisl 20:d40b6cba4280 40 DigitalOut motordir1(M2_DIR);
irisl 20:d40b6cba4280 41
irisl 20:d40b6cba4280 42 //motor2 37D
irisl 21:674fafb6301d 43 Encoder motor2(PTD2, PTD0); //wit, geel
irisl 20:d40b6cba4280 44 PwmOut pwm_motor2(M1_PWM);
irisl 20:d40b6cba4280 45 DigitalOut motordir2(M1_DIR);
irisl 20:d40b6cba4280 46
irisl 28:c33a0658605e 47 // Motor variabelen
irisl 21:674fafb6301d 48 float pwm1_percentage = 0;
irisl 21:674fafb6301d 49 float pwm2_percentage = 0;
irisl 21:674fafb6301d 50 int cur_pos_motor1;
irisl 21:674fafb6301d 51 int prev_pos_motor1 = 0;
irisl 21:674fafb6301d 52 int cur_pos_motor2;
irisl 21:674fafb6301d 53 int prev_pos_motor2 = 0;
irisl 21:674fafb6301d 54 float speed1_rad;
irisl 21:674fafb6301d 55 float speed2_rad;
irisl 21:674fafb6301d 56 float pos_motor1_rad;
irisl 21:674fafb6301d 57 float pos_motor2_rad;
irisl 21:674fafb6301d 58 int staat1 = 0;
irisl 21:674fafb6301d 59 int staat2 = 0;
irisl 22:14f5161d7d7b 60 volatile float arm_hoogte = 0;
irisl 22:14f5161d7d7b 61 volatile float batje_hoek = 0;
irisl 21:674fafb6301d 62 int wait_iterator1 = 0;
irisl 21:674fafb6301d 63 int wait_iterator2 = 0;
irisl 20:d40b6cba4280 64
irisl 20:d40b6cba4280 65
irisl 28:c33a0658605e 66 // EMG Filters (settings en variabelen)
irisl 20:d40b6cba4280 67
irisl 28:c33a0658605e 68 // Filters
irisl 9:f7ec578a17c0 69 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
irisl 9:f7ec578a17c0 70 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
irisl 26:9859a71456fd 71 //lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
irisl 26:9859a71456fd 72 float lowpass_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
jessekaiser 0:db396b9f4b4c 73 //state values
irisl 9:f7ec578a17c0 74 float lowpass_biceps_states[4];
irisl 9:f7ec578a17c0 75 float lowpass_deltoid_states[4];
irisl 9:f7ec578a17c0 76 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
irisl 9:f7ec578a17c0 77 arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
irisl 27:691779624530 78 //highpass filter settings: Fc = 10 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
irisl 26:9859a71456fd 79 float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
jessekaiser 0:db396b9f4b4c 80 //state values
irisl 9:f7ec578a17c0 81 float highnotch_biceps_states[8];
irisl 9:f7ec578a17c0 82 float highnotch_deltoid_states[8];
jessekaiser 2:39e1bde54e73 83
jessekaiser 2:39e1bde54e73 84 //De globale variabele voor het gefilterde EMG signaal
jessekaiser 2:39e1bde54e73 85 float filtered_biceps;
jessekaiser 2:39e1bde54e73 86 float filtered_deltoid;
irisl 12:9e6e49af9304 87 float filtered_average_bi;
irisl 12:9e6e49af9304 88 float filtered_average_del;
jessekaiser 0:db396b9f4b4c 89
jessekaiser 0:db396b9f4b4c 90
irisl 12:9e6e49af9304 91 void average_biceps(float filtered_biceps,float *average)
irisl 12:9e6e49af9304 92 {
irisl 12:9e6e49af9304 93 static float total=0;
irisl 12:9e6e49af9304 94 static float number=0;
irisl 12:9e6e49af9304 95 total = total + filtered_biceps;
irisl 12:9e6e49af9304 96 number = number + 1;
irisl 26:9859a71456fd 97 if ( number == 250) {
irisl 26:9859a71456fd 98 *average = total/250;
irisl 12:9e6e49af9304 99 total = 0;
irisl 12:9e6e49af9304 100 number = 0;
irisl 12:9e6e49af9304 101 }
irisl 12:9e6e49af9304 102 }
irisl 12:9e6e49af9304 103
irisl 12:9e6e49af9304 104 void average_deltoid(float filtered_input,float *average_output)
irisl 12:9e6e49af9304 105 {
irisl 12:9e6e49af9304 106 static float total=0;
irisl 12:9e6e49af9304 107 static float number=0;
irisl 12:9e6e49af9304 108 total = total + filtered_input;
irisl 12:9e6e49af9304 109 number = number + 1;
irisl 26:9859a71456fd 110 if ( number == 250) {
irisl 26:9859a71456fd 111 *average_output = total/250;
irisl 12:9e6e49af9304 112 total = 0;
irisl 12:9e6e49af9304 113 number = 0;
irisl 12:9e6e49af9304 114 }
irisl 12:9e6e49af9304 115 }
irisl 12:9e6e49af9304 116
irisl 28:c33a0658605e 117 // EMG looper
jessekaiser 0:db396b9f4b4c 118 void looper()
jessekaiser 0:db396b9f4b4c 119 {
jessekaiser 1:099b19376f16 120 /*variable to store value in*/
jessekaiser 2:39e1bde54e73 121 uint16_t emg_value1;
jessekaiser 3:0895fa0a6ca4 122 uint16_t emg_value2;
jessekaiser 1:099b19376f16 123
jessekaiser 2:39e1bde54e73 124 float emg_value1_f32;
jessekaiser 2:39e1bde54e73 125 float emg_value2_f32;
jessekaiser 0:db396b9f4b4c 126 /*put raw emg value both in red and in emg_value*/
jessekaiser 2:39e1bde54e73 127 emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
jessekaiser 2:39e1bde54e73 128 emg_value1_f32 = emg0.read();
jessekaiser 3:0895fa0a6ca4 129
jessekaiser 2:39e1bde54e73 130 emg_value2 = emg1.read_u16();
jessekaiser 2:39e1bde54e73 131 emg_value2_f32 = emg1.read();
jessekaiser 0:db396b9f4b4c 132
jessekaiser 2:39e1bde54e73 133 //process emg biceps
irisl 9:f7ec578a17c0 134 arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
jessekaiser 2:39e1bde54e73 135 filtered_biceps = fabs(filtered_biceps);
irisl 9:f7ec578a17c0 136 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
irisl 12:9e6e49af9304 137 average_biceps(filtered_biceps,&filtered_average_bi);
jessekaiser 2:39e1bde54e73 138 //process emg deltoid
irisl 9:f7ec578a17c0 139 arm_biquad_cascade_df1_f32(&highnotch_deltoid, &emg_value2_f32, &filtered_deltoid, 1 );
jessekaiser 3:0895fa0a6ca4 140 filtered_deltoid = fabs(filtered_deltoid);
irisl 9:f7ec578a17c0 141 arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
irisl 12:9e6e49af9304 142 average_deltoid(filtered_deltoid, &filtered_average_del);
jessekaiser 1:099b19376f16 143
jessekaiser 0:db396b9f4b4c 144 /*send value to PC. */
irisl 9:f7ec578a17c0 145 //scope.set(0,emg_value1); //Raw EMG signal biceps
irisl 9:f7ec578a17c0 146 //scope.set(1,emg_value2); //Raw EMG signal Deltoid
irisl 25:a12d51345aa4 147 scope.set(0,filtered_biceps); //processed float biceps
irisl 25:a12d51345aa4 148 scope.set(1,filtered_average_bi); //processed float deltoid
irisl 25:a12d51345aa4 149 scope.set(2,filtered_deltoid); //processed float biceps
irisl 25:a12d51345aa4 150 scope.set(3,filtered_average_del); //processed float deltoid
jessekaiser 0:db396b9f4b4c 151 scope.send();
jessekaiser 0:db396b9f4b4c 152
jessekaiser 0:db396b9f4b4c 153 }
jessekaiser 0:db396b9f4b4c 154
irisl 16:d65458b85734 155 // LED AANSTURING
irisl 16:d65458b85734 156
jessekaiser 0:db396b9f4b4c 157 void BlinkRed(int n)
jessekaiser 0:db396b9f4b4c 158 {
jessekaiser 0:db396b9f4b4c 159 for (int i=0; i<n; i++) {
irisl 21:674fafb6301d 160 myledred = 0;
irisl 21:674fafb6301d 161 myledgreen = 0;
irisl 21:674fafb6301d 162 myledblue = 0;
jessekaiser 3:0895fa0a6ca4 163 wait(0.1);
irisl 21:674fafb6301d 164 myledred = 1;
irisl 21:674fafb6301d 165 myledgreen = 0;
irisl 21:674fafb6301d 166 myledblue = 0;
jessekaiser 3:0895fa0a6ca4 167 wait(0.1);
jessekaiser 0:db396b9f4b4c 168 }
jessekaiser 0:db396b9f4b4c 169 }
jessekaiser 0:db396b9f4b4c 170
irisl 28:c33a0658605e 171 // Ticker voor groen knipperen, zodat tijdens dit knipperen presets gekozen kunnen worden
irisl 28:c33a0658605e 172 Ticker ledticker;
irisl 28:c33a0658605e 173
irisl 17:dbbe4e126203 174 void greenblink()
irisl 16:d65458b85734 175 {
irisl 21:674fafb6301d 176 if(myledgreen.read())
irisl 21:674fafb6301d 177 myledgreen = 0;
irisl 16:d65458b85734 178 else
irisl 21:674fafb6301d 179 myledgreen = 1;
irisl 16:d65458b85734 180 }
irisl 16:d65458b85734 181
irisl 17:dbbe4e126203 182 void BlinkGreen()
irisl 18:ed2afe6953de 183 {
irisl 21:674fafb6301d 184 myledred= 0;
irisl 21:674fafb6301d 185 myledblue =0;
irisl 18:ed2afe6953de 186 ledticker.attach(&greenblink,.5);
irisl 18:ed2afe6953de 187 /* myled1 = 1;
irisl 18:ed2afe6953de 188 myled2 = 1;
irisl 18:ed2afe6953de 189 myled3 = 1;
irisl 18:ed2afe6953de 190 wait(0.1);
irisl 18:ed2afe6953de 191 myled1 = 0;
irisl 18:ed2afe6953de 192 myled2 = 1;
irisl 18:ed2afe6953de 193 myled3 = 1;
irisl 18:ed2afe6953de 194 wait(0.1);*/
irisl 16:d65458b85734 195 }
irisl 16:d65458b85734 196
irisl 18:ed2afe6953de 197 void stopblinkgreen()
irisl 16:d65458b85734 198 {
irisl 16:d65458b85734 199 ledticker.detach();
irisl 16:d65458b85734 200 }
irisl 16:d65458b85734 201
irisl 28:c33a0658605e 202 // Groen schijnen
irisl 13:493a953a2a85 203 void ShineGreen ()
irisl 13:493a953a2a85 204 {
irisl 21:674fafb6301d 205 myledred = 0;
irisl 21:674fafb6301d 206 myledgreen = 1;
irisl 21:674fafb6301d 207 myledblue = 0;
irisl 13:493a953a2a85 208 }
irisl 13:493a953a2a85 209
irisl 28:c33a0658605e 210 // Blauw schijnen
irisl 13:493a953a2a85 211 void ShineBlue ()
irisl 13:493a953a2a85 212 {
irisl 21:674fafb6301d 213 myledred = 0;
irisl 21:674fafb6301d 214 myledgreen = 0;
irisl 21:674fafb6301d 215 myledblue = 1;
irisl 13:493a953a2a85 216 }
irisl 13:493a953a2a85 217
irisl 28:c33a0658605e 218 // Rood schijnen
irisl 13:493a953a2a85 219 void ShineRed ()
irisl 13:493a953a2a85 220 {
irisl 21:674fafb6301d 221 myledred = 1;
irisl 21:674fafb6301d 222 myledgreen = 0;
irisl 21:674fafb6301d 223 myledblue = 0;
irisl 13:493a953a2a85 224 }
irisl 13:493a953a2a85 225
irisl 20:d40b6cba4280 226 // MOTORFUNCTIES
irisl 20:d40b6cba4280 227
irisl 28:c33a0658605e 228 // Motor1 = batje
irisl 28:c33a0658605e 229 // Motor2 = arm
irisl 28:c33a0658605e 230
irisl 21:674fafb6301d 231 void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
irisl 21:674fafb6301d 232 // maar de locatie van de variabele.
irisl 21:674fafb6301d 233 {
irisl 21:674fafb6301d 234 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
irisl 21:674fafb6301d 235 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
irisl 21:674fafb6301d 236 }
irisl 21:674fafb6301d 237
irisl 28:c33a0658605e 238 // PI-regelaar motor1: batje
irisl 21:674fafb6301d 239 float pid1(float setpoint1, float measurement1)
irisl 20:d40b6cba4280 240 {
irisl 21:674fafb6301d 241 float error1;
irisl 21:674fafb6301d 242 float out_p1 = 0;
irisl 21:674fafb6301d 243 static float out_i1 = 0;
irisl 21:674fafb6301d 244 error1 = (setpoint1 - measurement1);
irisl 21:674fafb6301d 245 out_p1 = error1*K_P1;
irisl 21:674fafb6301d 246 out_i1 += error1*K_I1;
irisl 21:674fafb6301d 247 clamp(&out_i1,-I_LIMIT,I_LIMIT);
irisl 21:674fafb6301d 248 return out_p1 + out_i1;
irisl 20:d40b6cba4280 249 }
irisl 20:d40b6cba4280 250
irisl 28:c33a0658605e 251 // PI-regelaar motor2: arm
irisl 21:674fafb6301d 252 float pid2(float setpoint2, float measurement2)
irisl 20:d40b6cba4280 253 {
irisl 21:674fafb6301d 254 float error2;
irisl 21:674fafb6301d 255 float out_p2 = 0;
irisl 21:674fafb6301d 256 static float out_i2 = 0;
irisl 21:674fafb6301d 257 error2 = (setpoint2 - measurement2);
irisl 21:674fafb6301d 258 out_p2 = error2*K_P2;
irisl 21:674fafb6301d 259 out_i2 += error2*K_I2;
irisl 21:674fafb6301d 260 clamp(&out_i2,-I_LIMIT,I_LIMIT);
irisl 21:674fafb6301d 261 return out_p2 + out_i2;
irisl 21:674fafb6301d 262 }
irisl 28:c33a0658605e 263
irisl 28:c33a0658605e 264 // Variabelen
irisl 21:674fafb6301d 265 float prev_setpoint1 = 0;
irisl 21:674fafb6301d 266 float setpoint1 = 0;
irisl 21:674fafb6301d 267 float prev_setpoint2 = 0;
irisl 21:674fafb6301d 268 float setpoint2 = 0;
irisl 21:674fafb6301d 269
irisl 28:c33a0658605e 270 // Functies motoren
irisl 28:c33a0658605e 271
irisl 28:c33a0658605e 272 // Motor1 links draaien
irisl 21:674fafb6301d 273 void batje_links ()
irisl 21:674fafb6301d 274 {
irisl 21:674fafb6301d 275 speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 276 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 28:c33a0658605e 277 if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden, nu 11.3. 2.3 is de verhouding van de tanden
irisl 23:94b5746e52f5 278 setpoint1 = (11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 279 }
irisl 23:94b5746e52f5 280 if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
irisl 23:94b5746e52f5 281 setpoint1 = -(11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 282 }
irisl 21:674fafb6301d 283 prev_setpoint1 = setpoint1;
irisl 20:d40b6cba4280 284 }
irisl 20:d40b6cba4280 285
irisl 28:c33a0658605e 286 // Motor1 rechts draaien
irisl 21:674fafb6301d 287 void batje_rechts ()
irisl 20:d40b6cba4280 288 {
irisl 23:94b5746e52f5 289 speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 290 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 21:674fafb6301d 291 if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
irisl 21:674fafb6301d 292 setpoint1 = (11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 293 }
irisl 22:14f5161d7d7b 294 if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
irisl 22:14f5161d7d7b 295 setpoint1 = -(11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 296 }
irisl 21:674fafb6301d 297 pwm_motor1.write(abs(pwm1_percentage));
irisl 21:674fafb6301d 298 prev_setpoint1 = setpoint1;
irisl 21:674fafb6301d 299 if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 300 staat1 = 1;
irisl 21:674fafb6301d 301 }
irisl 20:d40b6cba4280 302 }
irisl 20:d40b6cba4280 303
irisl 28:c33a0658605e 304 //Motor1 na links draaien weer terug laten draaien naar beginstand
irisl 21:674fafb6301d 305 void batje_begin_links ()
irisl 21:674fafb6301d 306 {
irisl 21:674fafb6301d 307 speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 308 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 22:14f5161d7d7b 309 if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
irisl 22:14f5161d7d7b 310 setpoint1 = (0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 311 }
irisl 22:14f5161d7d7b 312 if(setpoint1 < -(0*2.3*2.0*PI/360)) {
irisl 22:14f5161d7d7b 313 setpoint1 = -(0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 314 }
irisl 21:674fafb6301d 315 prev_setpoint1 = setpoint1;
irisl 21:674fafb6301d 316 }
irisl 21:674fafb6301d 317
irisl 28:c33a0658605e 318 //Motor1 na links draaien weer terug laten draaien naar beginstand
irisl 21:674fafb6301d 319 void batje_begin_rechts ()
irisl 21:674fafb6301d 320 {
irisl 23:94b5746e52f5 321 speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 322 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 22:14f5161d7d7b 323 if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
irisl 22:14f5161d7d7b 324 setpoint1 = (0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 325 }
irisl 21:674fafb6301d 326 if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
irisl 21:674fafb6301d 327 setpoint1 = -(0.0*2.3*2.0*PI/360);
irisl 21:674fafb6301d 328 }
irisl 21:674fafb6301d 329 prev_setpoint1 = setpoint1;
irisl 21:674fafb6301d 330 }
irisl 21:674fafb6301d 331
irisl 28:c33a0658605e 332 // Motor2 balletje op zn hoogst slaan
irisl 21:674fafb6301d 333 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
irisl 21:674fafb6301d 334 {
irisl 23:94b5746e52f5 335 speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 336 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 23:94b5746e52f5 337 if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
irisl 23:94b5746e52f5 338 setpoint2 = (155.0*2.0*PI/360);
irisl 21:674fafb6301d 339 }
irisl 23:94b5746e52f5 340 if(setpoint2 < -(155.0*2.0*PI/360)) {
irisl 23:94b5746e52f5 341 setpoint2 = -(155.0*2.0*PI/360);
irisl 21:674fafb6301d 342 }
irisl 21:674fafb6301d 343 prev_setpoint2 = setpoint2;
irisl 23:94b5746e52f5 344 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 345 staat2 = 1;
irisl 21:674fafb6301d 346 }
irisl 21:674fafb6301d 347 }
irisl 21:674fafb6301d 348
irisl 28:c33a0658605e 349 // Motor2 balletje in het midden slaan
irisl 21:674fafb6301d 350 void arm_mid ()
irisl 21:674fafb6301d 351 {
irisl 23:94b5746e52f5 352 speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 353 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 23:94b5746e52f5 354 if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
irisl 23:94b5746e52f5 355 setpoint2 = (155.0*2.0*PI/360);
irisl 21:674fafb6301d 356 }
irisl 23:94b5746e52f5 357 if(setpoint2 < -(155.0*2.0*PI/360)) {
irisl 23:94b5746e52f5 358 setpoint2 = -(155.0*2.0*PI/360);
irisl 21:674fafb6301d 359 }
irisl 21:674fafb6301d 360 prev_setpoint2 = setpoint2;
irisl 23:94b5746e52f5 361 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 362 staat2 = 1;
irisl 21:674fafb6301d 363 }
irisl 21:674fafb6301d 364 }
irisl 21:674fafb6301d 365
irisl 28:c33a0658605e 366 // Motor2 balletje op het laagst slaan
irisl 21:674fafb6301d 367 void arm_laag ()
irisl 21:674fafb6301d 368 {
irisl 23:94b5746e52f5 369 speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 370 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 23:94b5746e52f5 371 if(setpoint2 > (155*2.0*PI/360)) { //setpoint in graden
irisl 23:94b5746e52f5 372 setpoint2 = (155*2.0*PI/360);
irisl 21:674fafb6301d 373 }
irisl 23:94b5746e52f5 374 if(setpoint2 < -(155.0*2.0*PI/360)) {
irisl 23:94b5746e52f5 375 setpoint2 = -(155.0*2.0*PI/360);
irisl 21:674fafb6301d 376 }
irisl 21:674fafb6301d 377 prev_setpoint2 = setpoint2;
irisl 23:94b5746e52f5 378 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
irisl 21:674fafb6301d 379 staat2 = 1;
irisl 21:674fafb6301d 380 }
irisl 21:674fafb6301d 381 }
irisl 21:674fafb6301d 382
irisl 28:c33a0658605e 383 // Motor2 arm terug zetten in beginstand
irisl 21:674fafb6301d 384 void arm_begin ()
irisl 21:674fafb6301d 385 {
irisl 27:691779624530 386 speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 21:674fafb6301d 387 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 21:674fafb6301d 388 if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
irisl 21:674fafb6301d 389 setpoint2 = (0.0*2.0*PI/360);
irisl 21:674fafb6301d 390 }
irisl 21:674fafb6301d 391 if(setpoint2 < -(0.0*2.0*PI/360)) {
irisl 21:674fafb6301d 392 setpoint2 = -(0.0*2.0*PI/360);
irisl 21:674fafb6301d 393 }
irisl 21:674fafb6301d 394 prev_setpoint2 = setpoint2;
irisl 21:674fafb6301d 395 }
irisl 21:674fafb6301d 396
irisl 28:c33a0658605e 397 // MOTOR aansturing
irisl 21:674fafb6301d 398 void looper_motor()
irisl 20:d40b6cba4280 399 {
irisl 21:674fafb6301d 400 //MOTOR1
irisl 21:674fafb6301d 401 pc.printf("%d \r\n", motor1.getPosition());
irisl 21:674fafb6301d 402 cur_pos_motor1 = motor1.getPosition();
irisl 21:674fafb6301d 403 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
irisl 21:674fafb6301d 404 pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
irisl 21:674fafb6301d 405 if (pwm1_percentage < -1.0) {
irisl 21:674fafb6301d 406 pwm1_percentage = -1.0;
irisl 21:674fafb6301d 407 }
irisl 21:674fafb6301d 408 if (pwm1_percentage > 1.0) {
irisl 21:674fafb6301d 409 pwm1_percentage = 1.0;
irisl 21:674fafb6301d 410 }
irisl 21:674fafb6301d 411 pwm_motor1.write(abs(pwm1_percentage));
irisl 21:674fafb6301d 412 if(pwm1_percentage > 0) {
irisl 21:674fafb6301d 413 motordir1 = 0;
irisl 21:674fafb6301d 414 } else {
irisl 21:674fafb6301d 415 motordir1 = 1;
irisl 21:674fafb6301d 416 }
irisl 21:674fafb6301d 417
irisl 21:674fafb6301d 418 //MOTOR2
irisl 21:674fafb6301d 419 cur_pos_motor2 = motor2.getPosition();
irisl 21:674fafb6301d 420 pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
irisl 21:674fafb6301d 421 pwm2_percentage = pid2(setpoint2, pos_motor2_rad); //
irisl 21:674fafb6301d 422 if (pwm2_percentage < -1.0) {
irisl 21:674fafb6301d 423 pwm2_percentage = -1.0;
irisl 21:674fafb6301d 424 }
irisl 21:674fafb6301d 425 if (pwm2_percentage > 1.0) {
irisl 21:674fafb6301d 426 pwm2_percentage = 1.0;
irisl 21:674fafb6301d 427 }
irisl 21:674fafb6301d 428 pwm_motor2.write(abs(pwm2_percentage));
irisl 21:674fafb6301d 429 if(pwm2_percentage > 0) {
irisl 21:674fafb6301d 430 motordir2 = 0;
irisl 21:674fafb6301d 431 } else {
irisl 21:674fafb6301d 432 motordir2 = 1;
irisl 21:674fafb6301d 433 }
irisl 21:674fafb6301d 434
irisl 21:674fafb6301d 435
irisl 21:674fafb6301d 436 //STATES
irisl 21:674fafb6301d 437
irisl 22:14f5161d7d7b 438 if (batje_hoek == 1) {
irisl 21:674fafb6301d 439 if(staat1 == 0) {
irisl 21:674fafb6301d 440 batje_rechts();
irisl 21:674fafb6301d 441 wait_iterator1 = 0;
irisl 21:674fafb6301d 442 } else if(staat1 ==1) {
irisl 21:674fafb6301d 443 wait_iterator1++;
irisl 27:691779624530 444 if(wait_iterator1 > 400) {
irisl 21:674fafb6301d 445 staat1 = 2;
irisl 28:c33a0658605e 446
irisl 27:691779624530 447 batje_begin_rechts();
irisl 27:691779624530 448 }
irisl 21:674fafb6301d 449 }
irisl 21:674fafb6301d 450 }
irisl 22:14f5161d7d7b 451 if (batje_hoek == 2) {
irisl 21:674fafb6301d 452 if(staat1 == 0) {
irisl 21:674fafb6301d 453 batje_links();
irisl 21:674fafb6301d 454 wait_iterator1 = 0;
irisl 21:674fafb6301d 455 } else if(staat1 ==1) {
irisl 21:674fafb6301d 456 wait_iterator1++;
irisl 27:691779624530 457 if(wait_iterator1 > 400) {
irisl 21:674fafb6301d 458 staat1 = 2;
irisl 28:c33a0658605e 459
irisl 27:691779624530 460 batje_begin_links ();
irisl 27:691779624530 461 }
irisl 21:674fafb6301d 462 }
irisl 21:674fafb6301d 463 }
irisl 21:674fafb6301d 464
irisl 22:14f5161d7d7b 465 if(arm_hoogte == 1) {
irisl 21:674fafb6301d 466 if(staat2 == 0) {
irisl 21:674fafb6301d 467 arm_laag();
irisl 21:674fafb6301d 468 wait_iterator2 = 0;
irisl 21:674fafb6301d 469 } else if(staat2 == 1) {
irisl 21:674fafb6301d 470 wait_iterator2++;
irisl 27:691779624530 471 if(wait_iterator2 > 400) {
irisl 21:674fafb6301d 472 staat2 = 2;
irisl 28:c33a0658605e 473
irisl 27:691779624530 474 arm_begin();
irisl 27:691779624530 475 }
irisl 21:674fafb6301d 476 }
irisl 21:674fafb6301d 477 }
irisl 22:14f5161d7d7b 478 if(arm_hoogte == 2) {
irisl 21:674fafb6301d 479 if(staat2 == 0) {
irisl 21:674fafb6301d 480 arm_mid();
irisl 21:674fafb6301d 481 wait_iterator2 = 0;
irisl 21:674fafb6301d 482 } else if(staat2 == 1) {
irisl 21:674fafb6301d 483 wait_iterator2++;
irisl 27:691779624530 484 if(wait_iterator2 > 400) {
irisl 21:674fafb6301d 485 staat2 = 2;
irisl 28:c33a0658605e 486
irisl 27:691779624530 487 arm_begin();
irisl 27:691779624530 488 }
irisl 21:674fafb6301d 489 }
irisl 21:674fafb6301d 490 }
irisl 22:14f5161d7d7b 491 if(arm_hoogte == 3) {
irisl 21:674fafb6301d 492 if(staat2 == 0) {
irisl 21:674fafb6301d 493 arm_hoog();
irisl 21:674fafb6301d 494 wait_iterator2 = 0;
irisl 21:674fafb6301d 495 } else if(staat2 == 1) {
irisl 21:674fafb6301d 496 wait_iterator2++;
irisl 27:691779624530 497 if(wait_iterator2 > 400) {
irisl 21:674fafb6301d 498 staat2 = 2;
irisl 28:c33a0658605e 499
irisl 27:691779624530 500 arm_begin();
irisl 27:691779624530 501 }
irisl 21:674fafb6301d 502 }
irisl 21:674fafb6301d 503 }
irisl 27:691779624530 504
irisl 20:d40b6cba4280 505 }
irisl 15:eb6498bb7ca0 506
irisl 27:691779624530 507
irisl 28:c33a0658605e 508 // Hoofdprogramma, hierin staat de aansturing vd LED
irisl 16:d65458b85734 509 int main()
jessekaiser 1:099b19376f16 510 {
irisl 21:674fafb6301d 511
irisl 21:674fafb6301d 512 pwm_motor1.period_us(100);
irisl 21:674fafb6301d 513 motor1.setPosition(0);
irisl 21:674fafb6301d 514 pwm_motor2.period_us(100);
irisl 21:674fafb6301d 515 motor2.setPosition(0);
jessekaiser 0:db396b9f4b4c 516 pc.baud(115200);
irisl 28:c33a0658605e 517 // Ticker EMG signaal meten
jessekaiser 0:db396b9f4b4c 518 Ticker log_timer;
jessekaiser 1:099b19376f16 519 //set up filters. Use external array for constants
irisl 9:f7ec578a17c0 520 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 9:f7ec578a17c0 521 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 9:f7ec578a17c0 522 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 9:f7ec578a17c0 523 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
irisl 28:c33a0658605e 524 // Uitvoeren van ticker EMG, sample frequentie 500Hz
irisl 26:9859a71456fd 525 log_timer.attach(looper, 0.002);
irisl 21:674fafb6301d 526
irisl 28:c33a0658605e 527 // Aanroepen van motoraansturing in motor ticker
irisl 21:674fafb6301d 528 Ticker looptimer;
irisl 21:674fafb6301d 529 looptimer.attach(looper_motor,TSAMP);
irisl 28:c33a0658605e 530
irisl 28:c33a0658605e 531 while(1) {
jessekaiser 6:8f4138a811e0 532
irisl 13:493a953a2a85 533 while(1) {
irisl 18:ed2afe6953de 534 pc.printf("Span de biceps aan om het instellen te starten.\n");
irisl 13:493a953a2a85 535 do {
irisl 16:d65458b85734 536 ShineRed();
irisl 17:dbbe4e126203 537 } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
irisl 28:c33a0658605e 538 if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen
irisl 28:c33a0658605e 539 BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
irisl 28:c33a0658605e 540 BlinkGreen(); // groen knipperen, meten van spieraanspanning
irisl 28:c33a0658605e 541 while (1) { // eerste loop, keuze voor de positie van het batje
irisl 18:ed2afe6953de 542 pc.printf("In de loop.\n");
irisl 28:c33a0658605e 543 if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden
irisl 18:ed2afe6953de 544 stopblinkgreen();
irisl 20:d40b6cba4280 545 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 546 ShineGreen();
irisl 18:ed2afe6953de 547 wait (4);
irisl 18:ed2afe6953de 548 break;
irisl 18:ed2afe6953de 549 }
irisl 28:c33a0658605e 550 if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links
irisl 18:ed2afe6953de 551 stopblinkgreen();
irisl 18:ed2afe6953de 552 pc.printf("ShineBlue.\n");
irisl 18:ed2afe6953de 553 ShineBlue();
irisl 22:14f5161d7d7b 554 batje_hoek = 2;
irisl 18:ed2afe6953de 555 wait(4);
irisl 18:ed2afe6953de 556 break;
irisl 28:c33a0658605e 557 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts
irisl 18:ed2afe6953de 558 stopblinkgreen();
irisl 20:d40b6cba4280 559 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 560 ShineRed();
irisl 22:14f5161d7d7b 561 batje_hoek = 1;
irisl 18:ed2afe6953de 562 wait (4);
irisl 18:ed2afe6953de 563 break;
irisl 18:ed2afe6953de 564 }
irisl 14:257026c95f22 565 }
irisl 19:1bd2fc3bce1e 566 BlinkGreen();
irisl 28:c33a0658605e 567 while (1) { // loop voor het instellen van de kracht
irisl 19:1bd2fc3bce1e 568 pc.printf("In de loop.\n");
irisl 28:c33a0658605e 569 if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan
irisl 19:1bd2fc3bce1e 570 stopblinkgreen();
irisl 20:d40b6cba4280 571 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 572 ShineGreen();
irisl 22:14f5161d7d7b 573 arm_hoogte = 3;
irisl 19:1bd2fc3bce1e 574 wait (4);
irisl 19:1bd2fc3bce1e 575 break;
irisl 19:1bd2fc3bce1e 576 }
irisl 28:c33a0658605e 577 if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan
irisl 19:1bd2fc3bce1e 578 stopblinkgreen();
irisl 19:1bd2fc3bce1e 579 pc.printf("ShineBlue.\n");
irisl 19:1bd2fc3bce1e 580 ShineBlue();
irisl 22:14f5161d7d7b 581 arm_hoogte = 1;
irisl 19:1bd2fc3bce1e 582 wait(4);
irisl 19:1bd2fc3bce1e 583 break;
irisl 28:c33a0658605e 584 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan
irisl 19:1bd2fc3bce1e 585 stopblinkgreen();
irisl 20:d40b6cba4280 586 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 587 ShineRed();
irisl 22:14f5161d7d7b 588 arm_hoogte = 2;
irisl 19:1bd2fc3bce1e 589 wait (4);
irisl 19:1bd2fc3bce1e 590 break;
irisl 19:1bd2fc3bce1e 591 }
irisl 19:1bd2fc3bce1e 592 }
irisl 26:9859a71456fd 593
irisl 14:257026c95f22 594 }
irisl 19:1bd2fc3bce1e 595
jessekaiser 0:db396b9f4b4c 596 }
irisl 16:d65458b85734 597 }
irisl 19:1bd2fc3bce1e 598 }