kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_incl_regelaar by
PROJECT_main.cpp@12:b09b7fe5550c, 2014-11-03 (annotated)
- Committer:
- Hooglugt
- Date:
- Mon Nov 03 22:47:59 2014 +0000
- Revision:
- 12:b09b7fe5550c
- Parent:
- 11:b517e73a98ab
- Child:
- 13:05697c9b13d7
- Child:
- 14:c2389571f8d6
voor het verslag,
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hooglugt | 0:99cbc87af37c | 1 | #include "mbed.h" |
Hooglugt | 0:99cbc87af37c | 2 | #include "MODSERIAL.h" |
Hooglugt | 0:99cbc87af37c | 3 | #include "HIDScope.h" |
Hooglugt | 0:99cbc87af37c | 4 | #include "arm_math.h" |
Hooglugt | 0:99cbc87af37c | 5 | #include "encoder.h" |
Hooglugt | 0:99cbc87af37c | 6 | |
Hooglugt | 0:99cbc87af37c | 7 | #define TSAMP 0.001 // sample freq encoder motor |
Hooglugt | 0:99cbc87af37c | 8 | #define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan |
Hooglugt | 1:d44a866de64f | 9 | #define TIMEBETWEENBLINK 100 // sec voor volgende blink |
Hooglugt | 0:99cbc87af37c | 10 | #define TSAMP_EMG 0.002 //sample frequency emg |
Hooglugt | 7:ca1ade91bd14 | 11 | #define KALIBRATIONTIME 500 // 10 sec voor bepalen van maximale biceps/triceps waarde |
Hooglugt | 1:d44a866de64f | 12 | #define FACTOR 0.6 //factor*max_waarde = threshold emg |
Hooglugt | 7:ca1ade91bd14 | 13 | |
Hooglugt | 0:99cbc87af37c | 14 | //Define objects |
Hooglugt | 7:ca1ade91bd14 | 15 | |
Hooglugt | 11:b517e73a98ab | 16 | HIDScope scope(5); |
Hooglugt | 7:ca1ade91bd14 | 17 | |
Hooglugt | 0:99cbc87af37c | 18 | AnalogIn emg0(PTB1); //Analog input biceps |
Hooglugt | 0:99cbc87af37c | 19 | AnalogIn emg1(PTB2); //Analog input triceps |
Hooglugt | 0:99cbc87af37c | 20 | |
Hooglugt | 0:99cbc87af37c | 21 | Ticker log_timer; //sample emg |
Hooglugt | 1:d44a866de64f | 22 | Ticker blink; //ledjes aan/uit |
Hooglugt | 1:d44a866de64f | 23 | Ticker blink2; //extra tikker zodat kalbi en kaltri tegelijkertijd aankunnen |
Hooglugt | 0:99cbc87af37c | 24 | Ticker looptimer; //motor regelaar |
Hooglugt | 0:99cbc87af37c | 25 | |
Hooglugt | 0:99cbc87af37c | 26 | MODSERIAL pc(USBTX,USBRX); |
Hooglugt | 0:99cbc87af37c | 27 | |
Hooglugt | 0:99cbc87af37c | 28 | arm_biquad_casd_df1_inst_f32 bihighpass; |
Hooglugt | 0:99cbc87af37c | 29 | float bihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071 |
Hooglugt | 0:99cbc87af37c | 30 | float bihighpass_states[4]; |
Hooglugt | 0:99cbc87af37c | 31 | |
Hooglugt | 0:99cbc87af37c | 32 | arm_biquad_casd_df1_inst_f32 binotch; |
Hooglugt | 0:99cbc87af37c | 33 | float binotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10 |
Hooglugt | 0:99cbc87af37c | 34 | float binotch_states[4]; |
Hooglugt | 0:99cbc87af37c | 35 | |
Hooglugt | 0:99cbc87af37c | 36 | arm_biquad_casd_df1_inst_f32 trihighpass; |
Hooglugt | 0:99cbc87af37c | 37 | float trihighpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071 |
Hooglugt | 0:99cbc87af37c | 38 | float trihighpass_states[4]; |
Hooglugt | 0:99cbc87af37c | 39 | |
Hooglugt | 0:99cbc87af37c | 40 | arm_biquad_casd_df1_inst_f32 trinotch; |
Hooglugt | 0:99cbc87af37c | 41 | float trinotch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10 |
Hooglugt | 0:99cbc87af37c | 42 | float trinotch_states[4]; |
Hooglugt | 0:99cbc87af37c | 43 | |
Hooglugt | 0:99cbc87af37c | 44 | float bi_result = 0; |
Hooglugt | 0:99cbc87af37c | 45 | float tri_result = 0; |
Hooglugt | 0:99cbc87af37c | 46 | |
Hooglugt | 0:99cbc87af37c | 47 | float bi_max = 0; |
Hooglugt | 0:99cbc87af37c | 48 | float tri_max = 0; |
Hooglugt | 0:99cbc87af37c | 49 | |
Hooglugt | 0:99cbc87af37c | 50 | // variables for biceps MAF |
Hooglugt | 0:99cbc87af37c | 51 | float y0 = 0; |
Hooglugt | 0:99cbc87af37c | 52 | float y1 = 0; |
Hooglugt | 0:99cbc87af37c | 53 | float y2 = 0; |
Hooglugt | 0:99cbc87af37c | 54 | float y3 = 0; |
Hooglugt | 0:99cbc87af37c | 55 | float y4 = 0; |
Hooglugt | 0:99cbc87af37c | 56 | float y5 = 0; |
Hooglugt | 0:99cbc87af37c | 57 | float y6 = 0; |
Hooglugt | 0:99cbc87af37c | 58 | float y7 = 0; |
Hooglugt | 0:99cbc87af37c | 59 | float y8 = 0; |
Hooglugt | 0:99cbc87af37c | 60 | float y9 = 0; |
Hooglugt | 0:99cbc87af37c | 61 | |
Hooglugt | 0:99cbc87af37c | 62 | // variables for triceps MAF |
Hooglugt | 0:99cbc87af37c | 63 | float x0 = 0; |
Hooglugt | 0:99cbc87af37c | 64 | float x1 = 0; |
Hooglugt | 0:99cbc87af37c | 65 | float x2 = 0; |
Hooglugt | 0:99cbc87af37c | 66 | float x3 = 0; |
Hooglugt | 0:99cbc87af37c | 67 | float x4 = 0; |
Hooglugt | 0:99cbc87af37c | 68 | float x5 = 0; |
Hooglugt | 0:99cbc87af37c | 69 | float x6 = 0; |
Hooglugt | 0:99cbc87af37c | 70 | float x7 = 0; |
Hooglugt | 0:99cbc87af37c | 71 | float x8 = 0; |
Hooglugt | 0:99cbc87af37c | 72 | float x9 = 0; |
Hooglugt | 0:99cbc87af37c | 73 | |
Hooglugt | 0:99cbc87af37c | 74 | //LED interface |
Hooglugt | 0:99cbc87af37c | 75 | DigitalOut dir1(PTA1); |
Hooglugt | 0:99cbc87af37c | 76 | DigitalOut dir2(PTA2); |
Hooglugt | 0:99cbc87af37c | 77 | DigitalOut dir3(PTD4); |
Hooglugt | 0:99cbc87af37c | 78 | DigitalOut for1(PTA12); |
Hooglugt | 0:99cbc87af37c | 79 | DigitalOut for2(PTA13); |
Hooglugt | 0:99cbc87af37c | 80 | DigitalOut for3(PTD1); |
Hooglugt | 0:99cbc87af37c | 81 | |
Hooglugt | 0:99cbc87af37c | 82 | uint8_t direction = 0; |
Hooglugt | 0:99cbc87af37c | 83 | uint8_t force = 0; |
Hooglugt | 0:99cbc87af37c | 84 | |
Hooglugt | 0:99cbc87af37c | 85 | //motorcontrol objects |
Hooglugt | 0:99cbc87af37c | 86 | |
Hooglugt | 0:99cbc87af37c | 87 | //motor 1, voltage pins op M2 |
Hooglugt | 0:99cbc87af37c | 88 | Encoder motor1(PTD3, PTD5); |
Hooglugt | 0:99cbc87af37c | 89 | DigitalOut motor1dir(PTC9); |
Hooglugt | 0:99cbc87af37c | 90 | PwmOut pwm_motor1(PTC8); |
Hooglugt | 0:99cbc87af37c | 91 | |
Hooglugt | 0:99cbc87af37c | 92 | //motor 2, voltage pins op M1 |
Hooglugt | 0:99cbc87af37c | 93 | Encoder motor2(PTD2,PTD0); |
Hooglugt | 0:99cbc87af37c | 94 | DigitalOut motor2dir(PTA4); |
Hooglugt | 0:99cbc87af37c | 95 | PwmOut pwm_motor2(PTA5); |
Hooglugt | 0:99cbc87af37c | 96 | |
Hooglugt | 0:99cbc87af37c | 97 | float integral = 0; |
Hooglugt | 7:ca1ade91bd14 | 98 | float derivative = 0; |
Hooglugt | 4:697d5a806cc4 | 99 | float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden |
Hooglugt | 4:697d5a806cc4 | 100 | float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd |
Hooglugt | 7:ca1ade91bd14 | 101 | float kalibratie = 0; |
Hooglugt | 0:99cbc87af37c | 102 | float controlerror = 0; |
Hooglugt | 6:14051758db6f | 103 | float previouserror = 0; |
Hooglugt | 0:99cbc87af37c | 104 | float pwm = 0; |
Hooglugt | 0:99cbc87af37c | 105 | |
Hooglugt | 8:75980dc35763 | 106 | float pwm1 =0; |
Hooglugt | 8:75980dc35763 | 107 | float integral1 = 0; |
Hooglugt | 8:75980dc35763 | 108 | float derivative1 = 0; |
Hooglugt | 8:75980dc35763 | 109 | float controlerror1 = 0; |
Hooglugt | 8:75980dc35763 | 110 | float previouserror1 = 0; |
Hooglugt | 8:75980dc35763 | 111 | |
Hooglugt | 8:75980dc35763 | 112 | int state = 1; |
Hooglugt | 9:0bc7f83b761e | 113 | int count = 0; |
Hooglugt | 9:0bc7f83b761e | 114 | float angle = 0; |
Hooglugt | 8:75980dc35763 | 115 | |
Hooglugt | 0:99cbc87af37c | 116 | float omrekenfactor1 = 0.0028035714; // 6.28/(32*70) |
Hooglugt | 8:75980dc35763 | 117 | float omrekenfactor2 = 0.0015213178; // 6.28/(24*172); |
Hooglugt | 0:99cbc87af37c | 118 | |
Hooglugt | 1:d44a866de64f | 119 | float setpoint1 = 0; //te behalen speed van motor1 (37D) |
Hooglugt | 1:d44a866de64f | 120 | float setpoint2 = 0; //te behalen hoek van motor2 (25D) |
Hooglugt | 0:99cbc87af37c | 121 | |
Hooglugt | 9:0bc7f83b761e | 122 | float Kp1 = 12.0; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF |
Hooglugt | 9:0bc7f83b761e | 123 | float Ki1 = 0.0; //Kp en Ki van motor1, voor de slag |
Hooglugt | 6:14051758db6f | 124 | float Kd1 = 0.0; |
Hooglugt | 6:14051758db6f | 125 | |
Hooglugt | 9:0bc7f83b761e | 126 | float Kp2 = 8.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return |
Hooglugt | 9:0bc7f83b761e | 127 | float Ki2 = 0.0; //0.30 en 0.20 |
Hooglugt | 9:0bc7f83b761e | 128 | float Kd2 = 0.0; |
Hooglugt | 0:99cbc87af37c | 129 | |
Hooglugt | 0:99cbc87af37c | 130 | volatile bool looptimerflag; //voor motorcontrol TSAMP |
Hooglugt | 0:99cbc87af37c | 131 | |
Hooglugt | 0:99cbc87af37c | 132 | //functies |
Hooglugt | 0:99cbc87af37c | 133 | |
Hooglugt | 0:99cbc87af37c | 134 | void setlooptimerflag(void) |
Hooglugt | 0:99cbc87af37c | 135 | { |
Hooglugt | 0:99cbc87af37c | 136 | looptimerflag = true; |
Hooglugt | 10:6bf3e25f020a | 137 | } |
Hooglugt | 10:6bf3e25f020a | 138 | |
Hooglugt | 10:6bf3e25f020a | 139 | Ticker hid; |
Hooglugt | 10:6bf3e25f020a | 140 | |
Hooglugt | 10:6bf3e25f020a | 141 | void hidscope(void){ |
Hooglugt | 11:b517e73a98ab | 142 | |
Hooglugt | 10:6bf3e25f020a | 143 | scope.send(); |
Hooglugt | 0:99cbc87af37c | 144 | } |
Hooglugt | 0:99cbc87af37c | 145 | |
Hooglugt | 0:99cbc87af37c | 146 | void keep_in_range(float * in, float min, float max) |
Hooglugt | 0:99cbc87af37c | 147 | { |
Hooglugt | 0:99cbc87af37c | 148 | *in > min ? *in < max? : *in = max: *in = max; |
Hooglugt | 0:99cbc87af37c | 149 | } |
Hooglugt | 0:99cbc87af37c | 150 | |
Hooglugt | 0:99cbc87af37c | 151 | void looper() |
Hooglugt | 0:99cbc87af37c | 152 | { |
Hooglugt | 0:99cbc87af37c | 153 | //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively |
Hooglugt | 0:99cbc87af37c | 154 | float emg_biceps; //Float voor EMG-waarde biceps |
Hooglugt | 0:99cbc87af37c | 155 | float emg_triceps; //Float voor EMG-waarde triceps |
Hooglugt | 0:99cbc87af37c | 156 | |
Hooglugt | 0:99cbc87af37c | 157 | emg_biceps = emg0.read(); // read float value (0..1 = 0..3.3V) biceps |
Hooglugt | 0:99cbc87af37c | 158 | emg_triceps = emg1.read(); // read float value (0..1 = 0..3.3V) triceps |
Hooglugt | 0:99cbc87af37c | 159 | |
Hooglugt | 0:99cbc87af37c | 160 | //process emg biceps |
Hooglugt | 0:99cbc87af37c | 161 | arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 ); |
Hooglugt | 0:99cbc87af37c | 162 | arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 ); |
Hooglugt | 0:99cbc87af37c | 163 | y0 = fabs(emg_biceps); |
Hooglugt | 0:99cbc87af37c | 164 | bi_result = (y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1); |
Hooglugt | 0:99cbc87af37c | 165 | y9=y8; |
Hooglugt | 0:99cbc87af37c | 166 | y8=y7; |
Hooglugt | 0:99cbc87af37c | 167 | y7=y6; |
Hooglugt | 0:99cbc87af37c | 168 | y6=y5; |
Hooglugt | 0:99cbc87af37c | 169 | y5=y4; |
Hooglugt | 0:99cbc87af37c | 170 | y4=y3; |
Hooglugt | 0:99cbc87af37c | 171 | y3=y2; |
Hooglugt | 0:99cbc87af37c | 172 | y2=y1; |
Hooglugt | 0:99cbc87af37c | 173 | y1=y0; |
Hooglugt | 0:99cbc87af37c | 174 | |
Hooglugt | 0:99cbc87af37c | 175 | //process emg triceps |
Hooglugt | 0:99cbc87af37c | 176 | arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 ); |
Hooglugt | 0:99cbc87af37c | 177 | arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 ); |
Hooglugt | 0:99cbc87af37c | 178 | x0 = fabs(emg_triceps); |
Hooglugt | 0:99cbc87af37c | 179 | tri_result = (x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1); |
Hooglugt | 0:99cbc87af37c | 180 | x9=x8; |
Hooglugt | 0:99cbc87af37c | 181 | x8=x7; |
Hooglugt | 0:99cbc87af37c | 182 | x7=x6; |
Hooglugt | 0:99cbc87af37c | 183 | x6=x5; |
Hooglugt | 0:99cbc87af37c | 184 | x5=x4; |
Hooglugt | 0:99cbc87af37c | 185 | x4=x3; |
Hooglugt | 0:99cbc87af37c | 186 | x3=x2; |
Hooglugt | 0:99cbc87af37c | 187 | x2=x1; |
Hooglugt | 0:99cbc87af37c | 188 | x1=x0; |
Hooglugt | 0:99cbc87af37c | 189 | } |
Hooglugt | 0:99cbc87af37c | 190 | |
Hooglugt | 0:99cbc87af37c | 191 | void kalbi() //blinking three lights, first row - 2nd row unlit |
Hooglugt | 0:99cbc87af37c | 192 | { |
Hooglugt | 0:99cbc87af37c | 193 | if(dir1==0) { |
Hooglugt | 0:99cbc87af37c | 194 | dir1 = dir2 = dir3 = 1; |
Hooglugt | 0:99cbc87af37c | 195 | } else { |
Hooglugt | 0:99cbc87af37c | 196 | dir1 = dir2 = dir3 = 0; |
Hooglugt | 0:99cbc87af37c | 197 | } |
Hooglugt | 0:99cbc87af37c | 198 | } |
Hooglugt | 0:99cbc87af37c | 199 | |
Hooglugt | 0:99cbc87af37c | 200 | void kaltri() //blinking three lights, 2nd row - first row lit |
Hooglugt | 0:99cbc87af37c | 201 | { |
Hooglugt | 0:99cbc87af37c | 202 | if(for1==0) { |
Hooglugt | 0:99cbc87af37c | 203 | for1 = for2 = for3 = 1; |
Hooglugt | 0:99cbc87af37c | 204 | } else { |
Hooglugt | 0:99cbc87af37c | 205 | for1 = for2 = for3 = 0; |
Hooglugt | 0:99cbc87af37c | 206 | } |
Hooglugt | 0:99cbc87af37c | 207 | } |
Hooglugt | 0:99cbc87af37c | 208 | |
Hooglugt | 0:99cbc87af37c | 209 | void okay() //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P) |
Hooglugt | 0:99cbc87af37c | 210 | { |
Hooglugt | 0:99cbc87af37c | 211 | if(direction == 1 && force == 1) { // links zwak |
Hooglugt | 0:99cbc87af37c | 212 | if(for1 == 0 && dir1 == 0) { |
Hooglugt | 0:99cbc87af37c | 213 | for1 = dir1 = 1; |
Hooglugt | 0:99cbc87af37c | 214 | } else { |
Hooglugt | 0:99cbc87af37c | 215 | for1 = dir1 = 0; |
Hooglugt | 0:99cbc87af37c | 216 | } |
Hooglugt | 0:99cbc87af37c | 217 | } |
Hooglugt | 0:99cbc87af37c | 218 | if(direction == 1 && force == 2) { // links normaal |
Hooglugt | 0:99cbc87af37c | 219 | if(for2 == 0 && dir1 == 0) { |
Hooglugt | 0:99cbc87af37c | 220 | for2 = dir1 = 1; |
Hooglugt | 0:99cbc87af37c | 221 | } else { |
Hooglugt | 0:99cbc87af37c | 222 | for2 = dir1 = 0; |
Hooglugt | 0:99cbc87af37c | 223 | } |
Hooglugt | 0:99cbc87af37c | 224 | } |
Hooglugt | 0:99cbc87af37c | 225 | if(direction == 1 && force == 3) { // links sterk |
Hooglugt | 0:99cbc87af37c | 226 | if(for3 == 0 && dir1 == 0) { |
Hooglugt | 0:99cbc87af37c | 227 | for3 = dir1 = 1; |
Hooglugt | 0:99cbc87af37c | 228 | } else { |
Hooglugt | 0:99cbc87af37c | 229 | for3 = dir1 = 0; |
Hooglugt | 0:99cbc87af37c | 230 | } |
Hooglugt | 0:99cbc87af37c | 231 | } |
Hooglugt | 0:99cbc87af37c | 232 | if(direction == 2 && force == 1) { // mid zwak |
Hooglugt | 0:99cbc87af37c | 233 | if(for1 == 0 && dir2 == 0) { |
Hooglugt | 0:99cbc87af37c | 234 | for1 = dir2 = 1; |
Hooglugt | 0:99cbc87af37c | 235 | } else { |
Hooglugt | 0:99cbc87af37c | 236 | for1 = dir2 = 0; |
Hooglugt | 0:99cbc87af37c | 237 | } |
Hooglugt | 0:99cbc87af37c | 238 | } |
Hooglugt | 0:99cbc87af37c | 239 | if(direction == 2 && force == 2) { // mid normaal |
Hooglugt | 0:99cbc87af37c | 240 | if(for2 == 0 && dir2 == 0) { |
Hooglugt | 0:99cbc87af37c | 241 | for2 = dir2 = 1; |
Hooglugt | 0:99cbc87af37c | 242 | } else { |
Hooglugt | 0:99cbc87af37c | 243 | for2 = dir2 = 0; |
Hooglugt | 0:99cbc87af37c | 244 | } |
Hooglugt | 0:99cbc87af37c | 245 | } |
Hooglugt | 0:99cbc87af37c | 246 | if(direction == 2 && force == 3) { // mid sterk |
Hooglugt | 0:99cbc87af37c | 247 | if(for3 == 0 && dir2 == 0) { |
Hooglugt | 0:99cbc87af37c | 248 | for3 = dir2 = 1; |
Hooglugt | 0:99cbc87af37c | 249 | } else { |
Hooglugt | 0:99cbc87af37c | 250 | for3 = dir2 = 0; |
Hooglugt | 0:99cbc87af37c | 251 | } |
Hooglugt | 0:99cbc87af37c | 252 | } |
Hooglugt | 0:99cbc87af37c | 253 | if(direction == 3 && force == 1) { // rechts zwak |
Hooglugt | 0:99cbc87af37c | 254 | if(for1 == 0 && dir3 == 0) { |
Hooglugt | 0:99cbc87af37c | 255 | for1 = dir3 = 1; |
Hooglugt | 0:99cbc87af37c | 256 | } else { |
Hooglugt | 0:99cbc87af37c | 257 | for1 = dir3 = 0; |
Hooglugt | 0:99cbc87af37c | 258 | } |
Hooglugt | 0:99cbc87af37c | 259 | } |
Hooglugt | 0:99cbc87af37c | 260 | if(direction == 3 && force == 2) { // rechts normaal |
Hooglugt | 0:99cbc87af37c | 261 | if(for2 == 0 && dir3 == 0) { |
Hooglugt | 0:99cbc87af37c | 262 | for2 = dir3 = 1; |
Hooglugt | 0:99cbc87af37c | 263 | } else { |
Hooglugt | 0:99cbc87af37c | 264 | for2 = dir3 = 0; |
Hooglugt | 0:99cbc87af37c | 265 | } |
Hooglugt | 0:99cbc87af37c | 266 | } |
Hooglugt | 0:99cbc87af37c | 267 | if(direction == 3 && force == 3) { // rechts sterk |
Hooglugt | 0:99cbc87af37c | 268 | if(for3 == 0 && dir3 == 0) { |
Hooglugt | 0:99cbc87af37c | 269 | for3 = dir3 = 1; |
Hooglugt | 0:99cbc87af37c | 270 | } else { |
Hooglugt | 0:99cbc87af37c | 271 | for3 = dir3 = 0; |
Hooglugt | 0:99cbc87af37c | 272 | } |
Hooglugt | 0:99cbc87af37c | 273 | } |
Hooglugt | 0:99cbc87af37c | 274 | } |
Hooglugt | 0:99cbc87af37c | 275 | |
Hooglugt | 0:99cbc87af37c | 276 | int main() |
Hooglugt | 0:99cbc87af37c | 277 | { |
Hooglugt | 11:b517e73a98ab | 278 | hid.attach(hidscope, 0.01); |
Hooglugt | 0:99cbc87af37c | 279 | pc.baud(115200); //baudrate instellen |
Hooglugt | 0:99cbc87af37c | 280 | log_timer.attach(looper, TSAMP_EMG); //EMG, Fsample 500 Hz |
Hooglugt | 0:99cbc87af37c | 281 | looptimer.attach(setlooptimerflag,TSAMP); |
Hooglugt | 0:99cbc87af37c | 282 | pwm_motor1.period_us(100); //10kHz PWM frequency |
Hooglugt | 0:99cbc87af37c | 283 | pwm_motor2.period_us(100); //10kHz PWM frequency |
Hooglugt | 0:99cbc87af37c | 284 | |
Hooglugt | 0:99cbc87af37c | 285 | //set up filters |
Hooglugt | 0:99cbc87af37c | 286 | arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states); |
Hooglugt | 0:99cbc87af37c | 287 | arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states); |
Hooglugt | 0:99cbc87af37c | 288 | |
Hooglugt | 0:99cbc87af37c | 289 | arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states); |
Hooglugt | 0:99cbc87af37c | 290 | arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states); |
Hooglugt | 0:99cbc87af37c | 291 | |
Hooglugt | 0:99cbc87af37c | 292 | //kalibratie |
Hooglugt | 0:99cbc87af37c | 293 | |
Hooglugt | 0:99cbc87af37c | 294 | //motorarm naar nul-positie |
Hooglugt | 0:99cbc87af37c | 295 | blink.attach(kalbi, 0.2); |
Hooglugt | 1:d44a866de64f | 296 | blink2.attach(kaltri, 0.2); |
Hooglugt | 0:99cbc87af37c | 297 | |
Hooglugt | 0:99cbc87af37c | 298 | //calibration motor 2 |
Hooglugt | 7:ca1ade91bd14 | 299 | pwm_motor2.write(0.65); //lage PWM |
Hooglugt | 5:e5ca53305b87 | 300 | motor2dir = 0; //rechtsom |
Hooglugt | 0:99cbc87af37c | 301 | wait(1); // anders wordt de while(1) meteen onderbroken |
Hooglugt | 4:697d5a806cc4 | 302 | while(1) { |
Hooglugt | 5:e5ca53305b87 | 303 | if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // motor2.getSpeed()*omrekenfactor2 > -0.70), 0.70 is nog aan te passen |
Hooglugt | 0:99cbc87af37c | 304 | pwm_motor2.write(0); |
Hooglugt | 4:697d5a806cc4 | 305 | motor2.setPosition(0); |
Hooglugt | 4:697d5a806cc4 | 306 | goto motor1cal; |
Hooglugt | 0:99cbc87af37c | 307 | } |
Hooglugt | 0:99cbc87af37c | 308 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 309 | } |
Hooglugt | 4:697d5a806cc4 | 310 | motor1cal: |
Hooglugt | 0:99cbc87af37c | 311 | //calibration motor 1 |
Hooglugt | 7:ca1ade91bd14 | 312 | pwm_motor1.write(0.65); //lage PWM |
Hooglugt | 7:ca1ade91bd14 | 313 | motor1dir = 0; //linksom |
Hooglugt | 0:99cbc87af37c | 314 | wait(1); // anders wordt de while(1) meteen onderbroken |
Hooglugt | 4:697d5a806cc4 | 315 | while(1) { |
Hooglugt | 8:75980dc35763 | 316 | if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen |
Hooglugt | 0:99cbc87af37c | 317 | pwm_motor1.write(0); |
Hooglugt | 4:697d5a806cc4 | 318 | motor1.setPosition(0); |
Hooglugt | 4:697d5a806cc4 | 319 | goto emgcal; |
Hooglugt | 0:99cbc87af37c | 320 | } |
Hooglugt | 0:99cbc87af37c | 321 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 322 | } |
Hooglugt | 4:697d5a806cc4 | 323 | emgcal: |
Hooglugt | 0:99cbc87af37c | 324 | blink.detach(); |
Hooglugt | 1:d44a866de64f | 325 | blink2.detach(); |
Hooglugt | 0:99cbc87af37c | 326 | dir1 = dir2 = dir3 = 1; |
Hooglugt | 4:697d5a806cc4 | 327 | for1 = for2 = for3 = 1; |
Hooglugt | 4:697d5a806cc4 | 328 | pc.printf("kalmoarm "); |
Hooglugt | 0:99cbc87af37c | 329 | wait (1); |
Hooglugt | 0:99cbc87af37c | 330 | for1 = for2 = for3 = 0; |
Hooglugt | 0:99cbc87af37c | 331 | |
Hooglugt | 8:75980dc35763 | 332 | if(kalibratie==0) { |
Hooglugt | 8:75980dc35763 | 333 | //biceps kalibratie |
Hooglugt | 8:75980dc35763 | 334 | blink.attach(kalbi, 0.2); |
Hooglugt | 8:75980dc35763 | 335 | for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { |
Hooglugt | 8:75980dc35763 | 336 | if (bi_max < bi_result) { |
Hooglugt | 8:75980dc35763 | 337 | bi_max = bi_result; |
Hooglugt | 8:75980dc35763 | 338 | } |
Hooglugt | 8:75980dc35763 | 339 | wait (0.01); |
Hooglugt | 0:99cbc87af37c | 340 | } |
Hooglugt | 8:75980dc35763 | 341 | blink.detach(); |
Hooglugt | 8:75980dc35763 | 342 | dir1 = dir2 = dir3 = 1; |
Hooglugt | 8:75980dc35763 | 343 | pc.printf("kalbi "); |
Hooglugt | 8:75980dc35763 | 344 | wait (1); |
Hooglugt | 0:99cbc87af37c | 345 | |
Hooglugt | 8:75980dc35763 | 346 | //triceps kalibratie |
Hooglugt | 8:75980dc35763 | 347 | blink.attach(kaltri, 0.2); |
Hooglugt | 8:75980dc35763 | 348 | for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { |
Hooglugt | 8:75980dc35763 | 349 | if (tri_max < tri_result) { |
Hooglugt | 8:75980dc35763 | 350 | tri_max = tri_result; |
Hooglugt | 8:75980dc35763 | 351 | } |
Hooglugt | 8:75980dc35763 | 352 | wait (0.01); |
Hooglugt | 0:99cbc87af37c | 353 | } |
Hooglugt | 8:75980dc35763 | 354 | blink.detach(); |
Hooglugt | 8:75980dc35763 | 355 | for1 = for2 = for3 = 1; |
Hooglugt | 8:75980dc35763 | 356 | pc.printf("kaltri "); |
Hooglugt | 8:75980dc35763 | 357 | wait (1); |
Hooglugt | 8:75980dc35763 | 358 | for1 = for2 = for3 = 0; |
Hooglugt | 8:75980dc35763 | 359 | kalibratie = 1; |
Hooglugt | 0:99cbc87af37c | 360 | } |
Hooglugt | 0:99cbc87af37c | 361 | |
Hooglugt | 4:697d5a806cc4 | 362 | directionchoice: |
Hooglugt | 1:d44a866de64f | 363 | log_timer.attach(looper, TSAMP_EMG); |
Hooglugt | 9:0bc7f83b761e | 364 | direction = 1; |
Hooglugt | 9:0bc7f83b761e | 365 | force = 1; |
Hooglugt | 9:0bc7f83b761e | 366 | goto motorcontrol; |
Hooglugt | 0:99cbc87af37c | 367 | |
Hooglugt | 4:697d5a806cc4 | 368 | while(1) { //Loop keuze DIRECTION |
Hooglugt | 0:99cbc87af37c | 369 | for(int i=1; i<4; i++) { |
Hooglugt | 0:99cbc87af37c | 370 | if(i==1) { //red |
Hooglugt | 0:99cbc87af37c | 371 | dir1=1; |
Hooglugt | 0:99cbc87af37c | 372 | dir2=0; |
Hooglugt | 0:99cbc87af37c | 373 | dir3=0; |
Hooglugt | 0:99cbc87af37c | 374 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 1:d44a866de64f | 375 | if(bi_result>FACTOR*bi_max) { |
Hooglugt | 0:99cbc87af37c | 376 | direction = 1; |
Hooglugt | 4:697d5a806cc4 | 377 | pc.printf("links "); |
Hooglugt | 0:99cbc87af37c | 378 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie |
Hooglugt | 0:99cbc87af37c | 379 | goto forcechoice; // goes to second while(1) for the deciding the force |
Hooglugt | 0:99cbc87af37c | 380 | } else { |
Hooglugt | 0:99cbc87af37c | 381 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 382 | } |
Hooglugt | 0:99cbc87af37c | 383 | } |
Hooglugt | 0:99cbc87af37c | 384 | } |
Hooglugt | 0:99cbc87af37c | 385 | if(i==2) { //green |
Hooglugt | 0:99cbc87af37c | 386 | dir1 =0; |
Hooglugt | 0:99cbc87af37c | 387 | dir2 =1; |
Hooglugt | 0:99cbc87af37c | 388 | dir3 =0; |
Hooglugt | 0:99cbc87af37c | 389 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 1:d44a866de64f | 390 | if(bi_result>FACTOR*bi_max) { |
Hooglugt | 0:99cbc87af37c | 391 | direction = 2; |
Hooglugt | 4:697d5a806cc4 | 392 | pc.printf("mid "); |
Hooglugt | 0:99cbc87af37c | 393 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie |
Hooglugt | 0:99cbc87af37c | 394 | goto forcechoice; |
Hooglugt | 0:99cbc87af37c | 395 | } else { |
Hooglugt | 0:99cbc87af37c | 396 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 397 | } |
Hooglugt | 0:99cbc87af37c | 398 | } |
Hooglugt | 0:99cbc87af37c | 399 | } |
Hooglugt | 0:99cbc87af37c | 400 | if(i==3) { //blue |
Hooglugt | 0:99cbc87af37c | 401 | dir1 =0; |
Hooglugt | 0:99cbc87af37c | 402 | dir2 =0; |
Hooglugt | 0:99cbc87af37c | 403 | dir3 =1; |
Hooglugt | 0:99cbc87af37c | 404 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 1:d44a866de64f | 405 | if(bi_result>FACTOR*bi_max) { |
Hooglugt | 0:99cbc87af37c | 406 | direction = 3; |
Hooglugt | 4:697d5a806cc4 | 407 | pc.printf("rechts "); |
Hooglugt | 0:99cbc87af37c | 408 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie |
Hooglugt | 0:99cbc87af37c | 409 | goto forcechoice; |
Hooglugt | 0:99cbc87af37c | 410 | } else { |
Hooglugt | 0:99cbc87af37c | 411 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 412 | } |
Hooglugt | 0:99cbc87af37c | 413 | } |
Hooglugt | 0:99cbc87af37c | 414 | } |
Hooglugt | 0:99cbc87af37c | 415 | } |
Hooglugt | 0:99cbc87af37c | 416 | } |
Hooglugt | 4:697d5a806cc4 | 417 | forcechoice: |
Hooglugt | 4:697d5a806cc4 | 418 | while(1) { //Loop keuze FORCE |
Hooglugt | 0:99cbc87af37c | 419 | for(int j=1; j<4; j++) { |
Hooglugt | 0:99cbc87af37c | 420 | if(j==1) { //red |
Hooglugt | 0:99cbc87af37c | 421 | for1=1; |
Hooglugt | 0:99cbc87af37c | 422 | for2=0; |
Hooglugt | 0:99cbc87af37c | 423 | for3=0; |
Hooglugt | 0:99cbc87af37c | 424 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 1:d44a866de64f | 425 | if(tri_result>FACTOR*tri_max) { |
Hooglugt | 0:99cbc87af37c | 426 | for1 = for2 = for3 = 0; |
Hooglugt | 4:697d5a806cc4 | 427 | pc.printf("reset "); |
Hooglugt | 0:99cbc87af37c | 428 | goto directionchoice; |
Hooglugt | 0:99cbc87af37c | 429 | } else { |
Hooglugt | 1:d44a866de64f | 430 | if(bi_result>FACTOR*bi_max) { |
Hooglugt | 0:99cbc87af37c | 431 | force = 1; |
Hooglugt | 4:697d5a806cc4 | 432 | pc.printf("zwak "); |
Hooglugt | 0:99cbc87af37c | 433 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie |
Hooglugt | 0:99cbc87af37c | 434 | goto choicesmade; |
Hooglugt | 0:99cbc87af37c | 435 | } else { |
Hooglugt | 0:99cbc87af37c | 436 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 437 | } |
Hooglugt | 0:99cbc87af37c | 438 | } |
Hooglugt | 0:99cbc87af37c | 439 | } |
Hooglugt | 0:99cbc87af37c | 440 | } |
Hooglugt | 0:99cbc87af37c | 441 | if(j==2) { //green |
Hooglugt | 0:99cbc87af37c | 442 | for1=0; |
Hooglugt | 0:99cbc87af37c | 443 | for2=1; |
Hooglugt | 0:99cbc87af37c | 444 | for3=0; |
Hooglugt | 0:99cbc87af37c | 445 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 1:d44a866de64f | 446 | if(tri_result>FACTOR*tri_max) { |
Hooglugt | 0:99cbc87af37c | 447 | for1 = for2 = for3 = 0; |
Hooglugt | 4:697d5a806cc4 | 448 | pc.printf("reset "); |
Hooglugt | 0:99cbc87af37c | 449 | goto directionchoice; |
Hooglugt | 0:99cbc87af37c | 450 | } else { |
Hooglugt | 1:d44a866de64f | 451 | if(bi_result>FACTOR*bi_max) { |
Hooglugt | 0:99cbc87af37c | 452 | force = 2; |
Hooglugt | 4:697d5a806cc4 | 453 | pc.printf("normaal "); |
Hooglugt | 0:99cbc87af37c | 454 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie |
Hooglugt | 0:99cbc87af37c | 455 | goto choicesmade; |
Hooglugt | 0:99cbc87af37c | 456 | } else { |
Hooglugt | 0:99cbc87af37c | 457 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 458 | } |
Hooglugt | 0:99cbc87af37c | 459 | } |
Hooglugt | 0:99cbc87af37c | 460 | } |
Hooglugt | 0:99cbc87af37c | 461 | } |
Hooglugt | 0:99cbc87af37c | 462 | if(j==3) { //blue |
Hooglugt | 0:99cbc87af37c | 463 | for1=0; |
Hooglugt | 0:99cbc87af37c | 464 | for2=0; |
Hooglugt | 0:99cbc87af37c | 465 | for3=1; |
Hooglugt | 0:99cbc87af37c | 466 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 1:d44a866de64f | 467 | if(tri_result>FACTOR*tri_max) { |
Hooglugt | 0:99cbc87af37c | 468 | for1 = for2 = for3 = 0; |
Hooglugt | 4:697d5a806cc4 | 469 | pc.printf("reset "); |
Hooglugt | 0:99cbc87af37c | 470 | goto directionchoice; |
Hooglugt | 0:99cbc87af37c | 471 | } else { |
Hooglugt | 1:d44a866de64f | 472 | if(bi_result>FACTOR*bi_max) { |
Hooglugt | 0:99cbc87af37c | 473 | force = 3; |
Hooglugt | 4:697d5a806cc4 | 474 | pc.printf("sterk "); |
Hooglugt | 0:99cbc87af37c | 475 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie |
Hooglugt | 0:99cbc87af37c | 476 | goto choicesmade; |
Hooglugt | 0:99cbc87af37c | 477 | } else { |
Hooglugt | 0:99cbc87af37c | 478 | wait(0.01); |
Hooglugt | 0:99cbc87af37c | 479 | } |
Hooglugt | 0:99cbc87af37c | 480 | } |
Hooglugt | 0:99cbc87af37c | 481 | } |
Hooglugt | 0:99cbc87af37c | 482 | } |
Hooglugt | 0:99cbc87af37c | 483 | } |
Hooglugt | 0:99cbc87af37c | 484 | } |
Hooglugt | 0:99cbc87af37c | 485 | |
Hooglugt | 0:99cbc87af37c | 486 | choicesmade: |
Hooglugt | 0:99cbc87af37c | 487 | blink.attach(okay, 0.2); |
Hooglugt | 4:697d5a806cc4 | 488 | while(1) { |
Hooglugt | 4:697d5a806cc4 | 489 | if(tri_result>FACTOR*tri_max) { |
Hooglugt | 0:99cbc87af37c | 490 | blink.detach(); |
Hooglugt | 4:697d5a806cc4 | 491 | pc.printf("reset "); |
Hooglugt | 1:d44a866de64f | 492 | switch (direction) { |
Hooglugt | 1:d44a866de64f | 493 | case 1: |
Hooglugt | 1:d44a866de64f | 494 | dir1 = 1; |
Hooglugt | 1:d44a866de64f | 495 | for1 = 1; |
Hooglugt | 1:d44a866de64f | 496 | for2 = for3 = 0; |
Hooglugt | 1:d44a866de64f | 497 | break; |
Hooglugt | 1:d44a866de64f | 498 | case 2: |
Hooglugt | 1:d44a866de64f | 499 | dir2 = 1; |
Hooglugt | 1:d44a866de64f | 500 | for1 = 1; |
Hooglugt | 1:d44a866de64f | 501 | for2 = for3 = 0; |
Hooglugt | 1:d44a866de64f | 502 | break; |
Hooglugt | 1:d44a866de64f | 503 | case 3: |
Hooglugt | 1:d44a866de64f | 504 | dir3 = 1; |
Hooglugt | 1:d44a866de64f | 505 | for1 = 1; |
Hooglugt | 1:d44a866de64f | 506 | for2 = for3 = 0; |
Hooglugt | 1:d44a866de64f | 507 | break; |
Hooglugt | 1:d44a866de64f | 508 | } |
Hooglugt | 4:697d5a806cc4 | 509 | |
Hooglugt | 4:697d5a806cc4 | 510 | wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze |
Hooglugt | 4:697d5a806cc4 | 511 | goto forcechoice; |
Hooglugt | 0:99cbc87af37c | 512 | } else { |
Hooglugt | 1:d44a866de64f | 513 | if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) { |
Hooglugt | 4:697d5a806cc4 | 514 | blink.detach(); |
Hooglugt | 4:697d5a806cc4 | 515 | log_timer.detach(); |
Hooglugt | 4:697d5a806cc4 | 516 | goto motorcontrol; |
Hooglugt | 0:99cbc87af37c | 517 | } else { |
Hooglugt | 0:99cbc87af37c | 518 | wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje) |
Hooglugt | 0:99cbc87af37c | 519 | } |
Hooglugt | 0:99cbc87af37c | 520 | } |
Hooglugt | 0:99cbc87af37c | 521 | } |
Hooglugt | 4:697d5a806cc4 | 522 | |
Hooglugt | 4:697d5a806cc4 | 523 | motorcontrol: |
Hooglugt | 4:697d5a806cc4 | 524 | |
Hooglugt | 0:99cbc87af37c | 525 | /* Vanaf hier komt de aansturing van de motor */ |
Hooglugt | 0:99cbc87af37c | 526 | |
Hooglugt | 8:75980dc35763 | 527 | |
Hooglugt | 8:75980dc35763 | 528 | // FORMAT_CODE_START |
Hooglugt | 8:75980dc35763 | 529 | setpoint1=0; |
Hooglugt | 8:75980dc35763 | 530 | setpoint2=0; |
Hooglugt | 8:75980dc35763 | 531 | integral1 = integral = 0; |
Hooglugt | 8:75980dc35763 | 532 | previouserror1 = previouserror = 0; |
Hooglugt | 11:b517e73a98ab | 533 | |
Hooglugt | 11:b517e73a98ab | 534 | |
Hooglugt | 8:75980dc35763 | 535 | while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) |
Hooglugt | 11:b517e73a98ab | 536 | while(!looptimerflag); |
Hooglugt | 9:0bc7f83b761e | 537 | looptimerflag = false; //clear flag |
Hooglugt | 12:b09b7fe5550c | 538 | |
Hooglugt | 11:b517e73a98ab | 539 | // FORMAT_CODE_START |
Hooglugt | 11:b517e73a98ab | 540 | |
Hooglugt | 11:b517e73a98ab | 541 | scope.set(0, motor2.getPosition()*omrekenfactor2); |
Hooglugt | 11:b517e73a98ab | 542 | scope.set(1, setpoint2); |
Hooglugt | 11:b517e73a98ab | 543 | scope.set(2, motor1.getPosition()*omrekenfactor1); |
Hooglugt | 11:b517e73a98ab | 544 | scope.set(3, setpoint1); |
Hooglugt | 11:b517e73a98ab | 545 | scope.set(4, state); |
Hooglugt | 1:d44a866de64f | 546 | |
Hooglugt | 11:b517e73a98ab | 547 | switch(state) { |
Hooglugt | 11:b517e73a98ab | 548 | case 1: { |
Hooglugt | 11:b517e73a98ab | 549 | setpoint1=0; |
Hooglugt | 11:b517e73a98ab | 550 | setpoint2 += 0.4*TSAMP; |
Hooglugt | 11:b517e73a98ab | 551 | switch (direction) { |
Hooglugt | 11:b517e73a98ab | 552 | case 1: |
Hooglugt | 11:b517e73a98ab | 553 | angle = 0.436332313+0.197222205; //(0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel |
Hooglugt | 11:b517e73a98ab | 554 | break; |
Hooglugt | 11:b517e73a98ab | 555 | case 2: |
Hooglugt | 11:b517e73a98ab | 556 | angle = 0.436332313; |
Hooglugt | 11:b517e73a98ab | 557 | break; |
Hooglugt | 11:b517e73a98ab | 558 | case 3: |
Hooglugt | 11:b517e73a98ab | 559 | angle = 0.436332313-0.197222205; |
Hooglugt | 11:b517e73a98ab | 560 | break; |
Hooglugt | 11:b517e73a98ab | 561 | } |
Hooglugt | 11:b517e73a98ab | 562 | if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1 |
Hooglugt | 11:b517e73a98ab | 563 | setpoint2 = angle; |
Hooglugt | 11:b517e73a98ab | 564 | count = 0; |
Hooglugt | 11:b517e73a98ab | 565 | state=2; |
Hooglugt | 11:b517e73a98ab | 566 | } |
Hooglugt | 11:b517e73a98ab | 567 | /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) { |
Hooglugt | 11:b517e73a98ab | 568 | state = 2; |
Hooglugt | 11:b517e73a98ab | 569 | }*/ |
Hooglugt | 11:b517e73a98ab | 570 | break; |
Hooglugt | 9:0bc7f83b761e | 571 | } |
Hooglugt | 11:b517e73a98ab | 572 | case 2: { |
Hooglugt | 11:b517e73a98ab | 573 | setpoint1 = 0; |
Hooglugt | 11:b517e73a98ab | 574 | count++; |
Hooglugt | 11:b517e73a98ab | 575 | if(count>1000) { |
Hooglugt | 11:b517e73a98ab | 576 | count = 0; |
Hooglugt | 11:b517e73a98ab | 577 | state = 3; |
Hooglugt | 11:b517e73a98ab | 578 | } |
Hooglugt | 11:b517e73a98ab | 579 | break; |
Hooglugt | 9:0bc7f83b761e | 580 | } |
Hooglugt | 11:b517e73a98ab | 581 | case 3: { |
Hooglugt | 11:b517e73a98ab | 582 | switch (force) { |
Hooglugt | 11:b517e73a98ab | 583 | case 1: |
Hooglugt | 12:b09b7fe5550c | 584 | setpoint1 += 2.5*TSAMP; //6.8*TSAMP; |
Hooglugt | 11:b517e73a98ab | 585 | break; |
Hooglugt | 11:b517e73a98ab | 586 | case 2: |
Hooglugt | 11:b517e73a98ab | 587 | setpoint1 += 0.4*TSAMP; //7.4*TSAMP; |
Hooglugt | 11:b517e73a98ab | 588 | break; |
Hooglugt | 11:b517e73a98ab | 589 | case 3: |
Hooglugt | 11:b517e73a98ab | 590 | setpoint1 += 0.4*TSAMP; //8.0*TSAMP; |
Hooglugt | 11:b517e73a98ab | 591 | break; |
Hooglugt | 11:b517e73a98ab | 592 | } |
Hooglugt | 11:b517e73a98ab | 593 | if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) { |
Hooglugt | 12:b09b7fe5550c | 594 | setpoint1 = 2.36; |
Hooglugt | 11:b517e73a98ab | 595 | state = 4; |
Hooglugt | 11:b517e73a98ab | 596 | } |
Hooglugt | 11:b517e73a98ab | 597 | break; |
Hooglugt | 8:75980dc35763 | 598 | } |
Hooglugt | 11:b517e73a98ab | 599 | case 4: { |
Hooglugt | 11:b517e73a98ab | 600 | setpoint2 -= 0.25*TSAMP; |
Hooglugt | 11:b517e73a98ab | 601 | if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm |
Hooglugt | 11:b517e73a98ab | 602 | state = 5; |
Hooglugt | 11:b517e73a98ab | 603 | } |
Hooglugt | 11:b517e73a98ab | 604 | break; |
Hooglugt | 9:0bc7f83b761e | 605 | } |
Hooglugt | 11:b517e73a98ab | 606 | case 5: { |
Hooglugt | 11:b517e73a98ab | 607 | setpoint1 -= 0.5*TSAMP; |
Hooglugt | 11:b517e73a98ab | 608 | if(setpoint1 < 0) { |
Hooglugt | 11:b517e73a98ab | 609 | state = 6; |
Hooglugt | 11:b517e73a98ab | 610 | } |
Hooglugt | 11:b517e73a98ab | 611 | break; |
Hooglugt | 9:0bc7f83b761e | 612 | } |
Hooglugt | 11:b517e73a98ab | 613 | case 6: { |
Hooglugt | 11:b517e73a98ab | 614 | setpoint1 = 0; |
Hooglugt | 11:b517e73a98ab | 615 | count++; |
Hooglugt | 11:b517e73a98ab | 616 | if(count>3000) { |
Hooglugt | 11:b517e73a98ab | 617 | count = 0; |
Hooglugt | 11:b517e73a98ab | 618 | state = 1; |
Hooglugt | 11:b517e73a98ab | 619 | goto directionchoice; |
Hooglugt | 11:b517e73a98ab | 620 | } |
Hooglugt | 11:b517e73a98ab | 621 | break; |
Hooglugt | 11:b517e73a98ab | 622 | } |
Hooglugt | 11:b517e73a98ab | 623 | } |
Hooglugt | 8:75980dc35763 | 624 | |
Hooglugt | 11:b517e73a98ab | 625 | //motor regeling |
Hooglugt | 8:75980dc35763 | 626 | |
Hooglugt | 11:b517e73a98ab | 627 | //regelaar motor1, bepaalt positie |
Hooglugt | 12:b09b7fe5550c | 628 | controlerror1 = setpoint1 - (motor1.getPosition()*omrekenfactor1); |
Hooglugt | 12:b09b7fe5550c | 629 | integral1 = integral1 + (controlerror1*TSAMP); |
Hooglugt | 11:b517e73a98ab | 630 | derivative1 = (controlerror1 - previouserror1)/TSAMP; |
Hooglugt | 11:b517e73a98ab | 631 | pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1; |
Hooglugt | 11:b517e73a98ab | 632 | previouserror1 = controlerror1; |
Hooglugt | 11:b517e73a98ab | 633 | |
Hooglugt | 11:b517e73a98ab | 634 | keep_in_range(&pwm1, -1,1); |
Hooglugt | 11:b517e73a98ab | 635 | pwm_motor1.write(fabs(pwm1)); |
Hooglugt | 11:b517e73a98ab | 636 | if(pwm1 > 0) { |
Hooglugt | 11:b517e73a98ab | 637 | motor1dir = 1; |
Hooglugt | 11:b517e73a98ab | 638 | } else { |
Hooglugt | 11:b517e73a98ab | 639 | motor1dir = 0; |
Hooglugt | 11:b517e73a98ab | 640 | } |
Hooglugt | 8:75980dc35763 | 641 | |
Hooglugt | 0:99cbc87af37c | 642 | |
Hooglugt | 11:b517e73a98ab | 643 | //regelaar motor2, bepaalt positie |
Hooglugt | 12:b09b7fe5550c | 644 | controlerror = setpoint2 - (motor2.getPosition()*omrekenfactor2); |
Hooglugt | 12:b09b7fe5550c | 645 | integral = integral + (controlerror*TSAMP); |
Hooglugt | 11:b517e73a98ab | 646 | derivative = (controlerror - previouserror)/TSAMP; |
Hooglugt | 11:b517e73a98ab | 647 | pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative; |
Hooglugt | 11:b517e73a98ab | 648 | previouserror = controlerror; |
Hooglugt | 11:b517e73a98ab | 649 | |
Hooglugt | 11:b517e73a98ab | 650 | keep_in_range(&pwm, -1,1); |
Hooglugt | 11:b517e73a98ab | 651 | pwm_motor2.write(fabs(pwm)); |
Hooglugt | 11:b517e73a98ab | 652 | if(pwm > 0) { |
Hooglugt | 11:b517e73a98ab | 653 | motor2dir = 1; |
Hooglugt | 11:b517e73a98ab | 654 | } else { |
Hooglugt | 11:b517e73a98ab | 655 | motor2dir = 0; |
Hooglugt | 11:b517e73a98ab | 656 | } |
Hooglugt | 11:b517e73a98ab | 657 | |
Hooglugt | 11:b517e73a98ab | 658 | /* |
Hooglugt | 11:b517e73a98ab | 659 | //controleert of batje positie heeft bepaald |
Hooglugt | 11:b517e73a98ab | 660 | if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol |
Hooglugt | 11:b517e73a98ab | 661 | if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) { |
Hooglugt | 11:b517e73a98ab | 662 | batjeset = 0; |
Hooglugt | 11:b517e73a98ab | 663 | } else { |
Hooglugt | 11:b517e73a98ab | 664 | batjeset++; |
Hooglugt | 11:b517e73a98ab | 665 | } |
Hooglugt | 11:b517e73a98ab | 666 | } else { |
Hooglugt | 11:b517e73a98ab | 667 | pwm_motor2.write(0); |
Hooglugt | 11:b517e73a98ab | 668 | batjeset = integral = derivative = previouserror = 0; |
Hooglugt | 11:b517e73a98ab | 669 | wait(1); |
Hooglugt | 11:b517e73a98ab | 670 | //goto motor1control; |
Hooglugt | 11:b517e73a98ab | 671 | } |
Hooglugt | 11:b517e73a98ab | 672 | */ |
Hooglugt | 11:b517e73a98ab | 673 | } |
Hooglugt | 11:b517e73a98ab | 674 | /* |
Hooglugt | 11:b517e73a98ab | 675 | motor1control: |
Hooglugt | 11:b517e73a98ab | 676 | while(1) { // loop voor het slaan mbv motor1 (batje snelheid) |
Hooglugt | 11:b517e73a98ab | 677 | while(!looptimerflag); |
Hooglugt | 11:b517e73a98ab | 678 | looptimerflag = false; //clear flag |
Hooglugt | 11:b517e73a98ab | 679 | |
Hooglugt | 11:b517e73a98ab | 680 | if (balhit == 0) { //regelaar motor1, bepaalt snelheid |
Hooglugt | 11:b517e73a98ab | 681 | controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1; |
Hooglugt | 11:b517e73a98ab | 682 | integral = integral + controlerror*TSAMP; |
Hooglugt | 11:b517e73a98ab | 683 | derivative = (controlerror - previouserror)/TSAMP; |
Hooglugt | 11:b517e73a98ab | 684 | pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative; |
Hooglugt | 11:b517e73a98ab | 685 | previouserror = controlerror; |
Hooglugt | 11:b517e73a98ab | 686 | } else { //regelaar motor1, bepaalt positie |
Hooglugt | 11:b517e73a98ab | 687 | balhit = integral = derivative = previouserror = 0; |
Hooglugt | 11:b517e73a98ab | 688 | goto resetpositionmotor1; |
Hooglugt | 11:b517e73a98ab | 689 | } |
Hooglugt | 11:b517e73a98ab | 690 | |
Hooglugt | 11:b517e73a98ab | 691 | keep_in_range(&pwm, -1,1); |
Hooglugt | 11:b517e73a98ab | 692 | pwm_motor1.write(abs(pwm)); |
Hooglugt | 11:b517e73a98ab | 693 | |
Hooglugt | 11:b517e73a98ab | 694 | if(pwm > 0) { |
Hooglugt | 11:b517e73a98ab | 695 | motor1dir = 1; |
Hooglugt | 11:b517e73a98ab | 696 | } else { |
Hooglugt | 11:b517e73a98ab | 697 | motor1dir = 0; |
Hooglugt | 11:b517e73a98ab | 698 | } |
Hooglugt | 0:99cbc87af37c | 699 | |
Hooglugt | 11:b517e73a98ab | 700 | //controleert of batje balletje heeft bereikt |
Hooglugt | 11:b517e73a98ab | 701 | //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle |
Hooglugt | 11:b517e73a98ab | 702 | if (motor1.getPosition()*omrekenfactor1 > 1.60) { |
Hooglugt | 11:b517e73a98ab | 703 | balhit = 1; |
Hooglugt | 11:b517e73a98ab | 704 | } |
Hooglugt | 11:b517e73a98ab | 705 | } |
Hooglugt | 11:b517e73a98ab | 706 | // FORMAT_CODE_END |
Hooglugt | 11:b517e73a98ab | 707 | |
Hooglugt | 11:b517e73a98ab | 708 | resetpositionmotor1: |
Hooglugt | 11:b517e73a98ab | 709 | while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst |
Hooglugt | 11:b517e73a98ab | 710 | while(!looptimerflag); |
Hooglugt | 11:b517e73a98ab | 711 | looptimerflag = false; //clear flag |
Hooglugt | 11:b517e73a98ab | 712 | |
Hooglugt | 11:b517e73a98ab | 713 | //regelaar motor1, bepaalt positie |
Hooglugt | 11:b517e73a98ab | 714 | controlerror = -1*motor1.getPosition()*omrekenfactor1; |
Hooglugt | 11:b517e73a98ab | 715 | integral = integral + controlerror*TSAMP; |
Hooglugt | 11:b517e73a98ab | 716 | derivative = (controlerror - previouserror)/TSAMP; |
Hooglugt | 11:b517e73a98ab | 717 | pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative; |
Hooglugt | 11:b517e73a98ab | 718 | previouserror = controlerror; |
Hooglugt | 11:b517e73a98ab | 719 | |
Hooglugt | 11:b517e73a98ab | 720 | keep_in_range(&pwm, -1,1); |
Hooglugt | 11:b517e73a98ab | 721 | if(pwm > 0) { |
Hooglugt | 11:b517e73a98ab | 722 | motor1dir = 1; |
Hooglugt | 11:b517e73a98ab | 723 | } else { |
Hooglugt | 11:b517e73a98ab | 724 | motor1dir = 0; //1 = rechtsom, 0 = linksom |
Hooglugt | 11:b517e73a98ab | 725 | } |
Hooglugt | 11:b517e73a98ab | 726 | |
Hooglugt | 11:b517e73a98ab | 727 | pwm_motor1.write(abs(pwm)); |
Hooglugt | 0:99cbc87af37c | 728 | |
Hooglugt | 11:b517e73a98ab | 729 | //controleert of arm terug in positie is |
Hooglugt | 11:b517e73a98ab | 730 | if(batjeset < 200) { |
Hooglugt | 11:b517e73a98ab | 731 | if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) { |
Hooglugt | 11:b517e73a98ab | 732 | batjeset = 0; |
Hooglugt | 11:b517e73a98ab | 733 | } else { |
Hooglugt | 11:b517e73a98ab | 734 | batjeset++; |
Hooglugt | 11:b517e73a98ab | 735 | } |
Hooglugt | 11:b517e73a98ab | 736 | } else { |
Hooglugt | 11:b517e73a98ab | 737 | pwm_motor1.write(0); |
Hooglugt | 11:b517e73a98ab | 738 | batjeset = integral = derivative = previouserror = 0; |
Hooglugt | 11:b517e73a98ab | 739 | wait(1); |
Hooglugt | 11:b517e73a98ab | 740 | goto resetpositionmotor2; |
Hooglugt | 11:b517e73a98ab | 741 | } |
Hooglugt | 11:b517e73a98ab | 742 | } |
Hooglugt | 11:b517e73a98ab | 743 | |
Hooglugt | 11:b517e73a98ab | 744 | resetpositionmotor2: |
Hooglugt | 11:b517e73a98ab | 745 | while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) |
Hooglugt | 11:b517e73a98ab | 746 | while(!looptimerflag); |
Hooglugt | 11:b517e73a98ab | 747 | looptimerflag = false; //clear flag |
Hooglugt | 11:b517e73a98ab | 748 | |
Hooglugt | 11:b517e73a98ab | 749 | //regelaar motor2, bepaalt positie |
Hooglugt | 11:b517e73a98ab | 750 | controlerror = -1*motor2.getPosition()*omrekenfactor2; |
Hooglugt | 11:b517e73a98ab | 751 | integral = integral + controlerror*TSAMP; |
Hooglugt | 11:b517e73a98ab | 752 | derivative = (controlerror - previouserror)/TSAMP; |
Hooglugt | 11:b517e73a98ab | 753 | pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative; |
Hooglugt | 11:b517e73a98ab | 754 | previouserror = controlerror; |
Hooglugt | 11:b517e73a98ab | 755 | |
Hooglugt | 11:b517e73a98ab | 756 | keep_in_range(&pwm, -1,1); |
Hooglugt | 11:b517e73a98ab | 757 | |
Hooglugt | 11:b517e73a98ab | 758 | if(pwm > 0) { |
Hooglugt | 11:b517e73a98ab | 759 | motor2dir = 1; |
Hooglugt | 11:b517e73a98ab | 760 | } else { |
Hooglugt | 11:b517e73a98ab | 761 | motor2dir = 0; |
Hooglugt | 11:b517e73a98ab | 762 | } |
Hooglugt | 11:b517e73a98ab | 763 | |
Hooglugt | 11:b517e73a98ab | 764 | pwm_motor2.write(abs(pwm)); |
Hooglugt | 11:b517e73a98ab | 765 | |
Hooglugt | 8:75980dc35763 | 766 | //controleert of batje positie heeft bepaald |
Hooglugt | 8:75980dc35763 | 767 | if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol |
Hooglugt | 11:b517e73a98ab | 768 | if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) { |
Hooglugt | 8:75980dc35763 | 769 | batjeset = 0; |
Hooglugt | 8:75980dc35763 | 770 | } else { |
Hooglugt | 8:75980dc35763 | 771 | batjeset++; |
Hooglugt | 8:75980dc35763 | 772 | } |
Hooglugt | 8:75980dc35763 | 773 | } else { |
Hooglugt | 8:75980dc35763 | 774 | pwm_motor2.write(0); |
Hooglugt | 8:75980dc35763 | 775 | batjeset = integral = derivative = previouserror = 0; |
Hooglugt | 8:75980dc35763 | 776 | wait(1); |
Hooglugt | 11:b517e73a98ab | 777 | direction = force = 0; |
Hooglugt | 11:b517e73a98ab | 778 | goto motor1cal; |
Hooglugt | 8:75980dc35763 | 779 | } |
Hooglugt | 11:b517e73a98ab | 780 | }*/ |
Hooglugt | 11:b517e73a98ab | 781 | } // end main |