2 losse EMG signalen van de biceps en deltoid
Dependencies: HIDScope MODSERIAL mbed-dsp mbed Encoder
Fork of Lampje_EMG_Gr6 by
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?
User | Revision | Line number | New 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 | } |