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@29:d0dab8921e9d, 2014-11-03 (annotated)

Committer:
jessekaiser
Date:
Mon Nov 03 15:37:48 2014 +0000
Revision:
29:d0dab8921e9d
Parent:
28:c33a0658605e
Child:
30:7a0a3c272308
wat uitleg bijgeschreven

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