![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Alles bij elkaar
Dependencies: Encoder HIDScope MODSERIAL TouchButton mbed-dsp mbed
main.cpp@7:b40bbf5be443, 2014-10-30 (annotated)
- Committer:
- Tanja2211
- Date:
- Thu Oct 30 15:16:04 2014 +0000
- Revision:
- 7:b40bbf5be443
- Parent:
- 6:34da048d7168
- Child:
- 8:7bf274156e1f
pid functie bool toegevoegd
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Tanja2211 | 0:1594bb11fa13 | 1 | #include "mbed.h" |
Tanja2211 | 0:1594bb11fa13 | 2 | #include "encoder.h" |
Tanja2211 | 0:1594bb11fa13 | 3 | #include "HIDScope.h" |
Tanja2211 | 0:1594bb11fa13 | 4 | #include "MODSERIAL.h" |
Tanja2211 | 0:1594bb11fa13 | 5 | #include "arm_math.h" |
Tanja2211 | 0:1594bb11fa13 | 6 | #include "TouchButton.h" |
Tanja2211 | 0:1594bb11fa13 | 7 | |
Tanja2211 | 0:1594bb11fa13 | 8 | #define K_P (0.5) |
Tanja2211 | 0:1594bb11fa13 | 9 | #define K_I (0.02 *TSAMP1) |
Tanja2211 | 0:1594bb11fa13 | 10 | #define K_D (0 /TSAMP1) |
Tanja2211 | 0:1594bb11fa13 | 11 | #define I_LIMIT 1. |
Tanja2211 | 0:1594bb11fa13 | 12 | |
Tanja2211 | 0:1594bb11fa13 | 13 | #define TSAMP1 0.01 |
Tanja2211 | 0:1594bb11fa13 | 14 | #define TSAMP2 0.01 |
Tanja2211 | 0:1594bb11fa13 | 15 | #define WACHTEN 1 |
Tanja2211 | 0:1594bb11fa13 | 16 | #define SLAAN 2 |
Tanja2211 | 0:1594bb11fa13 | 17 | #define TERUGKEREN 3 |
Tanja2211 | 4:68dc27d284f7 | 18 | #define ANGLEMAX -251 |
Tanja2211 | 0:1594bb11fa13 | 19 | #define ANGLEMIN 0 |
Tanja2211 | 0:1594bb11fa13 | 20 | |
Tanja2211 | 0:1594bb11fa13 | 21 | //initiating functions |
Tanja2211 | 0:1594bb11fa13 | 22 | void Triceps(); |
Tanja2211 | 0:1594bb11fa13 | 23 | void Biceps(); |
Tanja2211 | 0:1594bb11fa13 | 24 | void Calibratie_Triceps(); |
Tanja2211 | 0:1594bb11fa13 | 25 | void Calibratie_Biceps(); |
Tanja2211 | 7:b40bbf5be443 | 26 | float pid(float setspeed, float measurement, bool reset = false); |
Tanja2211 | 0:1594bb11fa13 | 27 | void motor2aansturing(); |
Tanja2211 | 0:1594bb11fa13 | 28 | void motor1aansturing(); |
Tanja2211 | 0:1594bb11fa13 | 29 | |
Tanja2211 | 0:1594bb11fa13 | 30 | //alle initiaties voor EMG |
Tanja2211 | 0:1594bb11fa13 | 31 | MODSERIAL pc(USBTX,USBRX); |
Tanja2211 | 0:1594bb11fa13 | 32 | |
Tanja2211 | 0:1594bb11fa13 | 33 | HIDScope scope(4); |
Tanja2211 | 0:1594bb11fa13 | 34 | |
Tanja2211 | 0:1594bb11fa13 | 35 | AnalogIn emgB(PTB1); //biceps |
Tanja2211 | 0:1594bb11fa13 | 36 | AnalogIn emgT(PTB2); //tricep |
Tanja2211 | 0:1594bb11fa13 | 37 | |
Tanja2211 | 0:1594bb11fa13 | 38 | //*** OBJECTS *** |
Tanja2211 | 0:1594bb11fa13 | 39 | //bicep |
Tanja2211 | 0:1594bb11fa13 | 40 | uint16_t emg_valueB; |
Tanja2211 | 0:1594bb11fa13 | 41 | float emg_value_f32B; |
Tanja2211 | 0:1594bb11fa13 | 42 | float filtered_emgB; |
Tanja2211 | 0:1594bb11fa13 | 43 | float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3 |
Tanja2211 | 0:1594bb11fa13 | 44 | int yB1, yB2, yB3; |
Tanja2211 | 0:1594bb11fa13 | 45 | float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, B15, B16, B17, B18, B19, B20, B21, B22, B23, B24, B25, B26, B27, B28, B29, MOVAVG_B; //moving average objects |
Tanja2211 | 0:1594bb11fa13 | 46 | float B30, B31, B32, B33, B34, B35, B36, B37, B38, B39, B40, B41, B42, B43, B44, B45, B46, B47, B48, B49, B50, B51, B52, B53, B54, B55, B56, B57, B58, B59; |
Tanja2211 | 0:1594bb11fa13 | 47 | int snelheidsstand; |
Tanja2211 | 0:1594bb11fa13 | 48 | //tricep |
Tanja2211 | 0:1594bb11fa13 | 49 | uint16_t emg_valueT; |
Tanja2211 | 0:1594bb11fa13 | 50 | float emg_value_f32T; |
Tanja2211 | 0:1594bb11fa13 | 51 | float filtered_emgT; |
Tanja2211 | 0:1594bb11fa13 | 52 | float drempelwaardeT; |
Tanja2211 | 0:1594bb11fa13 | 53 | int yT1, yT2; |
Tanja2211 | 0:1594bb11fa13 | 54 | float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,MOVAVG_T; //moving average objects |
Tanja2211 | 0:1594bb11fa13 | 55 | float MOVAVG_Positie1, MOVAVG_Positie2; |
Tanja2211 | 0:1594bb11fa13 | 56 | int positie; |
Tanja2211 | 0:1594bb11fa13 | 57 | |
Tanja2211 | 0:1594bb11fa13 | 58 | //*** FILTERS *** |
Tanja2211 | 0:1594bb11fa13 | 59 | arm_biquad_casd_df1_inst_f32 notchT; |
Tanja2211 | 0:1594bb11fa13 | 60 | arm_biquad_casd_df1_inst_f32 notchB; |
Tanja2211 | 0:1594bb11fa13 | 61 | //constants for 50Hz |
Tanja2211 | 0:1594bb11fa13 | 62 | float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2} |
Tanja2211 | 0:1594bb11fa13 | 63 | float notch_states[4]; |
Tanja2211 | 0:1594bb11fa13 | 64 | |
Tanja2211 | 0:1594bb11fa13 | 65 | arm_biquad_casd_df1_inst_f32 lowpassT; |
Tanja2211 | 0:1594bb11fa13 | 66 | arm_biquad_casd_df1_inst_f32 lowpassB; |
Tanja2211 | 0:1594bb11fa13 | 67 | //constants for 60Hz lowpass |
Tanja2211 | 0:1594bb11fa13 | 68 | float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator |
Tanja2211 | 0:1594bb11fa13 | 69 | float lowpass_states[4]; |
Tanja2211 | 0:1594bb11fa13 | 70 | |
Tanja2211 | 0:1594bb11fa13 | 71 | arm_biquad_casd_df1_inst_f32 highpassT; |
Tanja2211 | 0:1594bb11fa13 | 72 | arm_biquad_casd_df1_inst_f32 highpassB; |
Tanja2211 | 0:1594bb11fa13 | 73 | //constants for 20Hz highpass |
Tanja2211 | 0:1594bb11fa13 | 74 | float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2} |
Tanja2211 | 0:1594bb11fa13 | 75 | float highpass_states[4]; |
Tanja2211 | 0:1594bb11fa13 | 76 | |
Tanja2211 | 0:1594bb11fa13 | 77 | bool stop; |
Tanja2211 | 0:1594bb11fa13 | 78 | float new_pwm; |
Tanja2211 | 0:1594bb11fa13 | 79 | float PWM2 = 0.3; //PWM voor instellen hoek batje |
Tanja2211 | 0:1594bb11fa13 | 80 | int toestand = TERUGKEREN; |
Jolein | 1:5d30a2ea2e11 | 81 | float setspeed = 0, V3=60, V2=40, V1 =30, Vreturn= 35;//V in counts/s |
Tanja2211 | 0:1594bb11fa13 | 82 | |
Tanja2211 | 0:1594bb11fa13 | 83 | |
Tanja2211 | 0:1594bb11fa13 | 84 | Encoder motor1(PTD5,PTD3); |
Tanja2211 | 0:1594bb11fa13 | 85 | Encoder motor2(PTD0,PTD2); |
Tanja2211 | 0:1594bb11fa13 | 86 | DigitalOut motordir1(PTA4); |
Tanja2211 | 0:1594bb11fa13 | 87 | DigitalOut motordir2(PTC9); |
Tanja2211 | 0:1594bb11fa13 | 88 | PwmOut pwm_motor1(PTA5); |
Tanja2211 | 0:1594bb11fa13 | 89 | PwmOut pwm_motor2(PTC8); |
Tanja2211 | 0:1594bb11fa13 | 90 | |
Tanja2211 | 0:1594bb11fa13 | 91 | DigitalOut myled1(LED1);//red |
Tanja2211 | 0:1594bb11fa13 | 92 | DigitalOut myled2(LED2);//green |
Tanja2211 | 0:1594bb11fa13 | 93 | DigitalOut myled3(LED3);//blue |
Tanja2211 | 0:1594bb11fa13 | 94 | |
Tanja2211 | 0:1594bb11fa13 | 95 | /* FRDM-KL25Z built-in touch slider |
Tanja2211 | 0:1594bb11fa13 | 96 | ******************* |
Tanja2211 | 0:1594bb11fa13 | 97 | * * * * |
Tanja2211 | 0:1594bb11fa13 | 98 | * 1 * 2 * 3 * |
Tanja2211 | 0:1594bb11fa13 | 99 | * * * * |
Tanja2211 | 0:1594bb11fa13 | 100 | ******************* |
Tanja2211 | 0:1594bb11fa13 | 101 | * key 1 will light Red LED -> CALIBRATIE TRICEPS |
Tanja2211 | 0:1594bb11fa13 | 102 | * key 2 will light Green LED -> CALIBRATIE BICEPS |
Tanja2211 | 0:1594bb11fa13 | 103 | * key 3 will light Blue LED -> START*/ |
Tanja2211 | 0:1594bb11fa13 | 104 | |
Tanja2211 | 0:1594bb11fa13 | 105 | |
Tanja2211 | 0:1594bb11fa13 | 106 | enum standen {STAND1=0, STAND2=1, STAND3=2}; |
Tanja2211 | 0:1594bb11fa13 | 107 | standen hoek2 = STAND1; |
Tanja2211 | 0:1594bb11fa13 | 108 | |
Tanja2211 | 0:1594bb11fa13 | 109 | int main () |
Tanja2211 | 0:1594bb11fa13 | 110 | { |
Tanja2211 | 0:1594bb11fa13 | 111 | pc.baud(115200); |
Tanja2211 | 0:1594bb11fa13 | 112 | |
Tanja2211 | 0:1594bb11fa13 | 113 | drempelwaardeT=0; |
Tanja2211 | 0:1594bb11fa13 | 114 | drempelwaardeB1=0; |
Tanja2211 | 0:1594bb11fa13 | 115 | drempelwaardeB2=0; |
Tanja2211 | 0:1594bb11fa13 | 116 | drempelwaardeB3=0; |
Tanja2211 | 0:1594bb11fa13 | 117 | |
Tanja2211 | 0:1594bb11fa13 | 118 | TouchButton TButton; |
Tanja2211 | 0:1594bb11fa13 | 119 | |
Tanja2211 | 0:1594bb11fa13 | 120 | myled1=1; |
Tanja2211 | 0:1594bb11fa13 | 121 | myled2=1; |
Tanja2211 | 0:1594bb11fa13 | 122 | myled3=1; |
Tanja2211 | 0:1594bb11fa13 | 123 | |
Tanja2211 | 0:1594bb11fa13 | 124 | int key=0; |
Tanja2211 | 0:1594bb11fa13 | 125 | |
Tanja2211 | 0:1594bb11fa13 | 126 | pc.printf("key 1 calibratie triceps\n"); |
Tanja2211 | 0:1594bb11fa13 | 127 | pc.printf("key 2 caliratie biceps\n"); |
Tanja2211 | 0:1594bb11fa13 | 128 | pc.printf("key 3 START\n"); |
Tanja2211 | 0:1594bb11fa13 | 129 | |
Tanja2211 | 0:1594bb11fa13 | 130 | while(true) { |
Tanja2211 | 0:1594bb11fa13 | 131 | |
Tanja2211 | 0:1594bb11fa13 | 132 | key = TButton.PressedButton(); |
Tanja2211 | 0:1594bb11fa13 | 133 | |
Tanja2211 | 0:1594bb11fa13 | 134 | if (key==1) { |
Tanja2211 | 0:1594bb11fa13 | 135 | //rood |
Tanja2211 | 0:1594bb11fa13 | 136 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 137 | myled2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 138 | myled3 = 1; |
Tanja2211 | 0:1594bb11fa13 | 139 | |
Tanja2211 | 0:1594bb11fa13 | 140 | pc.printf("calibratie tricep aan\n"); |
Tanja2211 | 0:1594bb11fa13 | 141 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 142 | |
Tanja2211 | 0:1594bb11fa13 | 143 | Calibratie_Triceps(); |
Tanja2211 | 0:1594bb11fa13 | 144 | drempelwaardeT=MOVAVG_T-1; |
Tanja2211 | 0:1594bb11fa13 | 145 | pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT); |
Tanja2211 | 0:1594bb11fa13 | 146 | |
Tanja2211 | 0:1594bb11fa13 | 147 | pc.printf("calibratie tricep klaar,\n"); |
Tanja2211 | 0:1594bb11fa13 | 148 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 149 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 150 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 151 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 152 | |
Tanja2211 | 0:1594bb11fa13 | 153 | myled1=1; |
Tanja2211 | 0:1594bb11fa13 | 154 | myled2=1; |
Tanja2211 | 0:1594bb11fa13 | 155 | myled3=1; |
Tanja2211 | 0:1594bb11fa13 | 156 | } |
Tanja2211 | 0:1594bb11fa13 | 157 | if (key==2) { |
Tanja2211 | 0:1594bb11fa13 | 158 | //green |
Tanja2211 | 0:1594bb11fa13 | 159 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 160 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 161 | myled3 = 1; |
Tanja2211 | 0:1594bb11fa13 | 162 | |
Tanja2211 | 0:1594bb11fa13 | 163 | pc.printf("calibratie bicep snelheid 1 aan\n"); |
Tanja2211 | 0:1594bb11fa13 | 164 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 165 | |
Tanja2211 | 0:1594bb11fa13 | 166 | Calibratie_Biceps(); |
Tanja2211 | 0:1594bb11fa13 | 167 | drempelwaardeB1=MOVAVG_B-1; |
Tanja2211 | 0:1594bb11fa13 | 168 | pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1); |
Tanja2211 | 0:1594bb11fa13 | 169 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 170 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 171 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 172 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 173 | |
Tanja2211 | 0:1594bb11fa13 | 174 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 175 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 176 | myled3 = 1; |
Tanja2211 | 0:1594bb11fa13 | 177 | |
Tanja2211 | 0:1594bb11fa13 | 178 | pc.printf("calibratie biceps snelheid 2 aan\n"); |
Tanja2211 | 0:1594bb11fa13 | 179 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 180 | |
Tanja2211 | 0:1594bb11fa13 | 181 | Calibratie_Biceps(); |
Tanja2211 | 0:1594bb11fa13 | 182 | drempelwaardeB2=MOVAVG_B-1; |
Tanja2211 | 0:1594bb11fa13 | 183 | pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2); |
Tanja2211 | 0:1594bb11fa13 | 184 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 185 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 186 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 187 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 188 | |
Tanja2211 | 0:1594bb11fa13 | 189 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 190 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 191 | myled3 = 1; |
Tanja2211 | 0:1594bb11fa13 | 192 | |
Tanja2211 | 0:1594bb11fa13 | 193 | pc.printf("calibratie biceps snelheid 3 aan\n"); |
Tanja2211 | 0:1594bb11fa13 | 194 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 195 | Calibratie_Biceps(); |
Tanja2211 | 0:1594bb11fa13 | 196 | drempelwaardeB3=MOVAVG_B-1; |
Tanja2211 | 0:1594bb11fa13 | 197 | pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3); |
Tanja2211 | 0:1594bb11fa13 | 198 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 199 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 200 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 201 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 202 | |
Tanja2211 | 0:1594bb11fa13 | 203 | pc.printf("caliratie biceps is klaar\n"); |
Tanja2211 | 0:1594bb11fa13 | 204 | myled1=1; |
Tanja2211 | 0:1594bb11fa13 | 205 | myled2=1; |
Tanja2211 | 0:1594bb11fa13 | 206 | myled3=1; |
Tanja2211 | 0:1594bb11fa13 | 207 | } |
Tanja2211 | 0:1594bb11fa13 | 208 | |
Tanja2211 | 0:1594bb11fa13 | 209 | if (key==3) { |
Tanja2211 | 0:1594bb11fa13 | 210 | //blue |
Tanja2211 | 0:1594bb11fa13 | 211 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 212 | myled2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 213 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 214 | wait(3); |
Tanja2211 | 0:1594bb11fa13 | 215 | |
Tanja2211 | 0:1594bb11fa13 | 216 | if(drempelwaardeT==0) { |
Tanja2211 | 0:1594bb11fa13 | 217 | pc.printf("geen waarde calibratie TRICEPS \n"); |
Tanja2211 | 0:1594bb11fa13 | 218 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 219 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 220 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 221 | } |
Tanja2211 | 0:1594bb11fa13 | 222 | if (drempelwaardeB1==0) { |
Tanja2211 | 0:1594bb11fa13 | 223 | pc.printf("geen waarde calibratie BICEPS 1 \n"); |
Tanja2211 | 0:1594bb11fa13 | 224 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 225 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 226 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 227 | } |
Tanja2211 | 0:1594bb11fa13 | 228 | if (drempelwaardeB2==0) { |
Tanja2211 | 0:1594bb11fa13 | 229 | pc.printf("geen waarde calibratie BICEPS 2 \n"); |
Tanja2211 | 0:1594bb11fa13 | 230 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 231 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 232 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 233 | } |
Tanja2211 | 0:1594bb11fa13 | 234 | if (drempelwaardeB3==0) { |
Tanja2211 | 0:1594bb11fa13 | 235 | pc.printf("geen waarde calibratie BICEPS 3 \n"); |
Tanja2211 | 0:1594bb11fa13 | 236 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 237 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 238 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 239 | } else { |
Tanja2211 | 0:1594bb11fa13 | 240 | |
Tanja2211 | 0:1594bb11fa13 | 241 | pc.printf("eerst positie dan snelheid aangeven /n"); |
Tanja2211 | 0:1594bb11fa13 | 242 | |
Tanja2211 | 0:1594bb11fa13 | 243 | //bepaling van positie met triceps 1 |
Tanja2211 | 0:1594bb11fa13 | 244 | Ticker log_timerT1; |
Tanja2211 | 0:1594bb11fa13 | 245 | |
Tanja2211 | 0:1594bb11fa13 | 246 | arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states); |
Tanja2211 | 0:1594bb11fa13 | 247 | arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states); |
Tanja2211 | 0:1594bb11fa13 | 248 | arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states); |
Tanja2211 | 0:1594bb11fa13 | 249 | |
Tanja2211 | 0:1594bb11fa13 | 250 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 251 | myled2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 252 | myled3 = 1; |
Tanja2211 | 0:1594bb11fa13 | 253 | |
Tanja2211 | 0:1594bb11fa13 | 254 | log_timerT1.attach(Triceps, 0.005); |
Tanja2211 | 0:1594bb11fa13 | 255 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 256 | log_timerT1.detach(); |
Tanja2211 | 0:1594bb11fa13 | 257 | |
Tanja2211 | 0:1594bb11fa13 | 258 | // positie van batje met behulp van Triceps |
Tanja2211 | 0:1594bb11fa13 | 259 | |
Tanja2211 | 0:1594bb11fa13 | 260 | if (MOVAVG_T >= drempelwaardeT) { |
Tanja2211 | 0:1594bb11fa13 | 261 | yT1=1; |
Tanja2211 | 0:1594bb11fa13 | 262 | } else { |
Tanja2211 | 0:1594bb11fa13 | 263 | yT1=0; |
Tanja2211 | 0:1594bb11fa13 | 264 | } |
Tanja2211 | 0:1594bb11fa13 | 265 | |
Tanja2211 | 0:1594bb11fa13 | 266 | pc.printf("Triceps meting 1 is klaar.\n"); |
Tanja2211 | 0:1594bb11fa13 | 267 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 268 | myled2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 269 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 270 | wait(3); |
Tanja2211 | 0:1594bb11fa13 | 271 | |
Tanja2211 | 0:1594bb11fa13 | 272 | //bepaling van positie met tricep 2 |
Tanja2211 | 0:1594bb11fa13 | 273 | Ticker log_timerT2; |
Tanja2211 | 0:1594bb11fa13 | 274 | |
Tanja2211 | 0:1594bb11fa13 | 275 | arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states); |
Tanja2211 | 0:1594bb11fa13 | 276 | arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states); |
Tanja2211 | 0:1594bb11fa13 | 277 | arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states); |
Tanja2211 | 0:1594bb11fa13 | 278 | |
Tanja2211 | 0:1594bb11fa13 | 279 | myled1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 280 | myled2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 281 | myled3 = 1; |
Tanja2211 | 0:1594bb11fa13 | 282 | |
Tanja2211 | 0:1594bb11fa13 | 283 | log_timerT2.attach(Triceps, 0.005); |
Tanja2211 | 0:1594bb11fa13 | 284 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 285 | log_timerT2.detach(); |
Tanja2211 | 0:1594bb11fa13 | 286 | |
Tanja2211 | 0:1594bb11fa13 | 287 | if (MOVAVG_T >= drempelwaardeT) { |
Tanja2211 | 0:1594bb11fa13 | 288 | yT2=1; |
Tanja2211 | 0:1594bb11fa13 | 289 | } else { |
Tanja2211 | 0:1594bb11fa13 | 290 | yT2=0; |
Tanja2211 | 0:1594bb11fa13 | 291 | } |
Tanja2211 | 0:1594bb11fa13 | 292 | |
Tanja2211 | 0:1594bb11fa13 | 293 | pc.printf("Triceps meting 2 is klaar.\n"); |
Tanja2211 | 0:1594bb11fa13 | 294 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 295 | myled2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 296 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 297 | |
Tanja2211 | 0:1594bb11fa13 | 298 | //*** INPUT MOTOR 2 *** |
Tanja2211 | 0:1594bb11fa13 | 299 | positie=yT1+yT2; |
Tanja2211 | 0:1594bb11fa13 | 300 | |
Tanja2211 | 0:1594bb11fa13 | 301 | //controle positie op scherm |
Tanja2211 | 0:1594bb11fa13 | 302 | if (positie==0) { |
Tanja2211 | 0:1594bb11fa13 | 303 | pc.printf("Motor 2 blijft op stand 1\n"); |
Tanja2211 | 0:1594bb11fa13 | 304 | } else { |
Tanja2211 | 0:1594bb11fa13 | 305 | if (positie==1) { |
Tanja2211 | 0:1594bb11fa13 | 306 | pc.printf("Motor 2 gaat naar stand 2\n"); |
Tanja2211 | 0:1594bb11fa13 | 307 | } else { |
Tanja2211 | 0:1594bb11fa13 | 308 | if (positie==2) { |
Tanja2211 | 0:1594bb11fa13 | 309 | pc.printf("Motor 2 gaat naar stand 3\n"); |
Tanja2211 | 0:1594bb11fa13 | 310 | } |
Tanja2211 | 0:1594bb11fa13 | 311 | } |
Tanja2211 | 0:1594bb11fa13 | 312 | } |
Tanja2211 | 0:1594bb11fa13 | 313 | |
Tanja2211 | 0:1594bb11fa13 | 314 | Ticker looptimer2; |
Tanja2211 | 0:1594bb11fa13 | 315 | looptimer2.attach(motor2aansturing,TSAMP1); |
Tanja2211 | 0:1594bb11fa13 | 316 | wait(8); |
Tanja2211 | 0:1594bb11fa13 | 317 | looptimer2.detach(); |
Tanja2211 | 0:1594bb11fa13 | 318 | pc.printf("Detach Motor 1\n"); |
Tanja2211 | 0:1594bb11fa13 | 319 | |
Tanja2211 | 0:1594bb11fa13 | 320 | //------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2 |
Tanja2211 | 0:1594bb11fa13 | 321 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 322 | Ticker log_timerB; |
Tanja2211 | 0:1594bb11fa13 | 323 | |
Tanja2211 | 0:1594bb11fa13 | 324 | arm_biquad_cascade_df1_init_f32(¬chB,1,notch_const,notch_states); |
Tanja2211 | 0:1594bb11fa13 | 325 | arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states); |
Tanja2211 | 0:1594bb11fa13 | 326 | arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states); |
Tanja2211 | 0:1594bb11fa13 | 327 | |
Tanja2211 | 0:1594bb11fa13 | 328 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 329 | myled2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 330 | myled3 = 1; |
Tanja2211 | 0:1594bb11fa13 | 331 | |
Tanja2211 | 0:1594bb11fa13 | 332 | log_timerB.attach(Biceps,0.005); |
Tanja2211 | 0:1594bb11fa13 | 333 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 334 | log_timerB.detach(); |
Tanja2211 | 0:1594bb11fa13 | 335 | |
Tanja2211 | 0:1594bb11fa13 | 336 | //bepaling van snelheidsstand met biceps |
Tanja2211 | 0:1594bb11fa13 | 337 | |
Tanja2211 | 0:1594bb11fa13 | 338 | if (MOVAVG_B >= drempelwaardeB1) { |
Tanja2211 | 0:1594bb11fa13 | 339 | yB1=1; |
Tanja2211 | 0:1594bb11fa13 | 340 | if (MOVAVG_B >= drempelwaardeB2) { |
Tanja2211 | 0:1594bb11fa13 | 341 | yB2=1; |
Tanja2211 | 0:1594bb11fa13 | 342 | if (MOVAVG_B >= drempelwaardeB3) { |
Tanja2211 | 0:1594bb11fa13 | 343 | yB3=1; |
Tanja2211 | 0:1594bb11fa13 | 344 | } else { |
Tanja2211 | 0:1594bb11fa13 | 345 | yB3=0; |
Tanja2211 | 0:1594bb11fa13 | 346 | } |
Tanja2211 | 0:1594bb11fa13 | 347 | } else { |
Tanja2211 | 0:1594bb11fa13 | 348 | yB2=0; |
Tanja2211 | 0:1594bb11fa13 | 349 | } |
Tanja2211 | 0:1594bb11fa13 | 350 | } else { |
Tanja2211 | 0:1594bb11fa13 | 351 | yB1=0; |
Tanja2211 | 0:1594bb11fa13 | 352 | } |
Tanja2211 | 0:1594bb11fa13 | 353 | |
Tanja2211 | 0:1594bb11fa13 | 354 | pc.printf("Biceps meting is klaar.\n"); |
Tanja2211 | 0:1594bb11fa13 | 355 | myled1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 356 | myled2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 357 | myled3 = 0; |
Tanja2211 | 0:1594bb11fa13 | 358 | |
Tanja2211 | 0:1594bb11fa13 | 359 | //*** INPUT MOTOR 1 *** |
Tanja2211 | 0:1594bb11fa13 | 360 | snelheidsstand=yB1+yB2+yB3; |
Tanja2211 | 0:1594bb11fa13 | 361 | |
Tanja2211 | 0:1594bb11fa13 | 362 | //controle snelheidsstand op scherm |
Tanja2211 | 0:1594bb11fa13 | 363 | if (snelheidsstand==0) { |
Tanja2211 | 0:1594bb11fa13 | 364 | pc.printf("Motor 1 beweegt niet \n"); |
Tanja2211 | 0:1594bb11fa13 | 365 | } else { |
Tanja2211 | 0:1594bb11fa13 | 366 | if (snelheidsstand==1) { |
Tanja2211 | 0:1594bb11fa13 | 367 | pc.printf("Motor 1 beweegt met snelheid 1 \n"); |
Tanja2211 | 0:1594bb11fa13 | 368 | } else { |
Tanja2211 | 0:1594bb11fa13 | 369 | if (snelheidsstand==2) { |
Tanja2211 | 0:1594bb11fa13 | 370 | pc.printf("Motor 1 beweegt met snelheid 2 \n"); |
Tanja2211 | 0:1594bb11fa13 | 371 | } else { |
Tanja2211 | 0:1594bb11fa13 | 372 | if (snelheidsstand==3) { |
Tanja2211 | 0:1594bb11fa13 | 373 | pc.printf("Motor 1 beweegt met snelheid 3 \n"); |
Tanja2211 | 0:1594bb11fa13 | 374 | } |
Tanja2211 | 0:1594bb11fa13 | 375 | } |
Tanja2211 | 0:1594bb11fa13 | 376 | } |
Tanja2211 | 0:1594bb11fa13 | 377 | } |
Tanja2211 | 0:1594bb11fa13 | 378 | |
Tanja2211 | 0:1594bb11fa13 | 379 | Ticker looptimer1; |
Tanja2211 | 0:1594bb11fa13 | 380 | //pwm_motor1.write(0.3); |
Tanja2211 | 0:1594bb11fa13 | 381 | motordir1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 382 | stop = 0; |
Tanja2211 | 0:1594bb11fa13 | 383 | looptimer1.attach(motor1aansturing,TSAMP1); |
Tanja2211 | 0:1594bb11fa13 | 384 | wait(8); ////is aan te passen (tijd die nodig is om balletje te slaan |
Tanja2211 | 0:1594bb11fa13 | 385 | looptimer1.detach(); |
Tanja2211 | 0:1594bb11fa13 | 386 | pc.printf("detachMotor1\n"); |
Tanja2211 | 0:1594bb11fa13 | 387 | pwm_motor1.write(0); |
Tanja2211 | 0:1594bb11fa13 | 388 | |
Tanja2211 | 0:1594bb11fa13 | 389 | myled1=1; |
Tanja2211 | 0:1594bb11fa13 | 390 | myled2=1; |
Tanja2211 | 0:1594bb11fa13 | 391 | myled3=1; |
Tanja2211 | 7:b40bbf5be443 | 392 | } |
Tanja2211 | 0:1594bb11fa13 | 393 | } |
Tanja2211 | 0:1594bb11fa13 | 394 | } |
Tanja2211 | 0:1594bb11fa13 | 395 | }//end int main |
Tanja2211 | 0:1594bb11fa13 | 396 | |
Tanja2211 | 0:1594bb11fa13 | 397 | |
Tanja2211 | 7:b40bbf5be443 | 398 | float pid(float setspeed, float measurement, bool reset) |
Tanja2211 | 0:1594bb11fa13 | 399 | { |
Tanja2211 | 0:1594bb11fa13 | 400 | float error; |
Tanja2211 | 0:1594bb11fa13 | 401 | static float prev_error = 0; |
Tanja2211 | 0:1594bb11fa13 | 402 | float out_p = 0; |
Tanja2211 | 0:1594bb11fa13 | 403 | static float out_i = 0; |
Tanja2211 | 0:1594bb11fa13 | 404 | float out_d = 0; |
Tanja2211 | 7:b40bbf5be443 | 405 | if(reset) |
Tanja2211 | 7:b40bbf5be443 | 406 | { |
Tanja2211 | 7:b40bbf5be443 | 407 | out_i = 0; |
Tanja2211 | 7:b40bbf5be443 | 408 | prev_error = 0; |
Tanja2211 | 7:b40bbf5be443 | 409 | } |
Tanja2211 | 0:1594bb11fa13 | 410 | error = setspeed-measurement; |
Tanja2211 | 0:1594bb11fa13 | 411 | out_p = error*K_P; |
Tanja2211 | 0:1594bb11fa13 | 412 | out_i += error*K_I; |
Tanja2211 | 0:1594bb11fa13 | 413 | out_d = (error-prev_error)*K_D; |
Tanja2211 | 0:1594bb11fa13 | 414 | prev_error = error; |
Tanja2211 | 0:1594bb11fa13 | 415 | return out_p + out_i + out_d; |
Tanja2211 | 0:1594bb11fa13 | 416 | } |
Tanja2211 | 0:1594bb11fa13 | 417 | |
Tanja2211 | 0:1594bb11fa13 | 418 | void Triceps() |
Tanja2211 | 0:1594bb11fa13 | 419 | { |
Tanja2211 | 0:1594bb11fa13 | 420 | //Triceps lezen |
Tanja2211 | 0:1594bb11fa13 | 421 | emg_valueT = emgT.read_u16(); |
Tanja2211 | 0:1594bb11fa13 | 422 | emg_value_f32T = emgT.read(); |
Tanja2211 | 0:1594bb11fa13 | 423 | |
Tanja2211 | 0:1594bb11fa13 | 424 | //Triceps filteren |
Tanja2211 | 0:1594bb11fa13 | 425 | arm_biquad_cascade_df1_f32(¬chT, &emg_value_f32T, &filtered_emgT, 1); |
Tanja2211 | 0:1594bb11fa13 | 426 | arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 ); |
Tanja2211 | 0:1594bb11fa13 | 427 | filtered_emgT = fabs(filtered_emgT); |
Tanja2211 | 0:1594bb11fa13 | 428 | arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 ); |
Tanja2211 | 0:1594bb11fa13 | 429 | filtered_emgT = fabs(filtered_emgT); |
Tanja2211 | 0:1594bb11fa13 | 430 | |
Tanja2211 | 0:1594bb11fa13 | 431 | //Triceps moving average |
Tanja2211 | 0:1594bb11fa13 | 432 | T0=filtered_emgT*1000; |
Tanja2211 | 0:1594bb11fa13 | 433 | MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333; |
Tanja2211 | 0:1594bb11fa13 | 434 | T29=T28; |
Tanja2211 | 0:1594bb11fa13 | 435 | T28=T27; |
Tanja2211 | 0:1594bb11fa13 | 436 | T27=T26; |
Tanja2211 | 0:1594bb11fa13 | 437 | T26=T25; |
Tanja2211 | 0:1594bb11fa13 | 438 | T25=T24; |
Tanja2211 | 0:1594bb11fa13 | 439 | T24=T23; |
Tanja2211 | 0:1594bb11fa13 | 440 | T23=T22; |
Tanja2211 | 0:1594bb11fa13 | 441 | T22=T21; |
Tanja2211 | 0:1594bb11fa13 | 442 | T21=T20; |
Tanja2211 | 0:1594bb11fa13 | 443 | T20=T19; |
Tanja2211 | 0:1594bb11fa13 | 444 | T19=T18; |
Tanja2211 | 0:1594bb11fa13 | 445 | T18=T17; |
Tanja2211 | 0:1594bb11fa13 | 446 | T17=T16; |
Tanja2211 | 0:1594bb11fa13 | 447 | T16=T15; |
Tanja2211 | 0:1594bb11fa13 | 448 | T15=T14; |
Tanja2211 | 0:1594bb11fa13 | 449 | T14=T13; |
Tanja2211 | 0:1594bb11fa13 | 450 | T13=T12; |
Tanja2211 | 0:1594bb11fa13 | 451 | T12=T11; |
Tanja2211 | 0:1594bb11fa13 | 452 | T11=T10; |
Tanja2211 | 0:1594bb11fa13 | 453 | T10=T9; |
Tanja2211 | 0:1594bb11fa13 | 454 | T9=T8; |
Tanja2211 | 0:1594bb11fa13 | 455 | T8=T7; |
Tanja2211 | 0:1594bb11fa13 | 456 | T7=T6; |
Tanja2211 | 0:1594bb11fa13 | 457 | T6=T5; |
Tanja2211 | 0:1594bb11fa13 | 458 | T5=T4; |
Tanja2211 | 0:1594bb11fa13 | 459 | T4=T3; |
Tanja2211 | 0:1594bb11fa13 | 460 | T3=T2; |
Tanja2211 | 0:1594bb11fa13 | 461 | T2=T1; |
Tanja2211 | 0:1594bb11fa13 | 462 | T1=T0; |
Tanja2211 | 0:1594bb11fa13 | 463 | |
Tanja2211 | 0:1594bb11fa13 | 464 | //sturen naar scherm (Realterm) |
Tanja2211 | 0:1594bb11fa13 | 465 | pc.printf("Moving average T %f\r\n",MOVAVG_T); |
Tanja2211 | 0:1594bb11fa13 | 466 | |
Tanja2211 | 0:1594bb11fa13 | 467 | //sturen naar HID Scope |
Tanja2211 | 0:1594bb11fa13 | 468 | scope.set(0,emg_valueT); //ruwe data |
Tanja2211 | 0:1594bb11fa13 | 469 | scope.set(1,filtered_emgT); //filtered |
Tanja2211 | 0:1594bb11fa13 | 470 | scope.send(); |
Tanja2211 | 0:1594bb11fa13 | 471 | } |
Tanja2211 | 0:1594bb11fa13 | 472 | |
Tanja2211 | 0:1594bb11fa13 | 473 | void Biceps() |
Tanja2211 | 0:1594bb11fa13 | 474 | { |
Tanja2211 | 0:1594bb11fa13 | 475 | //Biceps lezen |
Tanja2211 | 0:1594bb11fa13 | 476 | emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
Tanja2211 | 0:1594bb11fa13 | 477 | emg_value_f32B = emgB.read(); |
Tanja2211 | 0:1594bb11fa13 | 478 | |
Tanja2211 | 0:1594bb11fa13 | 479 | //Biceps filteren |
Tanja2211 | 0:1594bb11fa13 | 480 | arm_biquad_cascade_df1_f32(¬chB, &emg_value_f32B, &filtered_emgB, 1 ); |
Tanja2211 | 0:1594bb11fa13 | 481 | arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 ); |
Tanja2211 | 0:1594bb11fa13 | 482 | filtered_emgB = fabs(filtered_emgB); |
Tanja2211 | 0:1594bb11fa13 | 483 | arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 ); |
Tanja2211 | 0:1594bb11fa13 | 484 | filtered_emgB = fabs(filtered_emgB); |
Tanja2211 | 0:1594bb11fa13 | 485 | |
Tanja2211 | 0:1594bb11fa13 | 486 | //Biceps moving average |
Tanja2211 | 0:1594bb11fa13 | 487 | B0=filtered_emgB*1000; |
Tanja2211 | 0:1594bb11fa13 | 488 | MOVAVG_B=B0*0.01667+B1*0.01667+B2*0.01667+B3*0.01667+B4*0.01667+B5*0.01667+B6*0.01667+B7*0.01667+B8*0.01667+B9*0.01667+B10*0.01667+B11*0.01667+B12*0.01667+B13*0.01667+B14*0.01667+B15*0.01667+B16*0.01667+B17*0.01667+B18*0.01667+B19*0.01667+B20*0.01667+B21*0.01667+B22*0.01667+B23*0.01667+B24*0.01667+B25*0.01667+B26*0.01667+B27*0.01667+B28*0.01667+B29*0.01667+B30*0.01667+B31*0.01667+B32*0.01667+B33*0.01667+B34*0.01667+B35*0.01667+B36*0.01667+B37*0.01667+B38*0.01667+B39*0.01667+B40*0.01667+B41*0.01667+B42*0.01667+B43*0.01667+B44*0.01667+B45*0.01667+B46*0.01667+B47*0.01667+B48*0.01667+B49*0.01667+B50*0.01667+B51*0.01667+B52*0.01667+B53*0.01667+B54*0.01667+B55*0.01667+B56*0.01667+B57*0.01667+B58*0.01667+B59*0.01667; |
Tanja2211 | 0:1594bb11fa13 | 489 | B59=B58; |
Tanja2211 | 0:1594bb11fa13 | 490 | B58=B57; |
Tanja2211 | 0:1594bb11fa13 | 491 | B57=B56; |
Tanja2211 | 0:1594bb11fa13 | 492 | B56=B55; |
Tanja2211 | 0:1594bb11fa13 | 493 | B55=B54; |
Tanja2211 | 0:1594bb11fa13 | 494 | B54=B53; |
Tanja2211 | 0:1594bb11fa13 | 495 | B53=B52; |
Tanja2211 | 0:1594bb11fa13 | 496 | B52=B51; |
Tanja2211 | 0:1594bb11fa13 | 497 | B51=B50; |
Tanja2211 | 0:1594bb11fa13 | 498 | B50=B48; |
Tanja2211 | 0:1594bb11fa13 | 499 | B49=B49; |
Tanja2211 | 0:1594bb11fa13 | 500 | B48=B47; |
Tanja2211 | 0:1594bb11fa13 | 501 | B47=B46; |
Tanja2211 | 0:1594bb11fa13 | 502 | B46=B45; |
Tanja2211 | 0:1594bb11fa13 | 503 | B45=B44; |
Tanja2211 | 0:1594bb11fa13 | 504 | B44=B43; |
Tanja2211 | 0:1594bb11fa13 | 505 | B43=B42; |
Tanja2211 | 0:1594bb11fa13 | 506 | B42=B41; |
Tanja2211 | 0:1594bb11fa13 | 507 | B41=B40; |
Tanja2211 | 0:1594bb11fa13 | 508 | B40=B39; |
Tanja2211 | 0:1594bb11fa13 | 509 | B39=B38; |
Tanja2211 | 0:1594bb11fa13 | 510 | B38=B37; |
Tanja2211 | 0:1594bb11fa13 | 511 | B37=B36; |
Tanja2211 | 0:1594bb11fa13 | 512 | B36=B35; |
Tanja2211 | 0:1594bb11fa13 | 513 | B35=B34; |
Tanja2211 | 0:1594bb11fa13 | 514 | B34=B33; |
Tanja2211 | 0:1594bb11fa13 | 515 | B33=B32; |
Tanja2211 | 0:1594bb11fa13 | 516 | B32=B31; |
Tanja2211 | 0:1594bb11fa13 | 517 | B31=B30; |
Tanja2211 | 0:1594bb11fa13 | 518 | B30=B29; |
Tanja2211 | 0:1594bb11fa13 | 519 | B29=B28; |
Tanja2211 | 0:1594bb11fa13 | 520 | B28=B27; |
Tanja2211 | 0:1594bb11fa13 | 521 | B27=B26; |
Tanja2211 | 0:1594bb11fa13 | 522 | B26=B25; |
Tanja2211 | 0:1594bb11fa13 | 523 | B25=B24; |
Tanja2211 | 0:1594bb11fa13 | 524 | B24=B23; |
Tanja2211 | 0:1594bb11fa13 | 525 | B23=B22; |
Tanja2211 | 0:1594bb11fa13 | 526 | B22=B21; |
Tanja2211 | 0:1594bb11fa13 | 527 | B21=B20; |
Tanja2211 | 0:1594bb11fa13 | 528 | B20=B19; |
Tanja2211 | 0:1594bb11fa13 | 529 | B19=B18; |
Tanja2211 | 0:1594bb11fa13 | 530 | B18=B17; |
Tanja2211 | 0:1594bb11fa13 | 531 | B17=B16; |
Tanja2211 | 0:1594bb11fa13 | 532 | B16=B15; |
Tanja2211 | 0:1594bb11fa13 | 533 | B15=B14; |
Tanja2211 | 0:1594bb11fa13 | 534 | B14=B13; |
Tanja2211 | 0:1594bb11fa13 | 535 | B13=B12; |
Tanja2211 | 0:1594bb11fa13 | 536 | B12=B11; |
Tanja2211 | 0:1594bb11fa13 | 537 | B11=B10; |
Tanja2211 | 0:1594bb11fa13 | 538 | B10=B9; |
Tanja2211 | 0:1594bb11fa13 | 539 | B9=B8; |
Tanja2211 | 0:1594bb11fa13 | 540 | B8=B7; |
Tanja2211 | 0:1594bb11fa13 | 541 | B7=B6; |
Tanja2211 | 0:1594bb11fa13 | 542 | B6=B5; |
Tanja2211 | 0:1594bb11fa13 | 543 | B5=B4; |
Tanja2211 | 0:1594bb11fa13 | 544 | B4=B3; |
Tanja2211 | 0:1594bb11fa13 | 545 | B3=B2; |
Tanja2211 | 0:1594bb11fa13 | 546 | B2=B1; |
Tanja2211 | 0:1594bb11fa13 | 547 | B1=B0; |
Tanja2211 | 0:1594bb11fa13 | 548 | |
Tanja2211 | 0:1594bb11fa13 | 549 | //sturen naar scherm |
Tanja2211 | 0:1594bb11fa13 | 550 | pc.printf("Moving average B %f\r\n",MOVAVG_B); |
Tanja2211 | 0:1594bb11fa13 | 551 | |
Tanja2211 | 0:1594bb11fa13 | 552 | //naar HID Scope |
Tanja2211 | 0:1594bb11fa13 | 553 | scope.set(2,emg_valueB); //ruwe data |
Tanja2211 | 0:1594bb11fa13 | 554 | scope.set(3,filtered_emgB); //filtered |
Tanja2211 | 0:1594bb11fa13 | 555 | scope.send(); |
Tanja2211 | 0:1594bb11fa13 | 556 | } |
Tanja2211 | 0:1594bb11fa13 | 557 | |
Tanja2211 | 0:1594bb11fa13 | 558 | void Calibratie_Triceps() |
Tanja2211 | 0:1594bb11fa13 | 559 | { |
Tanja2211 | 0:1594bb11fa13 | 560 | Ticker log_timerT; |
Tanja2211 | 0:1594bb11fa13 | 561 | |
Tanja2211 | 0:1594bb11fa13 | 562 | arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states); |
Tanja2211 | 0:1594bb11fa13 | 563 | arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states); |
Tanja2211 | 0:1594bb11fa13 | 564 | arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states); |
Tanja2211 | 0:1594bb11fa13 | 565 | |
Tanja2211 | 0:1594bb11fa13 | 566 | log_timerT.attach(Triceps, 0.005); |
Tanja2211 | 0:1594bb11fa13 | 567 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 568 | log_timerT.detach(); |
Tanja2211 | 0:1594bb11fa13 | 569 | } |
Tanja2211 | 0:1594bb11fa13 | 570 | |
Tanja2211 | 0:1594bb11fa13 | 571 | void Calibratie_Biceps() |
Tanja2211 | 0:1594bb11fa13 | 572 | { |
Tanja2211 | 0:1594bb11fa13 | 573 | Ticker log_timerB; |
Tanja2211 | 0:1594bb11fa13 | 574 | |
Tanja2211 | 0:1594bb11fa13 | 575 | arm_biquad_cascade_df1_init_f32(¬chB,1,notch_const,notch_states); |
Tanja2211 | 0:1594bb11fa13 | 576 | arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states); |
Tanja2211 | 0:1594bb11fa13 | 577 | arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states); |
Tanja2211 | 0:1594bb11fa13 | 578 | |
Tanja2211 | 0:1594bb11fa13 | 579 | log_timerB.attach(Biceps, 0.005); |
Tanja2211 | 0:1594bb11fa13 | 580 | wait(2); |
Tanja2211 | 0:1594bb11fa13 | 581 | log_timerB.detach(); |
Tanja2211 | 0:1594bb11fa13 | 582 | } |
Tanja2211 | 0:1594bb11fa13 | 583 | |
Tanja2211 | 0:1594bb11fa13 | 584 | void motor2aansturing() |
Tanja2211 | 0:1594bb11fa13 | 585 | { |
Tanja2211 | 0:1594bb11fa13 | 586 | if (positie == 0) { |
Tanja2211 | 0:1594bb11fa13 | 587 | if (motor2.getPosition()>= 0) { |
Tanja2211 | 0:1594bb11fa13 | 588 | motordir2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 589 | pwm_motor2.write(PWM2); |
Tanja2211 | 0:1594bb11fa13 | 590 | } else { |
Tanja2211 | 0:1594bb11fa13 | 591 | pwm_motor2.write(0); |
Tanja2211 | 0:1594bb11fa13 | 592 | } |
Tanja2211 | 0:1594bb11fa13 | 593 | } |
Tanja2211 | 0:1594bb11fa13 | 594 | if (positie ==1) { |
Tanja2211 | 0:1594bb11fa13 | 595 | if (motor2.getPosition()>= 2) { |
Tanja2211 | 0:1594bb11fa13 | 596 | motordir2 = 1; |
Tanja2211 | 0:1594bb11fa13 | 597 | pwm_motor2.write(PWM2); |
Tanja2211 | 0:1594bb11fa13 | 598 | } |
Tanja2211 | 0:1594bb11fa13 | 599 | if (motor2.getPosition()<= 2) { |
Tanja2211 | 0:1594bb11fa13 | 600 | motordir2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 601 | pwm_motor2.write(PWM2); |
Tanja2211 | 0:1594bb11fa13 | 602 | } else { |
Tanja2211 | 0:1594bb11fa13 | 603 | pwm_motor2.write(0); |
Tanja2211 | 0:1594bb11fa13 | 604 | } |
Tanja2211 | 0:1594bb11fa13 | 605 | } |
Tanja2211 | 0:1594bb11fa13 | 606 | if (positie ==2) { |
Tanja2211 | 0:1594bb11fa13 | 607 | if (motor2.getPosition()<= 4) { |
Tanja2211 | 0:1594bb11fa13 | 608 | motordir2 = 0; |
Tanja2211 | 0:1594bb11fa13 | 609 | pwm_motor2.write(PWM2); |
Tanja2211 | 0:1594bb11fa13 | 610 | } else { |
Tanja2211 | 0:1594bb11fa13 | 611 | pwm_motor2.write(0); |
Tanja2211 | 0:1594bb11fa13 | 612 | } |
Tanja2211 | 0:1594bb11fa13 | 613 | } //end if |
Tanja2211 | 0:1594bb11fa13 | 614 | } |
Tanja2211 | 0:1594bb11fa13 | 615 | |
Tanja2211 | 0:1594bb11fa13 | 616 | void motor1aansturing() |
Tanja2211 | 0:1594bb11fa13 | 617 | { |
Tanja2211 | 0:1594bb11fa13 | 618 | |
Tanja2211 | 4:68dc27d284f7 | 619 | if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) { |
Tanja2211 | 7:b40bbf5be443 | 620 | toestand = WACHTEN; |
Tanja2211 | 7:b40bbf5be443 | 621 | //out_i=0; |
Tanja2211 | 2:455216d1b5ba | 622 | pc.printf("if1\n"); |
Tanja2211 | 0:1594bb11fa13 | 623 | } |
Tanja2211 | 0:1594bb11fa13 | 624 | if (snelheidsstand != 0 && toestand == WACHTEN && stop == 0) { // stop = 0 in het begin let op dat dit na reset nog zo is |
Tanja2211 | 0:1594bb11fa13 | 625 | toestand = SLAAN; |
Tanja2211 | 0:1594bb11fa13 | 626 | pc.printf("slaan \n"); |
Tanja2211 | 0:1594bb11fa13 | 627 | if ( snelheidsstand==3) { |
Tanja2211 | 0:1594bb11fa13 | 628 | setspeed = V3; pc.printf("Snel 3 \n"); |
Tanja2211 | 0:1594bb11fa13 | 629 | } |
Tanja2211 | 0:1594bb11fa13 | 630 | if ( snelheidsstand==2) { |
Tanja2211 | 0:1594bb11fa13 | 631 | setspeed = V2; pc.printf("Snel 2\n"); |
Tanja2211 | 0:1594bb11fa13 | 632 | } |
Tanja2211 | 0:1594bb11fa13 | 633 | if ( snelheidsstand==1) { |
Tanja2211 | 0:1594bb11fa13 | 634 | setspeed = V1; pc.printf("Snel 1 \n"); |
Tanja2211 | 0:1594bb11fa13 | 635 | } |
Tanja2211 | 0:1594bb11fa13 | 636 | } |
Tanja2211 | 4:68dc27d284f7 | 637 | if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) { |
Jolein | 1:5d30a2ea2e11 | 638 | pc.printf("toestand = terugkeren\n\r"); |
Tanja2211 | 7:b40bbf5be443 | 639 | //out_i=0; |
Tanja2211 | 0:1594bb11fa13 | 640 | toestand = TERUGKEREN; |
Tanja2211 | 0:1594bb11fa13 | 641 | stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is... |
Tanja2211 | 0:1594bb11fa13 | 642 | } |
Tanja2211 | 0:1594bb11fa13 | 643 | if (toestand == TERUGKEREN) { |
Jolein | 1:5d30a2ea2e11 | 644 | pc.printf("motor gaat terugkeren\n\r"); |
Tanja2211 | 7:b40bbf5be443 | 645 | new_pwm=pid(setspeed, motor1.getSpeed()); |
Tanja2211 | 7:b40bbf5be443 | 646 | |
Tanja2211 | 5:8f256c16fe6c | 647 | pwm_motor1.write(new_pwm); |
Tanja2211 | 2:455216d1b5ba | 648 | pc.printf("new pwm %f\r\n",new_pwm); |
Jolein | 1:5d30a2ea2e11 | 649 | motordir1 = 0; |
Tanja2211 | 0:1594bb11fa13 | 650 | } |
Tanja2211 | 0:1594bb11fa13 | 651 | if (toestand == WACHTEN) { |
Tanja2211 | 0:1594bb11fa13 | 652 | pwm_motor1.write(0); |
Tanja2211 | 2:455216d1b5ba | 653 | pc.printf("ifwachten\n"); |
Tanja2211 | 0:1594bb11fa13 | 654 | } |
Tanja2211 | 0:1594bb11fa13 | 655 | if (toestand == SLAAN) { |
Tanja2211 | 7:b40bbf5be443 | 656 | pid(setspeed, motor1.getSpeed()); |
Jolein | 1:5d30a2ea2e11 | 657 | motordir1 = 1; |
Tanja2211 | 0:1594bb11fa13 | 658 | pwm_motor1.write(new_pwm); |
Tanja2211 | 2:455216d1b5ba | 659 | pc.printf("SLAAN\n"); |
Tanja2211 | 2:455216d1b5ba | 660 | |
Tanja2211 | 0:1594bb11fa13 | 661 | } |
Tanja2211 | 0:1594bb11fa13 | 662 | } |