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@20:d40b6cba4280, 2014-10-29 (annotated)

Committer:
irisl
Date:
Wed Oct 29 15:08:30 2014 +0000
Revision:
20:d40b6cba4280
Parent:
19:1bd2fc3bce1e
Child:
21:674fafb6301d
Met werkende motor in de presets

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"
jessekaiser 0:db396b9f4b4c 6
irisl 20:d40b6cba4280 7 #define TSAMP 0.01
irisl 20:d40b6cba4280 8 #define K_P (0.1)
irisl 20:d40b6cba4280 9 #define K_I (0.03 *TSAMP)
irisl 20:d40b6cba4280 10 #define K_D (0.001 /TSAMP)
irisl 20:d40b6cba4280 11 #define I_LIMIT 1.
irisl 20:d40b6cba4280 12
irisl 20:d40b6cba4280 13 #define M1_PWM PTC8
irisl 20:d40b6cba4280 14 #define M1_DIR PTC9
irisl 20:d40b6cba4280 15 #define M2_PWM PTA5
irisl 20:d40b6cba4280 16 #define M2_DIR PTA4
irisl 20:d40b6cba4280 17
irisl 20:d40b6cba4280 18 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
irisl 20:d40b6cba4280 19
irisl 20:d40b6cba4280 20 Serial pc(USBTX, USBRX);
irisl 20:d40b6cba4280 21
jessekaiser 0:db396b9f4b4c 22 DigitalOut myled1(LED_RED);
jessekaiser 0:db396b9f4b4c 23 DigitalOut myled2(LED_GREEN);
jessekaiser 0:db396b9f4b4c 24 DigitalOut myled3(LED_BLUE);
jessekaiser 0:db396b9f4b4c 25
jessekaiser 0:db396b9f4b4c 26 //Define objects
jessekaiser 3:0895fa0a6ca4 27 AnalogIn emg0(PTB1); //Analog input
jessekaiser 2:39e1bde54e73 28 AnalogIn emg1(PTB2); //Analog input
irisl 13:493a953a2a85 29 HIDScope scope(2);
jessekaiser 0:db396b9f4b4c 30
irisl 20:d40b6cba4280 31 //motor 25D
irisl 20:d40b6cba4280 32 Encoder motor1(PTD3,PTD5,true); //wit, geel
irisl 20:d40b6cba4280 33 PwmOut pwm_motor1(M2_PWM);
irisl 20:d40b6cba4280 34 DigitalOut motordir1(M2_DIR);
irisl 20:d40b6cba4280 35
irisl 20:d40b6cba4280 36 //motor2 37D
irisl 20:d40b6cba4280 37 Encoder motor2(PTD2, PTD0,true); //wit, geel
irisl 20:d40b6cba4280 38 PwmOut pwm_motor2(M1_PWM);
irisl 20:d40b6cba4280 39 DigitalOut motordir2(M1_DIR);
irisl 20:d40b6cba4280 40
irisl 20:d40b6cba4280 41 float speed1;
irisl 20:d40b6cba4280 42 float hoek1;
irisl 20:d40b6cba4280 43 float speed2;
irisl 20:d40b6cba4280 44 float hoek2;
irisl 20:d40b6cba4280 45
irisl 20:d40b6cba4280 46 bool flip=false;
irisl 20:d40b6cba4280 47
irisl 20:d40b6cba4280 48 void attime()
irisl 20:d40b6cba4280 49 {
irisl 20:d40b6cba4280 50 flip = !flip;
irisl 20:d40b6cba4280 51 }
irisl 20:d40b6cba4280 52
irisl 20:d40b6cba4280 53
irisl 20:d40b6cba4280 54 // EMG
irisl 20:d40b6cba4280 55
irisl 9:f7ec578a17c0 56 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
irisl 9:f7ec578a17c0 57 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
jessekaiser 2:39e1bde54e73 58 //lowpass filter settings: Fc = 225 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 0:db396b9f4b4c 59 float lowpass_const[] = {0.8005910266528647, 1.6011820533057295, 0.8005910266528647, -1.5610153912536877, -0.6413487153577715};
jessekaiser 0:db396b9f4b4c 60 //state values
irisl 9:f7ec578a17c0 61 float lowpass_biceps_states[4];
irisl 9:f7ec578a17c0 62 float lowpass_deltoid_states[4];
irisl 9:f7ec578a17c0 63 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
irisl 9:f7ec578a17c0 64 arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
jessekaiser 2:39e1bde54e73 65 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB
irisl 9:f7ec578a17c0 66 float highnotch_const[] = {0.956542835577484, -1.913085671154968, 0.956542835577484, 1.911196288237583, -0.914975054072353,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
jessekaiser 0:db396b9f4b4c 67 //state values
irisl 9:f7ec578a17c0 68 float highnotch_biceps_states[8];
irisl 9:f7ec578a17c0 69 float highnotch_deltoid_states[8];
jessekaiser 2:39e1bde54e73 70
jessekaiser 2:39e1bde54e73 71 //De globale variabele voor het gefilterde EMG signaal
jessekaiser 2:39e1bde54e73 72 float filtered_biceps;
jessekaiser 2:39e1bde54e73 73 float filtered_deltoid;
irisl 12:9e6e49af9304 74 float filtered_average_bi;
irisl 12:9e6e49af9304 75 float filtered_average_del;
jessekaiser 0:db396b9f4b4c 76
jessekaiser 0:db396b9f4b4c 77
irisl 12:9e6e49af9304 78 void average_biceps(float filtered_biceps,float *average)
irisl 12:9e6e49af9304 79 {
irisl 12:9e6e49af9304 80 static float total=0;
irisl 12:9e6e49af9304 81 static float number=0;
irisl 12:9e6e49af9304 82 total = total + filtered_biceps;
irisl 12:9e6e49af9304 83 number = number + 1;
irisl 18:ed2afe6953de 84 if ( number == 500) {
irisl 18:ed2afe6953de 85 *average = total/500;
irisl 12:9e6e49af9304 86 total = 0;
irisl 12:9e6e49af9304 87 number = 0;
irisl 12:9e6e49af9304 88 }
irisl 12:9e6e49af9304 89 }
irisl 12:9e6e49af9304 90
irisl 12:9e6e49af9304 91 void average_deltoid(float filtered_input,float *average_output)
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_input;
irisl 12:9e6e49af9304 96 number = number + 1;
irisl 18:ed2afe6953de 97 if ( number == 500) {
irisl 18:ed2afe6953de 98 *average_output = total/500;
irisl 12:9e6e49af9304 99 total = 0;
irisl 12:9e6e49af9304 100 number = 0;
irisl 12:9e6e49af9304 101 }
irisl 12:9e6e49af9304 102 }
irisl 12:9e6e49af9304 103
jessekaiser 0:db396b9f4b4c 104 /** Looper function
jessekaiser 0:db396b9f4b4c 105 * functions used for Ticker and Timeout should be of type void <name>(void)
jessekaiser 0:db396b9f4b4c 106 * i.e. no input arguments, no output arguments.
jessekaiser 0:db396b9f4b4c 107 * if you want to change a variable that you use in other places (for example in main)
jessekaiser 0:db396b9f4b4c 108 * you will have to make that variable global in order to be able to reach it both from
jessekaiser 0:db396b9f4b4c 109 * the function called at interrupt time, and in the main function.
jessekaiser 0:db396b9f4b4c 110 * To make a variable global, define it under the includes.
jessekaiser 0:db396b9f4b4c 111 * variables that are changed in the interrupt routine (written to) should be made
jessekaiser 0:db396b9f4b4c 112 * 'volatile' to let the compiler know that those values may change outside the current context.
jessekaiser 0:db396b9f4b4c 113 * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value"
jessekaiser 0:db396b9f4b4c 114 * in the example below, the variable is not re-used in the main function, and is thus declared
jessekaiser 0:db396b9f4b4c 115 * local in the looper function only.
jessekaiser 0:db396b9f4b4c 116 **/
jessekaiser 1:099b19376f16 117
jessekaiser 1:099b19376f16 118
jessekaiser 0:db396b9f4b4c 119 void looper()
jessekaiser 0:db396b9f4b4c 120 {
jessekaiser 1:099b19376f16 121 /*variable to store value in*/
jessekaiser 2:39e1bde54e73 122 uint16_t emg_value1;
jessekaiser 3:0895fa0a6ca4 123 uint16_t emg_value2;
jessekaiser 1:099b19376f16 124
jessekaiser 2:39e1bde54e73 125 float emg_value1_f32;
jessekaiser 2:39e1bde54e73 126 float emg_value2_f32;
jessekaiser 0:db396b9f4b4c 127 /*put raw emg value both in red and in emg_value*/
jessekaiser 2:39e1bde54e73 128 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 129 emg_value1_f32 = emg0.read();
jessekaiser 3:0895fa0a6ca4 130
jessekaiser 2:39e1bde54e73 131 emg_value2 = emg1.read_u16();
jessekaiser 2:39e1bde54e73 132 emg_value2_f32 = emg1.read();
jessekaiser 0:db396b9f4b4c 133
jessekaiser 2:39e1bde54e73 134 //process emg biceps
irisl 9:f7ec578a17c0 135 arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
jessekaiser 2:39e1bde54e73 136 filtered_biceps = fabs(filtered_biceps);
irisl 9:f7ec578a17c0 137 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
irisl 12:9e6e49af9304 138 average_biceps(filtered_biceps,&filtered_average_bi);
jessekaiser 2:39e1bde54e73 139 //process emg deltoid
irisl 9:f7ec578a17c0 140 arm_biquad_cascade_df1_f32(&highnotch_deltoid, &emg_value2_f32, &filtered_deltoid, 1 );
jessekaiser 3:0895fa0a6ca4 141 filtered_deltoid = fabs(filtered_deltoid);
irisl 9:f7ec578a17c0 142 arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
irisl 12:9e6e49af9304 143 average_deltoid(filtered_deltoid, &filtered_average_del);
jessekaiser 1:099b19376f16 144
jessekaiser 0:db396b9f4b4c 145 /*send value to PC. */
irisl 9:f7ec578a17c0 146 //scope.set(0,emg_value1); //Raw EMG signal biceps
irisl 9:f7ec578a17c0 147 //scope.set(1,emg_value2); //Raw EMG signal Deltoid
irisl 13:493a953a2a85 148 //scope.set(0,filtered_biceps); //processed float biceps
irisl 13:493a953a2a85 149 scope.set(0,filtered_average_bi); //processed float deltoid
irisl 14:257026c95f22 150 //scope.set(2,filtered_deltoid); //processed float biceps
irisl 13:493a953a2a85 151 scope.set(1,filtered_average_del); //processed float deltoid
jessekaiser 0:db396b9f4b4c 152 scope.send();
jessekaiser 0:db396b9f4b4c 153
jessekaiser 0:db396b9f4b4c 154 }
jessekaiser 0:db396b9f4b4c 155
irisl 16:d65458b85734 156 // LED AANSTURING
irisl 16:d65458b85734 157
irisl 16:d65458b85734 158 Ticker ledticker;
irisl 12:9e6e49af9304 159
irisl 12:9e6e49af9304 160
jessekaiser 0:db396b9f4b4c 161 void BlinkRed(int n)
jessekaiser 0:db396b9f4b4c 162 {
jessekaiser 0:db396b9f4b4c 163 for (int i=0; i<n; i++) {
jessekaiser 0:db396b9f4b4c 164 myled1 = 1;
jessekaiser 0:db396b9f4b4c 165 myled2 = 1;
jessekaiser 0:db396b9f4b4c 166 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 167 wait(0.1);
jessekaiser 0:db396b9f4b4c 168 myled1 = 0;
jessekaiser 0:db396b9f4b4c 169 myled2 = 1;
jessekaiser 0:db396b9f4b4c 170 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 171 wait(0.1);
jessekaiser 0:db396b9f4b4c 172 }
jessekaiser 0:db396b9f4b4c 173 }
jessekaiser 0:db396b9f4b4c 174
irisl 17:dbbe4e126203 175 void greenblink()
irisl 16:d65458b85734 176 {
irisl 17:dbbe4e126203 177 if(myled2.read())
irisl 17:dbbe4e126203 178 myled2 = 0;
irisl 16:d65458b85734 179 else
irisl 17:dbbe4e126203 180 myled2 = 1;
irisl 16:d65458b85734 181 }
irisl 16:d65458b85734 182
irisl 17:dbbe4e126203 183 void BlinkGreen()
irisl 18:ed2afe6953de 184 {
irisl 18:ed2afe6953de 185 myled1= 1;
irisl 18:ed2afe6953de 186 myled3 =1;
irisl 18:ed2afe6953de 187 ledticker.attach(&greenblink,.5);
irisl 18:ed2afe6953de 188 /* myled1 = 1;
irisl 18:ed2afe6953de 189 myled2 = 1;
irisl 18:ed2afe6953de 190 myled3 = 1;
irisl 18:ed2afe6953de 191 wait(0.1);
irisl 18:ed2afe6953de 192 myled1 = 0;
irisl 18:ed2afe6953de 193 myled2 = 1;
irisl 18:ed2afe6953de 194 myled3 = 1;
irisl 18:ed2afe6953de 195 wait(0.1);*/
irisl 16:d65458b85734 196 }
irisl 16:d65458b85734 197
irisl 18:ed2afe6953de 198 void stopblinkgreen()
irisl 16:d65458b85734 199 {
irisl 16:d65458b85734 200 ledticker.detach();
irisl 16:d65458b85734 201 }
irisl 16:d65458b85734 202
irisl 16:d65458b85734 203
irisl 17:dbbe4e126203 204 void BlinkGreen1 ()
jessekaiser 3:0895fa0a6ca4 205 {
jessekaiser 3:0895fa0a6ca4 206
jessekaiser 3:0895fa0a6ca4 207 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 208 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 209 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 210 wait(0.1);
jessekaiser 3:0895fa0a6ca4 211 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 212 myled2 = 0;
jessekaiser 3:0895fa0a6ca4 213 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 214 wait(0.1);
jessekaiser 3:0895fa0a6ca4 215 }
jessekaiser 3:0895fa0a6ca4 216
jessekaiser 3:0895fa0a6ca4 217
jessekaiser 3:0895fa0a6ca4 218 void BlinkBlue(int n)
jessekaiser 2:39e1bde54e73 219 {
jessekaiser 2:39e1bde54e73 220 for (int i=0; i<n; i++) {
jessekaiser 2:39e1bde54e73 221 myled1 = 1;
jessekaiser 2:39e1bde54e73 222 myled2 = 1;
jessekaiser 2:39e1bde54e73 223 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 224 wait(0.1);
jessekaiser 2:39e1bde54e73 225 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 226 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 227 myled3 = 0;
jessekaiser 3:0895fa0a6ca4 228 wait(0.1);
jessekaiser 2:39e1bde54e73 229 }
jessekaiser 2:39e1bde54e73 230 }
jessekaiser 2:39e1bde54e73 231
irisl 13:493a953a2a85 232 void ShineGreen ()
irisl 13:493a953a2a85 233 {
irisl 13:493a953a2a85 234 myled1 = 1;
irisl 13:493a953a2a85 235 myled2 = 0;
irisl 13:493a953a2a85 236 myled3 = 1;
irisl 13:493a953a2a85 237 }
irisl 13:493a953a2a85 238
irisl 13:493a953a2a85 239 void ShineBlue ()
irisl 13:493a953a2a85 240 {
irisl 13:493a953a2a85 241 myled1 = 1;
irisl 13:493a953a2a85 242 myled2 = 1;
irisl 13:493a953a2a85 243 myled3 = 0;
irisl 13:493a953a2a85 244 }
irisl 13:493a953a2a85 245
irisl 13:493a953a2a85 246 void ShineRed ()
irisl 13:493a953a2a85 247 {
irisl 13:493a953a2a85 248 myled1 = 0;
irisl 13:493a953a2a85 249 myled2 = 1;
irisl 13:493a953a2a85 250 myled3 = 1;
irisl 13:493a953a2a85 251 }
irisl 13:493a953a2a85 252
irisl 20:d40b6cba4280 253 // MOTORFUNCTIES
irisl 20:d40b6cba4280 254
irisl 20:d40b6cba4280 255 void motor2_speed_low ()
irisl 20:d40b6cba4280 256 {
irisl 20:d40b6cba4280 257 wait(1);
irisl 20:d40b6cba4280 258 speed2 = 1;
irisl 20:d40b6cba4280 259 motordir2=1;
irisl 20:d40b6cba4280 260 pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
irisl 20:d40b6cba4280 261 wait(0.3); //naar 140 graden
irisl 20:d40b6cba4280 262 pwm_motor2.write(0); //CCW
irisl 20:d40b6cba4280 263 wait(1);
irisl 20:d40b6cba4280 264 motordir2=0;
irisl 20:d40b6cba4280 265 pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
irisl 20:d40b6cba4280 266 wait(0.20); //balletje slaan, 160 graden
irisl 20:d40b6cba4280 267 pwm_motor2.write(0);
irisl 20:d40b6cba4280 268 wait(1);
irisl 20:d40b6cba4280 269 motordir2=1; //CW
irisl 20:d40b6cba4280 270 pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
irisl 20:d40b6cba4280 271 wait(1); //terug naar begin positie, 20 graden
irisl 20:d40b6cba4280 272 pwm_motor2.write(0);
irisl 20:d40b6cba4280 273 }
irisl 20:d40b6cba4280 274
irisl 20:d40b6cba4280 275 void motor2_speed_mid ()
irisl 20:d40b6cba4280 276 {
irisl 20:d40b6cba4280 277 wait(1);
irisl 20:d40b6cba4280 278 speed2 = 1;
irisl 20:d40b6cba4280 279 motordir2=1;
irisl 20:d40b6cba4280 280 pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
irisl 20:d40b6cba4280 281 wait(0.3); //naar 140 graden
irisl 20:d40b6cba4280 282 pwm_motor2.write(0); //CCW
irisl 20:d40b6cba4280 283 wait(1);
irisl 20:d40b6cba4280 284 motordir2=0;
irisl 20:d40b6cba4280 285 pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
irisl 20:d40b6cba4280 286 wait(0.20); //balletje slaan, 160 graden
irisl 20:d40b6cba4280 287 pwm_motor2.write(0);
irisl 20:d40b6cba4280 288 wait(1);
irisl 20:d40b6cba4280 289 motordir2=1; //CW
irisl 20:d40b6cba4280 290 pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
irisl 20:d40b6cba4280 291 wait(1); //terug naar begin positie, 20 graden
irisl 20:d40b6cba4280 292 pwm_motor2.write(0);
irisl 20:d40b6cba4280 293 }
irisl 20:d40b6cba4280 294
irisl 20:d40b6cba4280 295 void motor2_speed_high ()
irisl 20:d40b6cba4280 296 {
irisl 20:d40b6cba4280 297 wait(1);
irisl 20:d40b6cba4280 298 speed2 = 1;
irisl 20:d40b6cba4280 299 motordir2=1;
irisl 20:d40b6cba4280 300 pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
irisl 20:d40b6cba4280 301 wait(0.3); //naar 140 graden
irisl 20:d40b6cba4280 302 pwm_motor2.write(0); //CCW
irisl 20:d40b6cba4280 303 wait(1);
irisl 20:d40b6cba4280 304 motordir2=0;
irisl 20:d40b6cba4280 305 pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
irisl 20:d40b6cba4280 306 wait(0.20); //balletje slaan, 160 graden
irisl 20:d40b6cba4280 307 pwm_motor2.write(0);
irisl 20:d40b6cba4280 308 wait(1);
irisl 20:d40b6cba4280 309 motordir2=1; //CW
irisl 20:d40b6cba4280 310 pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
irisl 20:d40b6cba4280 311 wait(1); //terug naar begin positie, 20 graden
irisl 20:d40b6cba4280 312 pwm_motor2.write(0);
irisl 20:d40b6cba4280 313 }
irisl 20:d40b6cba4280 314
irisl 20:d40b6cba4280 315 void motor1_links()
irisl 20:d40b6cba4280 316 {
irisl 20:d40b6cba4280 317 speed1 = 0.7;
irisl 20:d40b6cba4280 318 hoek1 = 0.09; //in seconde
irisl 20:d40b6cba4280 319 wait(1);
irisl 20:d40b6cba4280 320 motordir1=0; //aangeven van directie (0 = CCW)
irisl 20:d40b6cba4280 321 pwm_motor1.write(speed1); //snelheid van de motor
irisl 20:d40b6cba4280 322 wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait
irisl 20:d40b6cba4280 323 pwm_motor1.write(0);
irisl 20:d40b6cba4280 324 }
irisl 20:d40b6cba4280 325
irisl 20:d40b6cba4280 326
irisl 20:d40b6cba4280 327 void motor1_rechts()
irisl 20:d40b6cba4280 328 {
irisl 20:d40b6cba4280 329 speed1 = 0.7;
irisl 20:d40b6cba4280 330 hoek1 = 0.09; //in seconde
irisl 20:d40b6cba4280 331 wait(1);
irisl 20:d40b6cba4280 332 motordir1=1; //aangeven van directie (1 = CW)
irisl 20:d40b6cba4280 333 pwm_motor1.write(speed1); //snelheid van de motor
irisl 20:d40b6cba4280 334 wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait
irisl 20:d40b6cba4280 335 pwm_motor1.write(0);
irisl 20:d40b6cba4280 336 }
irisl 15:eb6498bb7ca0 337
irisl 16:d65458b85734 338 int main()
jessekaiser 1:099b19376f16 339 {
jessekaiser 0:db396b9f4b4c 340 pc.baud(115200);
jessekaiser 1:099b19376f16 341
jessekaiser 0:db396b9f4b4c 342 Ticker log_timer;
jessekaiser 1:099b19376f16 343 //set up filters. Use external array for constants
irisl 9:f7ec578a17c0 344 arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
irisl 9:f7ec578a17c0 345 arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
irisl 9:f7ec578a17c0 346 arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
irisl 9:f7ec578a17c0 347 arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
jessekaiser 0:db396b9f4b4c 348 /**Here you attach the 'void looper(void)' function to the Ticker object
jessekaiser 0:db396b9f4b4c 349 * The looper() function will be called every 0.01 seconds.
jessekaiser 0:db396b9f4b4c 350 * Please mind that the parentheses after looper are omitted when using attach.
jessekaiser 0:db396b9f4b4c 351 */
jessekaiser 0:db396b9f4b4c 352 log_timer.attach(looper, 0.001);
jessekaiser 1:099b19376f16 353 while(1) { //Loop
jessekaiser 1:099b19376f16 354 /*Empty!*/
jessekaiser 1:099b19376f16 355 /*Everything is handled by the interrupt routine now!*/
jessekaiser 6:8f4138a811e0 356
irisl 13:493a953a2a85 357 while(1) {
irisl 20:d40b6cba4280 358 static float count = 0;
irisl 18:ed2afe6953de 359 pc.printf("Span de biceps aan om het instellen te starten.\n");
irisl 13:493a953a2a85 360 do {
irisl 16:d65458b85734 361 ShineRed();
irisl 17:dbbe4e126203 362 } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
irisl 17:dbbe4e126203 363 if (filtered_average_bi > 0.05) {
irisl 18:ed2afe6953de 364 BlinkRed(10);
irisl 18:ed2afe6953de 365 BlinkGreen();
irisl 18:ed2afe6953de 366 while (1) {
irisl 18:ed2afe6953de 367 pc.printf("In de loop.\n");
irisl 20:d40b6cba4280 368 if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
irisl 18:ed2afe6953de 369 stopblinkgreen();
irisl 20:d40b6cba4280 370 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 371 ShineGreen();
irisl 18:ed2afe6953de 372 wait (4);
irisl 18:ed2afe6953de 373 break;
irisl 18:ed2afe6953de 374 }
irisl 18:ed2afe6953de 375 if (filtered_average_bi < 0.05 && filtered_average_del > 0.05) {
irisl 18:ed2afe6953de 376 stopblinkgreen();
irisl 18:ed2afe6953de 377 pc.printf("ShineBlue.\n");
irisl 18:ed2afe6953de 378 ShineBlue();
irisl 20:d40b6cba4280 379 motor1_links();
irisl 18:ed2afe6953de 380 wait(4);
irisl 18:ed2afe6953de 381 break;
irisl 20:d40b6cba4280 382 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
irisl 18:ed2afe6953de 383
irisl 18:ed2afe6953de 384 {
irisl 18:ed2afe6953de 385 stopblinkgreen();
irisl 20:d40b6cba4280 386 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 387 ShineRed();
irisl 20:d40b6cba4280 388 motor1_rechts();
irisl 18:ed2afe6953de 389 wait (4);
irisl 18:ed2afe6953de 390 break;
irisl 18:ed2afe6953de 391 }
irisl 14:257026c95f22 392 }
irisl 19:1bd2fc3bce1e 393 BlinkGreen();
irisl 19:1bd2fc3bce1e 394 while (1) {
irisl 19:1bd2fc3bce1e 395 pc.printf("In de loop.\n");
irisl 20:d40b6cba4280 396 if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) {
irisl 19:1bd2fc3bce1e 397 stopblinkgreen();
irisl 20:d40b6cba4280 398 pc.printf("ShineGreen.\n");
irisl 20:d40b6cba4280 399 ShineGreen();
irisl 20:d40b6cba4280 400 motor2_speed_mid ();
irisl 19:1bd2fc3bce1e 401 wait (4);
irisl 19:1bd2fc3bce1e 402 break;
irisl 19:1bd2fc3bce1e 403 }
irisl 19:1bd2fc3bce1e 404 if (filtered_average_bi < 0.05 && filtered_average_del > 0.05) {
irisl 19:1bd2fc3bce1e 405 stopblinkgreen();
irisl 19:1bd2fc3bce1e 406 pc.printf("ShineBlue.\n");
irisl 19:1bd2fc3bce1e 407 ShineBlue();
irisl 20:d40b6cba4280 408 motor2_speed_low ();
irisl 19:1bd2fc3bce1e 409 wait(4);
irisl 19:1bd2fc3bce1e 410 break;
irisl 20:d40b6cba4280 411 } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
irisl 19:1bd2fc3bce1e 412
irisl 19:1bd2fc3bce1e 413 {
irisl 19:1bd2fc3bce1e 414 stopblinkgreen();
irisl 20:d40b6cba4280 415 pc.printf("ShineRed.\n");
irisl 20:d40b6cba4280 416 ShineRed();
irisl 20:d40b6cba4280 417 motor2_speed_high ();
irisl 19:1bd2fc3bce1e 418 wait (4);
irisl 19:1bd2fc3bce1e 419 break;
irisl 19:1bd2fc3bce1e 420 }
irisl 19:1bd2fc3bce1e 421 }
jessekaiser 6:8f4138a811e0 422
irisl 14:257026c95f22 423 }
irisl 19:1bd2fc3bce1e 424
jessekaiser 0:db396b9f4b4c 425 }
irisl 16:d65458b85734 426 }
irisl 19:1bd2fc3bce1e 427 }