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@35:72d6cd2b2162, 2014-11-04 (annotated)

Committer:
irisl
Date:
Tue Nov 04 09:46:26 2014 +0000
Revision:
35:72d6cd2b2162
Parent:
34:7e0bf3e9f135
Child:
36:2166eaca545e
kleine aanpassingen, heeft problemen met de filtering;

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 35:72d6cd2b2162 9 #define K_P1 (3.5)
irisl 33:381bc6635528 10 #define K_I1 (0.01)
irisl 35:72d6cd2b2162 11 #define K_P2 (3.5)
irisl 35:72d6cd2b2162 12 #define K_I2 (0.04)
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
jessekaiser 32:e35bedc7cefd 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 33:381bc6635528 34 HIDScope scope(3);
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 35:72d6cd2b2162 78 float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 , 0.2538762935939654, -0.4107804719728833, 0.2538762935939654, 0.4107804719728833, 0.4922474128120692};
irisl 35:72d6cd2b2162 79 //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 29:d0dab8921e9d 90 //gemiddelde EMG waarden over 250 sample stappen
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 33:381bc6635528 145 scope.set(0,emg_value1); //Raw EMG signal biceps
irisl 9:f7ec578a17c0 146 //scope.set(1,emg_value2); //Raw EMG signal Deltoid
irisl 33:381bc6635528 147 scope.set(1,filtered_biceps); //processed float biceps
irisl 33:381bc6635528 148 scope.set(2,filtered_average_bi); //processed float deltoid
irisl 33:381bc6635528 149 //scope.set(2,filtered_deltoid); //processed float biceps
irisl 33:381bc6635528 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 16:d65458b85734 187 }
irisl 16:d65458b85734 188
irisl 18:ed2afe6953de 189 void stopblinkgreen()
irisl 16:d65458b85734 190 {
irisl 16:d65458b85734 191 ledticker.detach();
irisl 16:d65458b85734 192 }
irisl 16:d65458b85734 193
irisl 28:c33a0658605e 194 // Groen schijnen
irisl 13:493a953a2a85 195 void ShineGreen ()
irisl 13:493a953a2a85 196 {
irisl 21:674fafb6301d 197 myledred = 0;
irisl 21:674fafb6301d 198 myledgreen = 1;
irisl 21:674fafb6301d 199 myledblue = 0;
irisl 13:493a953a2a85 200 }
irisl 13:493a953a2a85 201
irisl 28:c33a0658605e 202 // Blauw schijnen
irisl 13:493a953a2a85 203 void ShineBlue ()
irisl 13:493a953a2a85 204 {
irisl 21:674fafb6301d 205 myledred = 0;
irisl 21:674fafb6301d 206 myledgreen = 0;
irisl 21:674fafb6301d 207 myledblue = 1;
irisl 13:493a953a2a85 208 }
irisl 13:493a953a2a85 209
irisl 28:c33a0658605e 210 // Rood schijnen
irisl 13:493a953a2a85 211 void ShineRed ()
irisl 13:493a953a2a85 212 {
irisl 21:674fafb6301d 213 myledred = 1;
irisl 21:674fafb6301d 214 myledgreen = 0;
irisl 21:674fafb6301d 215 myledblue = 0;
irisl 13:493a953a2a85 216 }
irisl 13:493a953a2a85 217
irisl 20:d40b6cba4280 218 // MOTORFUNCTIES
irisl 20:d40b6cba4280 219
irisl 28:c33a0658605e 220 // Motor1 = batje
irisl 28:c33a0658605e 221 // Motor2 = arm
irisl 28:c33a0658605e 222
jessekaiser 29:d0dab8921e9d 223 void clamp(float* in, float min, float max) //Clamp geeft een maximum en minimum limiet aan een functie
irisl 21:674fafb6301d 224 {
jessekaiser 29:d0dab8921e9d 225 *in > min ? /*(*/*in < max? : *in = max : *in = min;
irisl 21:674fafb6301d 226 }
irisl 21:674fafb6301d 227
irisl 28:c33a0658605e 228 // PI-regelaar motor1: batje
irisl 21:674fafb6301d 229 float pid1(float setpoint1, float measurement1)
irisl 20:d40b6cba4280 230 {
irisl 21:674fafb6301d 231 float error1;
irisl 21:674fafb6301d 232 float out_p1 = 0;
irisl 21:674fafb6301d 233 static float out_i1 = 0;
irisl 21:674fafb6301d 234 error1 = (setpoint1 - measurement1);
irisl 21:674fafb6301d 235 out_p1 = error1*K_P1;
irisl 21:674fafb6301d 236 out_i1 += error1*K_I1;
irisl 21:674fafb6301d 237 clamp(&out_i1,-I_LIMIT,I_LIMIT);
irisl 21:674fafb6301d 238 return out_p1 + out_i1;
irisl 20:d40b6cba4280 239 }
irisl 20:d40b6cba4280 240
irisl 28:c33a0658605e 241 // PI-regelaar motor2: arm
irisl 21:674fafb6301d 242 float pid2(float setpoint2, float measurement2)
irisl 20:d40b6cba4280 243 {
irisl 21:674fafb6301d 244 float error2;
irisl 21:674fafb6301d 245 float out_p2 = 0;
irisl 21:674fafb6301d 246 static float out_i2 = 0;
irisl 21:674fafb6301d 247 error2 = (setpoint2 - measurement2);
irisl 21:674fafb6301d 248 out_p2 = error2*K_P2;
irisl 21:674fafb6301d 249 out_i2 += error2*K_I2;
irisl 21:674fafb6301d 250 clamp(&out_i2,-I_LIMIT,I_LIMIT);
irisl 21:674fafb6301d 251 return out_p2 + out_i2;
irisl 21:674fafb6301d 252 }
irisl 28:c33a0658605e 253
irisl 28:c33a0658605e 254 // Variabelen
irisl 21:674fafb6301d 255 float prev_setpoint1 = 0;
irisl 21:674fafb6301d 256 float setpoint1 = 0;
irisl 21:674fafb6301d 257 float prev_setpoint2 = 0;
irisl 21:674fafb6301d 258 float setpoint2 = 0;
irisl 21:674fafb6301d 259
irisl 28:c33a0658605e 260 // Functies motoren
irisl 28:c33a0658605e 261
irisl 28:c33a0658605e 262 // Motor1 links draaien
irisl 21:674fafb6301d 263 void batje_links ()
irisl 21:674fafb6301d 264 {
irisl 21:674fafb6301d 265 speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 29:d0dab8921e9d 266 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; //bepalen van de setpoint
jessekaiser 29:d0dab8921e9d 267 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 268 setpoint1 = (11.3*2.3*2.0*PI/360); //Hier wordt er een grens bepaald voor de hoek.
irisl 21:674fafb6301d 269 }
irisl 22:14f5161d7d7b 270 if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
irisl 22:14f5161d7d7b 271 setpoint1 = -(11.3*2.3*2.0*PI/360);
irisl 21:674fafb6301d 272 }
irisl 33:381bc6635528 273 if(setpoint1 <= -((11.3*2.3*2.0*PI/360)-0.1)) {
irisl 21:674fafb6301d 274 staat1 = 1;
jessekaiser 29:d0dab8921e9d 275 prev_setpoint1 = setpoint1;
irisl 21:674fafb6301d 276 }
irisl 21:674fafb6301d 277 }
jessekaiser 29:d0dab8921e9d 278 // Motor1 rechts draaien
irisl 33:381bc6635528 279 void batje_rechts ()
irisl 33:381bc6635528 280 {
irisl 33:381bc6635528 281 speed1_rad = 1.0;
irisl 33:381bc6635528 282 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 33:381bc6635528 283 if(setpoint1 > (11.3*2.3*2.0*PI/360)) {
irisl 33:381bc6635528 284 setpoint1 = (11.3*2.3*2.0*PI/360);
jessekaiser 29:d0dab8921e9d 285 }
irisl 33:381bc6635528 286 if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
irisl 33:381bc6635528 287 setpoint1 = -(11.3*2.3*2.0*PI/360);
irisl 33:381bc6635528 288 }
irisl 33:381bc6635528 289 prev_setpoint1 = setpoint1;
irisl 33:381bc6635528 290 if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
irisl 33:381bc6635528 291 staat1 = 1;
irisl 33:381bc6635528 292 }
irisl 33:381bc6635528 293 }
jessekaiser 29:d0dab8921e9d 294
jessekaiser 29:d0dab8921e9d 295 //Motor1 na links draaien weer terug laten draaien naar beginstand
irisl 33:381bc6635528 296 void batje_begin_links ()
irisl 33:381bc6635528 297 {
irisl 33:381bc6635528 298 speed1_rad = 1.0;
irisl 33:381bc6635528 299 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 33:381bc6635528 300 if(setpoint1 > (0*2.3*2.0*PI/360)) {
irisl 33:381bc6635528 301 setpoint1 = (0*2.3*2.0*PI/360);
jessekaiser 29:d0dab8921e9d 302 }
irisl 33:381bc6635528 303 //if(setpoint1 < -(0*2.3*2.0*PI/360)) {
irisl 35:72d6cd2b2162 304 // setpoint1 = -(0*2.3*2.0*PI/360);
irisl 33:381bc6635528 305 //}
irisl 33:381bc6635528 306 prev_setpoint1 = setpoint1;
irisl 33:381bc6635528 307 }
jessekaiser 29:d0dab8921e9d 308
jessekaiser 29:d0dab8921e9d 309 //Motor1 na links draaien weer terug laten draaien naar beginstand
irisl 33:381bc6635528 310 void batje_begin_rechts ()
irisl 33:381bc6635528 311 {
irisl 33:381bc6635528 312 speed1_rad = -1.0;
irisl 33:381bc6635528 313 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
irisl 33:381bc6635528 314 //if(setpoint1 > (0*2.3*2.0*PI/360)) {
irisl 33:381bc6635528 315 // setpoint1 = (0*2.3*2.0*PI/360);
irisl 33:381bc6635528 316 //}
irisl 33:381bc6635528 317 if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
irisl 33:381bc6635528 318 setpoint1 = -(0.0*2.3*2.0*PI/360);
jessekaiser 29:d0dab8921e9d 319 }
irisl 33:381bc6635528 320 prev_setpoint1 = setpoint1;
irisl 33:381bc6635528 321 }
jessekaiser 29:d0dab8921e9d 322
jessekaiser 29:d0dab8921e9d 323 // Motor2 balletje op zn hoogst slaan
irisl 33:381bc6635528 324 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
irisl 33:381bc6635528 325 {
irisl 33:381bc6635528 326 speed2_rad = -6.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 34:7e0bf3e9f135 327 //setpoint2 = -(120*2.0*PI/360);
irisl 34:7e0bf3e9f135 328 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 35:72d6cd2b2162 329 if(setpoint2 > (120*2.0*PI/360)) { //setpoint in graden
irisl 34:7e0bf3e9f135 330 setpoint2 = (120.0*2.0*PI/360);
jessekaiser 29:d0dab8921e9d 331 }
irisl 35:72d6cd2b2162 332 if(setpoint2 < -(120.0*2.0*PI/360)) {
irisl 35:72d6cd2b2162 333 setpoint2 = -(120.0*2.0*PI/360);
irisl 33:381bc6635528 334 }
irisl 35:72d6cd2b2162 335
irisl 33:381bc6635528 336 prev_setpoint2 = setpoint2;
irisl 35:72d6cd2b2162 337 if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
irisl 33:381bc6635528 338 staat2 = 1;
irisl 33:381bc6635528 339 }
irisl 33:381bc6635528 340 }
jessekaiser 29:d0dab8921e9d 341
jessekaiser 29:d0dab8921e9d 342 // Motor2 balletje in het midden slaan
irisl 33:381bc6635528 343 void arm_mid ()
irisl 33:381bc6635528 344 {
irisl 33:381bc6635528 345 speed2_rad = -4.0;
irisl 33:381bc6635528 346 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 35:72d6cd2b2162 347 if(setpoint2 > (120.0*2.0*PI/360)) {
irisl 35:72d6cd2b2162 348 setpoint2 = (120.0*2.0*PI/360);
irisl 33:381bc6635528 349 }
irisl 35:72d6cd2b2162 350 if(setpoint2 < -(120.0*2.0*PI/360)) {
irisl 35:72d6cd2b2162 351 setpoint2 = -(120.0*2.0*PI/360);
irisl 33:381bc6635528 352 }
irisl 33:381bc6635528 353 prev_setpoint2 = setpoint2;
irisl 35:72d6cd2b2162 354 if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
irisl 33:381bc6635528 355 staat2 = 1;
irisl 33:381bc6635528 356 }
irisl 33:381bc6635528 357 }
irisl 33:381bc6635528 358
irisl 33:381bc6635528 359 // Motor2 balletje op het laagst slaan
irisl 33:381bc6635528 360 void arm_laag ()
irisl 33:381bc6635528 361 {
irisl 33:381bc6635528 362 speed2_rad = -2.0;
irisl 33:381bc6635528 363 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 35:72d6cd2b2162 364 if(setpoint2 > (120*2.0*PI/360)) {
irisl 35:72d6cd2b2162 365 setpoint2 = (120*2.0*PI/360);
irisl 33:381bc6635528 366 }
irisl 35:72d6cd2b2162 367 if(setpoint2 < -(120.0*2.0*PI/360)) {
irisl 35:72d6cd2b2162 368 setpoint2 = -(120.0*2.0*PI/360);
irisl 33:381bc6635528 369 }
irisl 33:381bc6635528 370 prev_setpoint2 = setpoint2;
irisl 35:72d6cd2b2162 371 if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
irisl 33:381bc6635528 372 staat2 = 1;
irisl 33:381bc6635528 373 }
irisl 33:381bc6635528 374 }
irisl 33:381bc6635528 375
irisl 33:381bc6635528 376 // Motor2 arm terug zetten in beginstand
irisl 33:381bc6635528 377 void arm_begin ()
irisl 33:381bc6635528 378 {
irisl 33:381bc6635528 379 speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
irisl 33:381bc6635528 380 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
irisl 33:381bc6635528 381 if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
irisl 33:381bc6635528 382 setpoint2 = (0.0*2.0*PI/360);
irisl 33:381bc6635528 383 }
irisl 33:381bc6635528 384 //if(setpoint2 < -(0.0*2.0*PI/360)) {
irisl 33:381bc6635528 385 // setpoint2 = -(0.0*2.0*PI/360);
irisl 33:381bc6635528 386 //}
irisl 33:381bc6635528 387 prev_setpoint2 = setpoint2;
irisl 33:381bc6635528 388 }
irisl 33:381bc6635528 389
irisl 33:381bc6635528 390 // MOTOR aansturing
irisl 33:381bc6635528 391 void looper_motor()
irisl 33:381bc6635528 392 {
irisl 33:381bc6635528 393 pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition()); //Geeft de posities weer van beide motoren met een sample frequentie van 0.005
irisl 33:381bc6635528 394
irisl 33:381bc6635528 395 //MOTOR1
irisl 33:381bc6635528 396 cur_pos_motor1 = motor1.getPosition();
irisl 33:381bc6635528 397 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.
irisl 33:381bc6635528 398 pwm_out1 = pid1(setpoint1, pos_motor1_rad);
irisl 33:381bc6635528 399 if (pwm_out1 < -1.0) { //Hier wordt de grens voor de pwm waarde ingesteld.
irisl 33:381bc6635528 400 pwm_out1 = -1.0;
irisl 33:381bc6635528 401 }
irisl 33:381bc6635528 402 if (pwm_out1 > 1.0) {
irisl 33:381bc6635528 403 pwm_out1 = 1.0;
irisl 33:381bc6635528 404 }
irisl 33:381bc6635528 405 pwm_motor1.write(abs(pwm_out1));
irisl 33:381bc6635528 406 if(pwm_out1 > 0) {
irisl 33:381bc6635528 407 motordir1 = 0;
irisl 33:381bc6635528 408 } else {
irisl 33:381bc6635528 409 motordir1 = 1;
irisl 33:381bc6635528 410 }
irisl 33:381bc6635528 411
irisl 33:381bc6635528 412 //MOTOR2
irisl 33:381bc6635528 413 cur_pos_motor2 = motor2.getPosition();
irisl 33:381bc6635528 414 pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
irisl 33:381bc6635528 415 pwm_out2 = pid2(setpoint2, pos_motor2_rad); //
irisl 33:381bc6635528 416 if (pwm_out2 < -1.0) {
irisl 33:381bc6635528 417 pwm_out2 = -1.0;
irisl 33:381bc6635528 418 }
irisl 33:381bc6635528 419 if (pwm_out2 > 1.0) {
irisl 33:381bc6635528 420 pwm_out2 = 1.0;
irisl 33:381bc6635528 421 }
irisl 33:381bc6635528 422 pwm_motor2.write(abs(pwm_out2));
irisl 33:381bc6635528 423 if(pwm_out2 > 0) {
irisl 33:381bc6635528 424 motordir2 = 0;
irisl 33:381bc6635528 425 } else {
irisl 33:381bc6635528 426 motordir2 = 1;
irisl 33:381bc6635528 427 }
irisl 33:381bc6635528 428
irisl 33:381bc6635528 429
irisl 33:381bc6635528 430 //STATES
irisl 33:381bc6635528 431
irisl 33:381bc6635528 432 //Het batje draait naar opgegeven positie, doet dan een bepaalde tijd niks (wait_iterator), en draait daarna weer terug
irisl 33:381bc6635528 433 if (batje_hoek == 1) {
irisl 33:381bc6635528 434 if(staat1 == 0) {
irisl 33:381bc6635528 435 batje_rechts();
irisl 33:381bc6635528 436 wait_iterator1 = 0;
irisl 33:381bc6635528 437 } else if(staat1 ==1) {
irisl 33:381bc6635528 438 wait_iterator1++;
irisl 35:72d6cd2b2162 439 }
irisl 35:72d6cd2b2162 440 if(wait_iterator1 > 1200) {
irisl 35:72d6cd2b2162 441 staat1 = 2;
irisl 33:381bc6635528 442
irisl 35:72d6cd2b2162 443 batje_begin_rechts();
jessekaiser 29:d0dab8921e9d 444 }
jessekaiser 29:d0dab8921e9d 445 }
irisl 35:72d6cd2b2162 446
irisl 33:381bc6635528 447 if (batje_hoek == 2) {
irisl 33:381bc6635528 448 if(staat1 == 0) {
irisl 33:381bc6635528 449 batje_links();
irisl 33:381bc6635528 450 wait_iterator1 = 0;
irisl 33:381bc6635528 451 } else if(staat1 ==1) {
irisl 33:381bc6635528 452 wait_iterator1++;
irisl 35:72d6cd2b2162 453 }
irisl 35:72d6cd2b2162 454 if(wait_iterator1 > 1200) {
irisl 35:72d6cd2b2162 455 staat1 = 2;
irisl 21:674fafb6301d 456
irisl 35:72d6cd2b2162 457 batje_begin_links ();
jessekaiser 29:d0dab8921e9d 458 }
irisl 21:674fafb6301d 459 }
irisl 21:674fafb6301d 460
irisl 35:72d6cd2b2162 461
irisl 33:381bc6635528 462 if(arm_hoogte == 1) {
irisl 33:381bc6635528 463 if(staat2 == 0) {
irisl 33:381bc6635528 464 arm_laag();
irisl 33:381bc6635528 465 wait_iterator2 = 0;
irisl 33:381bc6635528 466 } else if(staat2 == 1) {
irisl 33:381bc6635528 467 wait_iterator2++;
irisl 35:72d6cd2b2162 468 }
irisl 35:72d6cd2b2162 469 if(wait_iterator2 > 1200) {
irisl 35:72d6cd2b2162 470 staat2 = 2;
irisl 33:381bc6635528 471
irisl 35:72d6cd2b2162 472 arm_begin();
jessekaiser 29:d0dab8921e9d 473 }
irisl 33:381bc6635528 474 }
irisl 35:72d6cd2b2162 475
irisl 33:381bc6635528 476 if(arm_hoogte == 2) {
irisl 33:381bc6635528 477 if(staat2 == 0) {
irisl 33:381bc6635528 478 arm_mid();
irisl 33:381bc6635528 479 wait_iterator2 = 0;
irisl 33:381bc6635528 480 } else if(staat2 == 1) {
irisl 35:72d6cd2b2162 481 } wait_iterator2++;
irisl 35:72d6cd2b2162 482 if(wait_iterator2 > 400) {
irisl 35:72d6cd2b2162 483 staat2 = 2;
irisl 33:381bc6635528 484
irisl 35:72d6cd2b2162 485 arm_begin();
jessekaiser 29:d0dab8921e9d 486 }
irisl 33:381bc6635528 487 }
irisl 35:72d6cd2b2162 488
irisl 33:381bc6635528 489 if(arm_hoogte == 3) {
irisl 33:381bc6635528 490 if(staat2 == 0) {
irisl 33:381bc6635528 491 arm_hoog();
irisl 33:381bc6635528 492 wait_iterator2 = 0;
irisl 33:381bc6635528 493 } else if(staat2 == 1) {
irisl 33:381bc6635528 494 wait_iterator2++;
irisl 35:72d6cd2b2162 495 }
irisl 35:72d6cd2b2162 496 if(wait_iterator2 > 400) {
irisl 35:72d6cd2b2162 497 staat2 = 2;
irisl 35:72d6cd2b2162 498 arm_begin();
irisl 33:381bc6635528 499 }
irisl 21:674fafb6301d 500 }
irisl 35:72d6cd2b2162 501 }
irisl 21:674fafb6301d 502
irisl 21:674fafb6301d 503
irisl 21:674fafb6301d 504
jessekaiser 29:d0dab8921e9d 505 // Hoofdprogramma, hierin staat de aansturing vd LED
irisl 33:381bc6635528 506 int main()
irisl 33:381bc6635528 507 {
irisl 35:72d6cd2b2162 508 float emg_arm1 = 0.45;
irisl 35:72d6cd2b2162 509 float emg_arm2 = 0.45;
irisl 33:381bc6635528 510 pwm_motor1.period_us(100);
irisl 33:381bc6635528 511 motor1.setPosition(0);
irisl 33:381bc6635528 512 pwm_motor2.period_us(100);
irisl 33:381bc6635528 513 motor2.setPosition(0);
irisl 33:381bc6635528 514 pc.baud(115200);
irisl 33:381bc6635528 515 // Ticker EMG signaal meten
irisl 33:381bc6635528 516 Ticker log_timer;
irisl 33:381bc6635528 517 //set up filters. Use external array for constants
irisl 33:381bc6635528 518 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 33:381bc6635528 519 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 33:381bc6635528 520 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 33:381bc6635528 521 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
irisl 33:381bc6635528 522 // Uitvoeren van ticker EMG, sample frequentie 500Hz
irisl 33:381bc6635528 523 log_timer.attach(looper, 0.002);
irisl 28:c33a0658605e 524
irisl 33:381bc6635528 525 // Aanroepen van motoraansturing in motor ticker
irisl 33:381bc6635528 526 Ticker looptimer;
irisl 33:381bc6635528 527 looptimer.attach(looper_motor,TSAMP);
irisl 33:381bc6635528 528
irisl 33:381bc6635528 529 while(1) {
jessekaiser 6:8f4138a811e0 530
irisl 13:493a953a2a85 531 while(1) {
irisl 33:381bc6635528 532 pc.printf("Span de biceps aan om het instellen te starten.\n");
irisl 33:381bc6635528 533 do {
irisl 33:381bc6635528 534 ShineRed();
irisl 35:72d6cd2b2162 535 } while(filtered_average_bi < emg_arm1 && filtered_average_del < emg_arm2); // In rust, geen meting
irisl 35:72d6cd2b2162 536 if (filtered_average_bi > 0.04) { // Beginnen met meting wanneer biceps wordt aangespannen
irisl 33:381bc6635528 537 BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
irisl 33:381bc6635528 538 BlinkGreen(); // groen knipperen, meten van spieraanspanning
irisl 33:381bc6635528 539 while (1) { // eerste loop, keuze voor de positie van het batje
irisl 33:381bc6635528 540 pc.printf("In de loop.\n");
irisl 35:72d6cd2b2162 541 if (filtered_average_bi > emg_arm1 && filtered_average_del > emg_arm2) { //bi en del aangespannen --> batje in het midden
irisl 33:381bc6635528 542 stopblinkgreen();
irisl 33:381bc6635528 543 pc.printf("ShineGreen.\n");
irisl 33:381bc6635528 544 ShineGreen();
irisl 33:381bc6635528 545 wait (4);
irisl 33:381bc6635528 546 break;
irisl 18:ed2afe6953de 547 }
irisl 35:72d6cd2b2162 548 if (filtered_average_bi < emg_arm1 && filtered_average_del > emg_arm2) { // del aanspannen --> batje naar links
irisl 33:381bc6635528 549 stopblinkgreen();
irisl 33:381bc6635528 550 pc.printf("ShineBlue.\n");
irisl 33:381bc6635528 551 ShineBlue();
irisl 33:381bc6635528 552 batje_hoek = 2;
irisl 33:381bc6635528 553 wait(4);
irisl 33:381bc6635528 554 break;
irisl 35:72d6cd2b2162 555 } else if (filtered_average_bi > emg_arm1 && filtered_average_del < emg_arm2) { // bi aanspannen --> batje naar rechts
irisl 33:381bc6635528 556 stopblinkgreen();
irisl 33:381bc6635528 557 pc.printf("ShineRed.\n");
irisl 33:381bc6635528 558 ShineRed();
irisl 33:381bc6635528 559 batje_hoek = 1;
irisl 33:381bc6635528 560 wait (4);
irisl 33:381bc6635528 561 break;
irisl 18:ed2afe6953de 562 }
irisl 33:381bc6635528 563 }
irisl 33:381bc6635528 564 BlinkGreen();
irisl 33:381bc6635528 565 while (1) { // loop voor het instellen van de kracht
irisl 33:381bc6635528 566 pc.printf("In de loop.\n");
irisl 35:72d6cd2b2162 567 if (filtered_average_bi > emg_arm1 && filtered_average_del > emg_arm2) { // bi en del aanspannen --> hoog slaan
irisl 33:381bc6635528 568 stopblinkgreen();
irisl 33:381bc6635528 569 pc.printf("ShineGreen.\n");
irisl 33:381bc6635528 570 ShineGreen();
irisl 33:381bc6635528 571 arm_hoogte = 3;
irisl 33:381bc6635528 572 wait (4);
irisl 33:381bc6635528 573 break;
irisl 33:381bc6635528 574 }
irisl 35:72d6cd2b2162 575 if (filtered_average_bi < emg_arm1 && filtered_average_del > emg_arm2) { // del aanspannen --> laag slaan
irisl 33:381bc6635528 576 stopblinkgreen();
irisl 33:381bc6635528 577 pc.printf("ShineBlue.\n");
irisl 33:381bc6635528 578 ShineBlue();
irisl 33:381bc6635528 579 arm_hoogte = 1;
irisl 33:381bc6635528 580 wait(4);
irisl 33:381bc6635528 581 break;
irisl 35:72d6cd2b2162 582 } else if (filtered_average_bi > emg_arm1 && filtered_average_del < emg_arm2) { // bi aanspannen --> midden slaan
irisl 33:381bc6635528 583 stopblinkgreen();
irisl 33:381bc6635528 584 pc.printf("ShineRed.\n");
irisl 33:381bc6635528 585 ShineRed();
irisl 33:381bc6635528 586 arm_hoogte = 2;
irisl 33:381bc6635528 587 wait (4);
irisl 33:381bc6635528 588 break;
irisl 33:381bc6635528 589 }
irisl 19:1bd2fc3bce1e 590 }
irisl 26:9859a71456fd 591
irisl 14:257026c95f22 592 }
jessekaiser 0:db396b9f4b4c 593 }
irisl 33:381bc6635528 594 }
irisl 33:381bc6635528 595 }