Aukie Hooglugt / Mbed 2 deprecated PROJECT_FINAL

richting is nog niet ingesteld, kan aan setpoint liggen

Dependencies:   Encoder HIDScope MODSERIAL- mbed-dsp mbed

PROJECT_main.cpp@3:2b2710b8b02e, 2014-10-31 (annotated)

Committer:
Hooglugt
Date:
Fri Oct 31 19:38:07 2014 +0000
Revision:
3:2b2710b8b02e
Parent:
2:10dd6a88dfd7
Child:
4:697d5a806cc4
dir en for loops zijn nog niet functioneel, tickers ontbreken nog

Who changed what in which revision?

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