Janine Klein Rot / Mbed 2 deprecated PIPO-2

2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range

Dependencies:   Encoder MODSERIAL TextLCD mbed mbed-dsp

main.cpp@32:80fc2de5b511, 2014-11-05 (annotated)

Committer:
JKleinRot
Date:
Wed Nov 05 18:29:00 2014 +0000
Revision:
32:80fc2de5b511
Parent:
31:36fe36657f8d 
2014-11-05 Laatste versie PIPO 2 groep 4. Kalibratie positie en EMG compleet. Keuze maken in doel werkt. Snelheidsregelaar werkt niet, PWM gestuurd. Niet alle cases voor doelen kunnen controleren, werken niet allemaal goed. Case BBBB werkt wel.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JKleinRot 0:859c89785d3f 1 #include "mbed.h" //Mbed bibliotheek inladen, standaard functies
JKleinRot 0:859c89785d3f 2 #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC
JKleinRot 0:859c89785d3f 3 #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder
JKleinRot 0:859c89785d3f 4 #include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm
JKleinRot 15:3ebd0e666a9c 5 #include "arm_math.h" //Filter bibliotheek inladen, makkelijk de filters maken, minder grote lappen tekst
JKleinRot 0:859c89785d3f 6
JKleinRot 0:859c89785d3f 7 //Constanten definiëren en waarde geven
JKleinRot 11:e9b906b5f572 8 #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
JKleinRot 25:71e52c56be13 9 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
JKleinRot 27:5ac1fc1005d7 10 #define KP_arm1 0.025 //Factor proprotionele regelaar arm 1
JKleinRot 25:71e52c56be13 11 #define KI_arm1 0 //Factor integraal regelaar arm 1
JKleinRot 25:71e52c56be13 12 #define KD_arm1 0 //Factor afgeleide regelaar arm 1
JKleinRot 23:5267c928ae2b 13 #define KP_arm2 0.01 //Factor proprotionele regelaar arm 2
JKleinRot 21:f4e9f6c28de1 14 #define KI_arm2 0 //Factor integraal regelaar arm 2
JKleinRot 29:f95f0cc84349 15 #define KD_arm2 0.0008 //Factor afgeleide regelaar arm 2
JKleinRot 0:859c89785d3f 16
JKleinRot 16:c34c5d9dfe1a 17 //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
JKleinRot 16:c34c5d9dfe1a 18 //High Pass filter
JKleinRot 15:3ebd0e666a9c 19 #define A1 1
JKleinRot 18:d3a7f8b4cb22 20 #define A2 -1.561018075800718
JKleinRot 18:d3a7f8b4cb22 21 #define A3 0.641351538057563
JKleinRot 18:d3a7f8b4cb22 22 #define B1 0.800592403464570
JKleinRot 18:d3a7f8b4cb22 23 #define B2 -1.601184806929141
JKleinRot 18:d3a7f8b4cb22 24 #define B3 0.800592403464570
JKleinRot 13:54ee98850a15 25
JKleinRot 16:c34c5d9dfe1a 26 //Notch filter
JKleinRot 13:54ee98850a15 27 #define C1 1
JKleinRot 18:d3a7f8b4cb22 28 #define C2 -1.18733334554802E-16
JKleinRot 18:d3a7f8b4cb22 29 #define C3 0.939062505817492
JKleinRot 18:d3a7f8b4cb22 30 #define D1 0.969531252908746
JKleinRot 18:d3a7f8b4cb22 31 #define D2 -1.18733334554802E-16
JKleinRot 18:d3a7f8b4cb22 32 #define D3 0.969531252908746
JKleinRot 13:54ee98850a15 33
JKleinRot 16:c34c5d9dfe1a 34 //Low pass filter
JKleinRot 13:54ee98850a15 35 #define E1 1
JKleinRot 18:d3a7f8b4cb22 36 #define E2 -1.77863177782459
JKleinRot 18:d3a7f8b4cb22 37 #define E3 0.800802646665708
JKleinRot 18:d3a7f8b4cb22 38 #define F1 0.00554271721028068
JKleinRot 18:d3a7f8b4cb22 39 #define F2 0.0110854344205614
JKleinRot 18:d3a7f8b4cb22 40 #define F3 0.00554271721028068
JKleinRot 18:d3a7f8b4cb22 41
JKleinRot 15:3ebd0e666a9c 42
JKleinRot 0:859c89785d3f 43 //Pinverdeling en naamgeving variabelen
JKleinRot 17:c694a0e63a89 44 TextLCD lcd(PTD1, PTA12, PTB2, PTB3, PTC2, PTD3, TextLCD::LCD16x2); //LCD scherm
JKleinRot 16:c34c5d9dfe1a 45 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 46
JKleinRot 16:c34c5d9dfe1a 47 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 16:c34c5d9dfe1a 48 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
JKleinRot 23:5267c928ae2b 49 Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1, (geel, wit)
JKleinRot 16:c34c5d9dfe1a 50 PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
JKleinRot 16:c34c5d9dfe1a 51 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
JKleinRot 16:c34c5d9dfe1a 52 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
JKleinRot 16:c34c5d9dfe1a 53 AnalogIn EMG_bi(PTB1); //Uitlezen EMG signaal biceps
JKleinRot 17:c694a0e63a89 54 AnalogIn EMG_tri(PTB0); //Uitlezen EMG signaal triceps
JKleinRot 16:c34c5d9dfe1a 55 //Blauw op 3,3 V en groen op GND, voeding beide encoders
JKleinRot 1:e5e1eb9d0025 56
JKleinRot 25:71e52c56be13 57 Ticker ticker_regelaar; //Ticker voor flag veranderen referentie
JKleinRot 25:71e52c56be13 58 Ticker ticker_EMG; //Ticker voor flag EMG meten
JKleinRot 25:71e52c56be13 59 Ticker ticker_motor_arm1_pid; //Ticker voor PID regelaar motor arm 1
JKleinRot 25:71e52c56be13 60 Ticker ticker_motor_arm2_pid; //Ticker voor PID regelaar motor arm 2
JKleinRot 25:71e52c56be13 61
JKleinRot 16:c34c5d9dfe1a 62 Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps
JKleinRot 16:c34c5d9dfe1a 63 Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps
JKleinRot 25:71e52c56be13 64 Timer EMG; //Timer voor aantal seconden EMG meten
JKleinRot 1:e5e1eb9d0025 65
JKleinRot 9:454e7da8ab65 66 //States definiëren
JKleinRot 25:71e52c56be13 67 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT, WACHT, THUISPOSITIE_MIDDEN, THUISPOSITIE_RECHTS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
JKleinRot 9:454e7da8ab65 68 uint8_t state = RUST; //State is rust aan het begin
JKleinRot 9:454e7da8ab65 69
JKleinRot 6:3b6fad529520 70 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 11:e9b906b5f572 71 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 11:e9b906b5f572 72 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 13:54ee98850a15 73 float error_arm1_kalibratie; //Error in pulsen arm 1
JKleinRot 13:54ee98850a15 74 float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
JKleinRot 13:54ee98850a15 75 float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
JKleinRot 13:54ee98850a15 76 float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
JKleinRot 13:54ee98850a15 77 float error_arm2_kalibratie; //Error in pulsen arm 2
JKleinRot 13:54ee98850a15 78 float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
JKleinRot 13:54ee98850a15 79 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
JKleinRot 13:54ee98850a15 80 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
JKleinRot 13:54ee98850a15 81 float xb; //Gemeten EMG waarde biceps
JKleinRot 16:c34c5d9dfe1a 82 float xt; //Gemeten EMG waarde triceps
JKleinRot 19:38c9d177b6ee 83 float referentie_arm1 = 0;
JKleinRot 19:38c9d177b6ee 84 float referentie_arm2 = 0;
JKleinRot 23:5267c928ae2b 85 float t = 0;
JKleinRot 23:5267c928ae2b 86
JKleinRot 25:71e52c56be13 87 //Gedefinieerde filters met constanten en gefilterde waarden
JKleinRot 16:c34c5d9dfe1a 88 arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1
JKleinRot 17:c694a0e63a89 89 float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 90 float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x)
JKleinRot 13:54ee98850a15 91
JKleinRot 16:c34c5d9dfe1a 92 arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1
JKleinRot 17:c694a0e63a89 93 float notch_biceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 94 float notch_biceps_states[8]; //Aantal states van het filter
JKleinRot 15:3ebd0e666a9c 95
JKleinRot 16:c34c5d9dfe1a 96 arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1
JKleinRot 17:c694a0e63a89 97 float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 98 float lowpass_biceps_states[8]; //Aantal states van het filter
JKleinRot 15:3ebd0e666a9c 99
JKleinRot 16:c34c5d9dfe1a 100 float xbf; //Gefilterde EMG waarde biceps
JKleinRot 16:c34c5d9dfe1a 101 float xbfmax = 0; //Maximale gefilterde EMG waarde biceps
JKleinRot 13:54ee98850a15 102
JKleinRot 16:c34c5d9dfe1a 103 arm_biquad_casd_df1_inst_f32 highpass_triceps; //Highpass_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 17:c694a0e63a89 104 float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 105 float highpass_triceps_states[8]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 106
JKleinRot 16:c34c5d9dfe1a 107 arm_biquad_casd_df1_inst_f32 notch_triceps; //Notch_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 17:c694a0e63a89 108 float notch_triceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 109 float notch_triceps_states[8]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 110
JKleinRot 16:c34c5d9dfe1a 111 arm_biquad_casd_df1_inst_f32 lowpass_triceps; //Lowpass_biceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 17:c694a0e63a89 112 float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 113 float lowpass_triceps_states[8]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 114
JKleinRot 16:c34c5d9dfe1a 115 float xtf; //Gefilterde EMG waarde triceps
JKleinRot 16:c34c5d9dfe1a 116 float xtfmax = 0; //Maximale gefilterde EMG waarde triceps
JKleinRot 15:3ebd0e666a9c 117
JKleinRot 16:c34c5d9dfe1a 118 float xbt; //Thresholdwaarde EMG biceps
JKleinRot 16:c34c5d9dfe1a 119 float xtt; //Thresholdwaarde EMG triceps
JKleinRot 7:dd3cba61b34b 120
JKleinRot 6:3b6fad529520 121 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 122 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 123 {
JKleinRot 10:52b22bff409a 124 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 125 }
JKleinRot 1:e5e1eb9d0025 126
JKleinRot 6:3b6fad529520 127 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 128 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 129 {
JKleinRot 10:52b22bff409a 130 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 131 }
JKleinRot 4:69540b9c0646 132
JKleinRot 10:52b22bff409a 133 void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
JKleinRot 10:52b22bff409a 134 {
JKleinRot 19:38c9d177b6ee 135 if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 136 *in = min;
JKleinRot 2:0cb899f2800a 137 }
JKleinRot 19:38c9d177b6ee 138 if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 139 *in = max;
JKleinRot 19:38c9d177b6ee 140 } else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 141 *in = *in;
JKleinRot 2:0cb899f2800a 142 }
JKleinRot 2:0cb899f2800a 143 }
JKleinRot 1:e5e1eb9d0025 144
JKleinRot 25:71e52c56be13 145 void motor_arm1_pid() //PID regelaar van motor arm 1
JKleinRot 10:52b22bff409a 146 {
JKleinRot 19:38c9d177b6ee 147 error_arm1_kalibratie = (referentie_arm1 - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 20:1cb0cf0d49ac 148 integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 20:1cb0cf0d49ac 149 afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 20:1cb0cf0d49ac 150 pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
JKleinRot 25:71e52c56be13 151 vorige_error_arm1 = error_arm1_kalibratie; //Vorige error is de volgende keer de huidige error van deze keer
JKleinRot 25:71e52c56be13 152 keep_in_range(&pwm_to_motor_arm1, -1, 1); //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben
JKleinRot 19:38c9d177b6ee 153
JKleinRot 21:f4e9f6c28de1 154 if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 23:5267c928ae2b 155 dir_motor_arm1.write(0);
JKleinRot 21:f4e9f6c28de1 156 } else { //Anders richting nul
JKleinRot 23:5267c928ae2b 157 dir_motor_arm1.write(1);
JKleinRot 7:dd3cba61b34b 158 }
JKleinRot 18:d3a7f8b4cb22 159
JKleinRot 11:e9b906b5f572 160 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 20:1cb0cf0d49ac 161 }
JKleinRot 10:52b22bff409a 162
JKleinRot 25:71e52c56be13 163 void motor_arm2_pid() //PID regelaar van motor arm 2
JKleinRot 20:1cb0cf0d49ac 164 {
JKleinRot 20:1cb0cf0d49ac 165 error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 20:1cb0cf0d49ac 166 integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 20:1cb0cf0d49ac 167 afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 20:1cb0cf0d49ac 168 pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
JKleinRot 25:71e52c56be13 169 vorige_error_arm2 = error_arm2_kalibratie; //Vorige error is de volgende keer de huidige error van deze keer
JKleinRot 25:71e52c56be13 170 keep_in_range(&pwm_to_motor_arm2, -1, 1); //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben
JKleinRot 19:38c9d177b6ee 171
JKleinRot 20:1cb0cf0d49ac 172 if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 20:1cb0cf0d49ac 173 dir_motor_arm2.write(1);
JKleinRot 20:1cb0cf0d49ac 174 } else { //Anders richting nul
JKleinRot 20:1cb0cf0d49ac 175 dir_motor_arm2.write(0);
JKleinRot 20:1cb0cf0d49ac 176 }
JKleinRot 20:1cb0cf0d49ac 177 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 178 }
JKleinRot 7:dd3cba61b34b 179
JKleinRot 16:c34c5d9dfe1a 180 void filter_biceps() //Filtert het EMG signaal van de biceps
JKleinRot 13:54ee98850a15 181 {
JKleinRot 16:c34c5d9dfe1a 182 arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1); //Highpass filter voor het EMG signaal van de biceps, filtert de xb en schrijft het over in de xbf, 1 EMG waarde filteren per keer
JKleinRot 19:38c9d177b6ee 183
JKleinRot 16:c34c5d9dfe1a 184 arm_biquad_cascade_df1_f32(&notch_biceps, &xbf, &xbf, 1); //Notch filter voor het EMG signaal van de biceps, filtert de xbf uit de highpass en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer
JKleinRot 16:c34c5d9dfe1a 185
JKleinRot 16:c34c5d9dfe1a 186 xbf = fabs(xbf); //Rectifier, schrijft de absolute waarde van de xbf uit de notch opnieuw in de xbf, fabs omdat het een float is
JKleinRot 15:3ebd0e666a9c 187
JKleinRot 16:c34c5d9dfe1a 188 arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1); //Lowpass filter voor het EMG signaal van de biceps, filtert de xbf uit de rectifier en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer
JKleinRot 19:38c9d177b6ee 189
JKleinRot 16:c34c5d9dfe1a 190 pc.printf("xbf is %f\n\r", xbf); //De gefilterde EMG waarde van de biceps naar pc sturen
JKleinRot 16:c34c5d9dfe1a 191 }
JKleinRot 15:3ebd0e666a9c 192
JKleinRot 16:c34c5d9dfe1a 193 void filter_triceps() //Filtert het EMG signaal van de triceps
JKleinRot 16:c34c5d9dfe1a 194 {
JKleinRot 16:c34c5d9dfe1a 195 arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1); //Highpass filter voor het EMG signaal van de triceps, filtert de xt en schrijft het over in de xtf, 1 EMG waarde filteren per keer
JKleinRot 16:c34c5d9dfe1a 196
JKleinRot 16:c34c5d9dfe1a 197 arm_biquad_cascade_df1_f32(&notch_triceps, &xtf, &xtf, 1); //Notch filter voor het EMG signaal van de triceps, filtert de xtf uit de highpass en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer
JKleinRot 15:3ebd0e666a9c 198
JKleinRot 16:c34c5d9dfe1a 199 xtf = fabs(xtf); //Rectifier, schrijft de absolute waarde van de xtf uit de notch opnieuw in de xtf, fabs omdat het een float is
JKleinRot 16:c34c5d9dfe1a 200
JKleinRot 16:c34c5d9dfe1a 201 arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1); //Lowpass filter voor het EMG signaal van de triceps, filtert de xtf uit de rectifier en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer
JKleinRot 16:c34c5d9dfe1a 202
JKleinRot 16:c34c5d9dfe1a 203 pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen
JKleinRot 15:3ebd0e666a9c 204
JKleinRot 13:54ee98850a15 205 }
JKleinRot 13:54ee98850a15 206
JKleinRot 25:71e52c56be13 207 void EMG_meten() //Meet de EMG van de biceps en de triceps
JKleinRot 22:838a17065bc7 208 {
JKleinRot 22:838a17065bc7 209 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 22:838a17065bc7 210 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 22:838a17065bc7 211 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 22:838a17065bc7 212 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 22:838a17065bc7 213 }
JKleinRot 22:838a17065bc7 214
JKleinRot 22:838a17065bc7 215
JKleinRot 16:c34c5d9dfe1a 216 int main() //Main script
JKleinRot 15:3ebd0e666a9c 217 {
JKleinRot 25:71e52c56be13 218 ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR); //Iedere SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor
JKleinRot 25:71e52c56be13 219 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); //Iederen SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor
JKleinRot 21:f4e9f6c28de1 220
JKleinRot 19:38c9d177b6ee 221 while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
JKleinRot 10:52b22bff409a 222 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 10:52b22bff409a 223
JKleinRot 19:38c9d177b6ee 224 switch(state) { //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
JKleinRot 10:52b22bff409a 225
JKleinRot 19:38c9d177b6ee 226 case RUST: { //Aanzetten
JKleinRot 19:38c9d177b6ee 227 pc.printf("RUST\n\r"); //Begin RUST naar pc sturen
JKleinRot 16:c34c5d9dfe1a 228 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 16:c34c5d9dfe1a 229 lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 230 lcd.locate(0,1); //Zet de tekst op de tweede regel
JKleinRot 16:c34c5d9dfe1a 231 lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 232 wait(2); //Twee seconden wachten
JKleinRot 16:c34c5d9dfe1a 233 lcd.cls(); //Maak LCD scherm leeg
JKleinRot 14:e1816efa712d 234 pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten
JKleinRot 25:71e52c56be13 235 state = KALIBRATIE_ARM1; //Door naar de volgende state
JKleinRot 16:c34c5d9dfe1a 236 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 237 }
JKleinRot 10:52b22bff409a 238
JKleinRot 19:38c9d177b6ee 239 case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
JKleinRot 16:c34c5d9dfe1a 240 pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen
JKleinRot 16:c34c5d9dfe1a 241 wait(1); //Een seconde wachten
JKleinRot 15:3ebd0e666a9c 242
JKleinRot 10:52b22bff409a 243 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 244
JKleinRot 19:38c9d177b6ee 245 while(state == KALIBRATIE_ARM1) {
JKleinRot 16:c34c5d9dfe1a 246 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 10:52b22bff409a 247 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 248
JKleinRot 32:80fc2de5b511 249 referentie_arm1 = referentie_arm1 + 203.0/200.0; //Referentie loopt in één seconde op tot het gewenste aantal pulsen
JKleinRot 21:f4e9f6c28de1 250
JKleinRot 25:71e52c56be13 251 pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 25:71e52c56be13 252 pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
JKleinRot 25:71e52c56be13 253 pc.printf("referentie1 %f\n\r", referentie_arm1); //Huidige referentie naar pc sturen
JKleinRot 25:71e52c56be13 254
JKleinRot 32:80fc2de5b511 255 if (puls_motor_arm1.getPosition() >= 203) { //Als het gewenste aantal pulsen behaald is
JKleinRot 32:80fc2de5b511 256 referentie_arm1 = 203; //Blijft de referentie op dat aantal pulsen staan
JKleinRot 25:71e52c56be13 257 state = KALIBRATIE_ARM2; //Door naar de volgende state
JKleinRot 20:1cb0cf0d49ac 258 }
JKleinRot 10:52b22bff409a 259 }
JKleinRot 10:52b22bff409a 260 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 261 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 262 }
JKleinRot 10:52b22bff409a 263
JKleinRot 19:38c9d177b6ee 264 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 19:38c9d177b6ee 265 pc.printf("KALIBRATIE_ARM2\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen
JKleinRot 16:c34c5d9dfe1a 266 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 267
JKleinRot 10:52b22bff409a 268 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 269
JKleinRot 19:38c9d177b6ee 270 while(state == KALIBRATIE_ARM2) {
JKleinRot 16:c34c5d9dfe1a 271 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 10:52b22bff409a 272 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 273
JKleinRot 25:71e52c56be13 274 referentie_arm2 = referentie_arm2 + 123.0/200.0; //Referentie loopt in één seconde op tot het gewenste aantal pulsen
JKleinRot 21:f4e9f6c28de1 275
JKleinRot 25:71e52c56be13 276 pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 25:71e52c56be13 277 pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen
JKleinRot 25:71e52c56be13 278 pc.printf("referentie2 %f\n\r", referentie_arm2); //Huidige referentie naar pc sturen
JKleinRot 25:71e52c56be13 279
JKleinRot 25:71e52c56be13 280 if(puls_motor_arm2.getPosition() >= 123) { //Als het gewenste aantal pulsen behaald is
JKleinRot 25:71e52c56be13 281 referentie_arm2 = 123; //Blijft de referentie op dat aantal pulsen staan
JKleinRot 25:71e52c56be13 282 state = EMG_KALIBRATIE_BICEPS; //Door naar de volgende state
JKleinRot 20:1cb0cf0d49ac 283 }
JKleinRot 10:52b22bff409a 284 }
JKleinRot 11:e9b906b5f572 285 wait(1); //Een seconde wachten
JKleinRot 16:c34c5d9dfe1a 286 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 287 }
JKleinRot 15:3ebd0e666a9c 288
JKleinRot 19:38c9d177b6ee 289 case EMG_KALIBRATIE_BICEPS: { //Kalibratie biceps voor bepalen threshold
JKleinRot 16:c34c5d9dfe1a 290 pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen
JKleinRot 19:38c9d177b6ee 291
JKleinRot 16:c34c5d9dfe1a 292 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 16:c34c5d9dfe1a 293 lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 294 lcd.locate(0,1); //Zet tekst op tweede regel
JKleinRot 16:c34c5d9dfe1a 295 lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 296 wait(1); //Een seconden wachten
JKleinRot 25:71e52c56be13 297 lcd.cls(); //LCD scherm leegmaken
JKleinRot 15:3ebd0e666a9c 298
JKleinRot 17:c694a0e63a89 299 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 2, highpass_biceps_const, highpass_biceps_states); //Highpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 17:c694a0e63a89 300 arm_biquad_cascade_df1_init_f32(&notch_biceps, 2, notch_biceps_const, notch_biceps_states); //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
JKleinRot 17:c694a0e63a89 301 arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 2, lowpass_biceps_const, lowpass_biceps_states); //Lowpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 15:3ebd0e666a9c 302
JKleinRot 16:c34c5d9dfe1a 303 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 16:c34c5d9dfe1a 304 biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
JKleinRot 15:3ebd0e666a9c 305
JKleinRot 19:38c9d177b6ee 306 while(biceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
JKleinRot 19:38c9d177b6ee 307
JKleinRot 15:3ebd0e666a9c 308 while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 15:3ebd0e666a9c 309 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 310
JKleinRot 15:3ebd0e666a9c 311 xb = EMG_bi.read(); //EMG meten van biceps
JKleinRot 15:3ebd0e666a9c 312
JKleinRot 17:c694a0e63a89 313 filter_biceps(); //Voer acties uit om EMG signaal van de biceps te filteren
JKleinRot 15:3ebd0e666a9c 314
JKleinRot 19:38c9d177b6ee 315 if(int(biceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen een seconde heeft gemeten
JKleinRot 17:c694a0e63a89 316 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 317 lcd.printf(" 5 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 318 pc.printf("4"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 319 }
JKleinRot 19:38c9d177b6ee 320 if(int(biceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 321 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 322 lcd.printf(" 4 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 323 pc.printf("3"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 324 }
JKleinRot 19:38c9d177b6ee 325 if(int(biceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
JKleinRot 17:c694a0e63a89 326 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 327 lcd.printf(" 3 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 328 pc.printf("2"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 329 }
JKleinRot 19:38c9d177b6ee 330 if(int(biceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
JKleinRot 17:c694a0e63a89 331 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 332 lcd.printf(" 2 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 333 pc.printf("1"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 334 }
JKleinRot 19:38c9d177b6ee 335 if(int(biceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
JKleinRot 17:c694a0e63a89 336 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 337 lcd.printf(" 1 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 338 pc.printf("1"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 339 }
JKleinRot 15:3ebd0e666a9c 340
JKleinRot 21:f4e9f6c28de1 341 if(xbf >= xbfmax) { //Als de gefilterde EMG waarde groter is dan xbfmax
JKleinRot 21:f4e9f6c28de1 342 xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax
JKleinRot 21:f4e9f6c28de1 343 }
JKleinRot 15:3ebd0e666a9c 344 }
JKleinRot 19:38c9d177b6ee 345
JKleinRot 27:5ac1fc1005d7 346 xbt = xbfmax * 0.80; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
JKleinRot 25:71e52c56be13 347 pc.printf("maximale biceps %f", xbfmax); //Maximale gefilterde EMG waarde naar pc sturen
JKleinRot 17:c694a0e63a89 348 pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen
JKleinRot 17:c694a0e63a89 349 state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps
JKleinRot 17:c694a0e63a89 350 break; //Stopt alle acties in deze case
JKleinRot 15:3ebd0e666a9c 351 }
JKleinRot 15:3ebd0e666a9c 352
JKleinRot 25:71e52c56be13 353 case EMG_KALIBRATIE_TRICEPS: { //Kalibratie triceps voor bepalen threshold
JKleinRot 17:c694a0e63a89 354 pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen
JKleinRot 15:3ebd0e666a9c 355
JKleinRot 17:c694a0e63a89 356 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 357 lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 358 lcd.locate(0,1); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 359 lcd.printf(" 2 X TRICEPS AAN"); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 360 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 361 lcd.cls(); //LCD scherm leegmaken
JKleinRot 15:3ebd0e666a9c 362
JKleinRot 19:38c9d177b6ee 363 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 2, highpass_triceps_const, highpass_triceps_states); //Highpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 17:c694a0e63a89 364 arm_biquad_cascade_df1_init_f32(&notch_triceps, 2, notch_triceps_const, notch_triceps_states); //Notchfilter triceps met bijbehorende filtercoëfficiënten en states definiëren
JKleinRot 17:c694a0e63a89 365 arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 2, lowpass_triceps_const, lowpass_triceps_states); //Lowpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 15:3ebd0e666a9c 366
JKleinRot 17:c694a0e63a89 367 triceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
JKleinRot 15:3ebd0e666a9c 368
JKleinRot 19:38c9d177b6ee 369 while(triceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
JKleinRot 15:3ebd0e666a9c 370 while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 15:3ebd0e666a9c 371 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 372
JKleinRot 17:c694a0e63a89 373 xt = EMG_tri.read(); //EMG meten van ticeps
JKleinRot 15:3ebd0e666a9c 374
JKleinRot 17:c694a0e63a89 375 filter_triceps(); //Voer acties uit om EMG signaal van de triceps te meten
JKleinRot 15:3ebd0e666a9c 376
JKleinRot 19:38c9d177b6ee 377 if(int(triceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 378 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 379 lcd.printf(" 5 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 380 pc.printf("4"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 381 }
JKleinRot 19:38c9d177b6ee 382 if(int(triceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 383 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 384 lcd.printf(" 4 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 385 pc.printf("3"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 386 }
JKleinRot 19:38c9d177b6ee 387 if(int(triceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
JKleinRot 17:c694a0e63a89 388 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 389 lcd.printf(" 3 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 390 pc.printf("2"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 391 }
JKleinRot 19:38c9d177b6ee 392 if(int(triceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
JKleinRot 17:c694a0e63a89 393 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 394 lcd.printf(" 2 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 395 pc.printf("1"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 396 }
JKleinRot 19:38c9d177b6ee 397 if(int(triceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
JKleinRot 17:c694a0e63a89 398 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 399 lcd.printf(" 1 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 400 pc.printf("1"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 401 }
JKleinRot 15:3ebd0e666a9c 402
JKleinRot 21:f4e9f6c28de1 403
JKleinRot 21:f4e9f6c28de1 404 if(xtf >= xtfmax) { //Als de gefilterde EMG waarde groter is dan xtfmax
JKleinRot 21:f4e9f6c28de1 405 xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax
JKleinRot 21:f4e9f6c28de1 406 }
JKleinRot 10:52b22bff409a 407 }
JKleinRot 19:38c9d177b6ee 408
JKleinRot 27:5ac1fc1005d7 409 xtt = xtfmax * 0.80; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten
JKleinRot 25:71e52c56be13 410 pc.printf("maximale triceps %f", xtfmax); //Maximale gefilterde EMG waarde naar pc sturen
JKleinRot 17:c694a0e63a89 411 pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen
JKleinRot 17:c694a0e63a89 412 state = START; //Gaat door naar het slaan, kalibratie nu afgerond
JKleinRot 17:c694a0e63a89 413 break; //Stopt alle acties in deze case
JKleinRot 10:52b22bff409a 414 }
JKleinRot 15:3ebd0e666a9c 415
JKleinRot 25:71e52c56be13 416 case START: { //Eerste keuze maken voor doel
JKleinRot 17:c694a0e63a89 417 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 418 lcd.printf(" START "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 419
JKleinRot 17:c694a0e63a89 420 pc.printf("START\n\r"); //Controle naar pc sturen
JKleinRot 19:38c9d177b6ee 421
JKleinRot 27:5ac1fc1005d7 422 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 27:5ac1fc1005d7 423
JKleinRot 27:5ac1fc1005d7 424 while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten
JKleinRot 27:5ac1fc1005d7 425 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 27:5ac1fc1005d7 426 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 27:5ac1fc1005d7 427
JKleinRot 27:5ac1fc1005d7 428 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 27:5ac1fc1005d7 429 }
JKleinRot 27:5ac1fc1005d7 430
JKleinRot 19:38c9d177b6ee 431 while(state == START) {
JKleinRot 17:c694a0e63a89 432 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 433 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 434
JKleinRot 17:c694a0e63a89 435 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 436 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 437 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 438 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 15:3ebd0e666a9c 439
JKleinRot 25:71e52c56be13 440 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 441 state = B; //Ga door naar state B
JKleinRot 15:3ebd0e666a9c 442 }
JKleinRot 25:71e52c56be13 443 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 444 state = T; //Ga door naar state T
JKleinRot 15:3ebd0e666a9c 445 }
JKleinRot 15:3ebd0e666a9c 446 }
JKleinRot 17:c694a0e63a89 447 break; //Stopt met de acties in deze case
JKleinRot 16:c34c5d9dfe1a 448 }
JKleinRot 19:38c9d177b6ee 449
JKleinRot 25:71e52c56be13 450 case B: { //Tweede keuze maken voor doel
JKleinRot 17:c694a0e63a89 451 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 452 lcd.printf(" B "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 453 pc.printf("B\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 454
JKleinRot 27:5ac1fc1005d7 455 EMG.reset();
JKleinRot 25:71e52c56be13 456 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 457
JKleinRot 25:71e52c56be13 458 while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten
JKleinRot 22:838a17065bc7 459 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 460 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 461
JKleinRot 25:71e52c56be13 462 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 463 }
JKleinRot 23:5267c928ae2b 464
JKleinRot 19:38c9d177b6ee 465 while(state == B) {
JKleinRot 19:38c9d177b6ee 466 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 19:38c9d177b6ee 467 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 468
JKleinRot 25:71e52c56be13 469 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 23:5267c928ae2b 470
JKleinRot 25:71e52c56be13 471 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 472 state = BB; //Ga door naar state BB
JKleinRot 19:38c9d177b6ee 473 }
JKleinRot 21:f4e9f6c28de1 474 if(xtf>= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 475 state = BT; //Ga door naar state BT
JKleinRot 19:38c9d177b6ee 476 }
JKleinRot 19:38c9d177b6ee 477 }
JKleinRot 19:38c9d177b6ee 478 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 479 }
JKleinRot 19:38c9d177b6ee 480
JKleinRot 25:71e52c56be13 481 case T: { //Tweede keuze maken voor doel
JKleinRot 19:38c9d177b6ee 482 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 483 lcd.printf(" T "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 484 pc.printf("T\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 485
JKleinRot 27:5ac1fc1005d7 486 EMG.reset();
JKleinRot 25:71e52c56be13 487 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 488
JKleinRot 26:438a498e5526 489 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 490 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 491 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 492
JKleinRot 25:71e52c56be13 493 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 494 }
JKleinRot 19:38c9d177b6ee 495
JKleinRot 19:38c9d177b6ee 496 while(state == T) {
JKleinRot 17:c694a0e63a89 497 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 498 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 499
JKleinRot 25:71e52c56be13 500 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 16:c34c5d9dfe1a 501
JKleinRot 25:71e52c56be13 502 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 503 state = TB; //Ga door naar state TB
JKleinRot 16:c34c5d9dfe1a 504 }
JKleinRot 25:71e52c56be13 505 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 506 state = TT; //Ga door naar state TT
JKleinRot 16:c34c5d9dfe1a 507 }
JKleinRot 16:c34c5d9dfe1a 508 }
JKleinRot 17:c694a0e63a89 509 break; //Stop met alle acties in deze case
JKleinRot 16:c34c5d9dfe1a 510 }
JKleinRot 16:c34c5d9dfe1a 511
JKleinRot 25:71e52c56be13 512 case BB: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 513 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 514 lcd.printf(" BB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 515 pc.printf("BB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 516
JKleinRot 25:71e52c56be13 517 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 518 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 519
JKleinRot 26:438a498e5526 520 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 521 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 522 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 523
JKleinRot 25:71e52c56be13 524 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 525 }
JKleinRot 16:c34c5d9dfe1a 526
JKleinRot 19:38c9d177b6ee 527 while(state == BB) {
JKleinRot 17:c694a0e63a89 528 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 529 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 16:c34c5d9dfe1a 530
JKleinRot 25:71e52c56be13 531 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 16:c34c5d9dfe1a 532
JKleinRot 25:71e52c56be13 533 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 534 state = BBB; //Ga door naar state BBB
JKleinRot 16:c34c5d9dfe1a 535 }
JKleinRot 25:71e52c56be13 536 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 537 state = BBT; //Ga door naar state BBT
JKleinRot 16:c34c5d9dfe1a 538 }
JKleinRot 16:c34c5d9dfe1a 539 }
JKleinRot 17:c694a0e63a89 540 break; //Stop met alle acties in deze case
JKleinRot 16:c34c5d9dfe1a 541 }
JKleinRot 19:38c9d177b6ee 542
JKleinRot 25:71e52c56be13 543 case BT: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 544 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 545 lcd.printf(" BT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 546 pc.printf("BT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 547
JKleinRot 25:71e52c56be13 548 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 549 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 550
JKleinRot 26:438a498e5526 551 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 552 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 553 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 554
JKleinRot 25:71e52c56be13 555 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 556 }
JKleinRot 15:3ebd0e666a9c 557
JKleinRot 19:38c9d177b6ee 558 while(state == BT) {
JKleinRot 17:c694a0e63a89 559 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 560 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 561
JKleinRot 22:838a17065bc7 562 EMG_meten();
JKleinRot 17:c694a0e63a89 563
JKleinRot 25:71e52c56be13 564 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 565 state = BTB; //Ga door naar state BTB
JKleinRot 17:c694a0e63a89 566 }
JKleinRot 25:71e52c56be13 567 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 568 state = BTT; //Ga door naar state BTT
JKleinRot 17:c694a0e63a89 569 }
JKleinRot 17:c694a0e63a89 570 }
JKleinRot 19:38c9d177b6ee 571 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 572 }
JKleinRot 19:38c9d177b6ee 573
JKleinRot 25:71e52c56be13 574 case TB: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 575 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 576 lcd.printf(" TB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 577 pc.printf("TB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 578
JKleinRot 25:71e52c56be13 579 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 580 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 581
JKleinRot 26:438a498e5526 582 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 583 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 584 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 585
JKleinRot 25:71e52c56be13 586 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 587 }
JKleinRot 17:c694a0e63a89 588
JKleinRot 19:38c9d177b6ee 589 while(state == TB) {
JKleinRot 17:c694a0e63a89 590 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 591 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 592
JKleinRot 25:71e52c56be13 593 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 594
JKleinRot 25:71e52c56be13 595 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 596 state = TBB; //Ga door naar state TBB
JKleinRot 17:c694a0e63a89 597 }
JKleinRot 25:71e52c56be13 598 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 599 state = TBT; //Ga door naar state TBT
JKleinRot 17:c694a0e63a89 600 }
JKleinRot 17:c694a0e63a89 601 }
JKleinRot 19:38c9d177b6ee 602 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 603 }
JKleinRot 17:c694a0e63a89 604
JKleinRot 25:71e52c56be13 605 case TT: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 606 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 607 lcd.printf(" TT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 608 pc.printf("TT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 609
JKleinRot 25:71e52c56be13 610 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 611 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 612
JKleinRot 26:438a498e5526 613 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 614 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 615 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 616
JKleinRot 25:71e52c56be13 617 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 618 }
JKleinRot 17:c694a0e63a89 619
JKleinRot 19:38c9d177b6ee 620 while(state == TT) {
JKleinRot 17:c694a0e63a89 621 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 622 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 623
JKleinRot 25:71e52c56be13 624 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 625
JKleinRot 25:71e52c56be13 626 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 627 state = TTB; //Ga door naar state TTB
JKleinRot 17:c694a0e63a89 628 }
JKleinRot 25:71e52c56be13 629 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 630 state = TTT; //Ga door naar state TTT
JKleinRot 17:c694a0e63a89 631 }
JKleinRot 17:c694a0e63a89 632 }
JKleinRot 19:38c9d177b6ee 633 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 634 }
JKleinRot 19:38c9d177b6ee 635
JKleinRot 25:71e52c56be13 636 case BBB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 637 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 638 lcd.printf(" BBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 639 pc.printf("BBB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 640
JKleinRot 25:71e52c56be13 641 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 642 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 643
JKleinRot 26:438a498e5526 644 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 645 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 646 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 647
JKleinRot 25:71e52c56be13 648 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 649 }
JKleinRot 17:c694a0e63a89 650
JKleinRot 19:38c9d177b6ee 651 while(state == BBB) {
JKleinRot 17:c694a0e63a89 652 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 653 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 654
JKleinRot 25:71e52c56be13 655 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 656
JKleinRot 25:71e52c56be13 657 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 658 state = BBBB; //Ga door naar state BBBB
JKleinRot 17:c694a0e63a89 659 }
JKleinRot 25:71e52c56be13 660 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 661 state = BBBT; //Ga door naar state BBBT
JKleinRot 17:c694a0e63a89 662 }
JKleinRot 17:c694a0e63a89 663 }
JKleinRot 19:38c9d177b6ee 664 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 665 }
JKleinRot 17:c694a0e63a89 666
JKleinRot 19:38c9d177b6ee 667
JKleinRot 25:71e52c56be13 668 case BBT: { //Vierde keuze maken in doel
JKleinRot 17:c694a0e63a89 669 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 670 lcd.printf(" BBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 671 pc.printf("BBT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 672
JKleinRot 25:71e52c56be13 673 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 674 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 675
JKleinRot 26:438a498e5526 676 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 677 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 678 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 679
JKleinRot 25:71e52c56be13 680 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 681 }
JKleinRot 17:c694a0e63a89 682
JKleinRot 19:38c9d177b6ee 683 while(state == BBT) {
JKleinRot 17:c694a0e63a89 684 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 685 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 686
JKleinRot 25:71e52c56be13 687 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 688
JKleinRot 25:71e52c56be13 689 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 690 state = BBTB; //Ga door naar state BBTB
JKleinRot 17:c694a0e63a89 691 }
JKleinRot 25:71e52c56be13 692 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 693 state = BBTT; //Ga door naar state BBTT
JKleinRot 17:c694a0e63a89 694 }
JKleinRot 17:c694a0e63a89 695 }
JKleinRot 19:38c9d177b6ee 696 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 697 }
JKleinRot 19:38c9d177b6ee 698
JKleinRot 25:71e52c56be13 699 case BTB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 700 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 701 lcd.printf(" BTB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 702 pc.printf("BTB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 703
JKleinRot 25:71e52c56be13 704 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 705 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 706
JKleinRot 26:438a498e5526 707 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 708 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 709 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 710
JKleinRot 25:71e52c56be13 711 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 712 }
JKleinRot 17:c694a0e63a89 713
JKleinRot 19:38c9d177b6ee 714 while(state == BTB) {
JKleinRot 17:c694a0e63a89 715 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 716 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 717
JKleinRot 25:71e52c56be13 718 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 719
JKleinRot 25:71e52c56be13 720 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 721 state = BTBB; //Ga door naar state BTBB
JKleinRot 17:c694a0e63a89 722 }
JKleinRot 25:71e52c56be13 723 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 724 state = BTBT; //Ga door naar state BTBT
JKleinRot 17:c694a0e63a89 725 }
JKleinRot 17:c694a0e63a89 726 }
JKleinRot 19:38c9d177b6ee 727 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 728 }
JKleinRot 19:38c9d177b6ee 729
JKleinRot 25:71e52c56be13 730 case BTT: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 731 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 732 lcd.printf(" BTT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 733 pc.printf("BTT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 734
JKleinRot 25:71e52c56be13 735 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 736 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 737
JKleinRot 26:438a498e5526 738 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 739 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 740 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 741
JKleinRot 25:71e52c56be13 742 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 743 }
JKleinRot 17:c694a0e63a89 744
JKleinRot 19:38c9d177b6ee 745 while(state == BTT) {
JKleinRot 17:c694a0e63a89 746 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 747 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 748
JKleinRot 25:71e52c56be13 749 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 750
JKleinRot 25:71e52c56be13 751 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 752 state = BTTB; //Ga door naar state BTTB
JKleinRot 17:c694a0e63a89 753 }
JKleinRot 25:71e52c56be13 754 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 755 state = BTTT; //Ga door naar state BTTT
JKleinRot 17:c694a0e63a89 756 }
JKleinRot 17:c694a0e63a89 757 }
JKleinRot 19:38c9d177b6ee 758 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 759 }
JKleinRot 19:38c9d177b6ee 760
JKleinRot 25:71e52c56be13 761 case TBB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 762 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 763 lcd.printf(" TBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 764 pc.printf("TBB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 765
JKleinRot 25:71e52c56be13 766 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 767 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 768
JKleinRot 26:438a498e5526 769 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 770 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 771 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 772
JKleinRot 25:71e52c56be13 773 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 774 }
JKleinRot 17:c694a0e63a89 775
JKleinRot 19:38c9d177b6ee 776 while(state == TBB) {
JKleinRot 17:c694a0e63a89 777 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 778 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 779
JKleinRot 25:71e52c56be13 780 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 781
JKleinRot 25:71e52c56be13 782 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 783 state = TBBB; //Ga door naar state TBBB
JKleinRot 17:c694a0e63a89 784 }
JKleinRot 25:71e52c56be13 785 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 786 state = TBBT; //Ga door naar state TBBT
JKleinRot 17:c694a0e63a89 787 }
JKleinRot 17:c694a0e63a89 788 }
JKleinRot 19:38c9d177b6ee 789 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 790 }
JKleinRot 19:38c9d177b6ee 791
JKleinRot 25:71e52c56be13 792 case TBT: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 793 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 794 lcd.printf(" TBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 795 pc.printf("TBT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 796
JKleinRot 25:71e52c56be13 797 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 798 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 799
JKleinRot 26:438a498e5526 800 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 801 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 802 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 803
JKleinRot 25:71e52c56be13 804 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 805 }
JKleinRot 17:c694a0e63a89 806
JKleinRot 19:38c9d177b6ee 807 while(state == TBT) {
JKleinRot 17:c694a0e63a89 808 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 809 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 810
JKleinRot 25:71e52c56be13 811 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 812
JKleinRot 26:438a498e5526 813 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 814 state = TBTB; //Ga door naar state TBTB
JKleinRot 17:c694a0e63a89 815 }
JKleinRot 26:438a498e5526 816 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 817 state = TBTT; //Ga door naar state TBTT
JKleinRot 17:c694a0e63a89 818 }
JKleinRot 17:c694a0e63a89 819 }
JKleinRot 19:38c9d177b6ee 820 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 821 }
JKleinRot 19:38c9d177b6ee 822
JKleinRot 25:71e52c56be13 823 case TTB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 824 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 22:838a17065bc7 825 lcd.printf(" TTB "); //Tekst op LCD scherm
JKleinRot 22:838a17065bc7 826 pc.printf("TTB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 827
JKleinRot 25:71e52c56be13 828 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 829 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 830
JKleinRot 26:438a498e5526 831 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 832 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 833 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 834
JKleinRot 25:71e52c56be13 835 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 836 }
JKleinRot 17:c694a0e63a89 837
JKleinRot 22:838a17065bc7 838 while(state == TTB) {
JKleinRot 17:c694a0e63a89 839 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 840 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 841
JKleinRot 25:71e52c56be13 842 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 843
JKleinRot 26:438a498e5526 844 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 845 state = TTBB; //Ga door naar state TTBB
JKleinRot 17:c694a0e63a89 846 }
JKleinRot 26:438a498e5526 847 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 848 state = TTBT; //Ga door naar state TTBT
JKleinRot 17:c694a0e63a89 849 }
JKleinRot 17:c694a0e63a89 850 }
JKleinRot 19:38c9d177b6ee 851 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 852 }
JKleinRot 19:38c9d177b6ee 853
JKleinRot 25:71e52c56be13 854 case TTT: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 855 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 856 lcd.printf(" TTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 857 pc.printf("TTT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 858
JKleinRot 25:71e52c56be13 859 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 860 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 861
JKleinRot 26:438a498e5526 862 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 863 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 22:838a17065bc7 864 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 865
JKleinRot 25:71e52c56be13 866 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 867 }
JKleinRot 17:c694a0e63a89 868
JKleinRot 19:38c9d177b6ee 869 while(state == TTT) {
JKleinRot 17:c694a0e63a89 870 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 17:c694a0e63a89 871 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 872
JKleinRot 25:71e52c56be13 873 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 874
JKleinRot 26:438a498e5526 875 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 876 state = TTTB; //Ga door naar state TTTB
JKleinRot 17:c694a0e63a89 877 }
JKleinRot 26:438a498e5526 878 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 879 state = TTTT; //Ga door naar state TTTT
JKleinRot 17:c694a0e63a89 880 }
JKleinRot 17:c694a0e63a89 881 }
JKleinRot 19:38c9d177b6ee 882 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 883 }
JKleinRot 19:38c9d177b6ee 884
JKleinRot 25:71e52c56be13 885 case BBBB: { //Motoraansturing voor doel rechtsonder
JKleinRot 17:c694a0e63a89 886 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 887 lcd.printf(" BBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 888 pc.printf("BBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 889
JKleinRot 19:38c9d177b6ee 890 while(state == BBBB) {
JKleinRot 25:71e52c56be13 891 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 892 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 893
JKleinRot 32:80fc2de5b511 894 while(puls_motor_arm1.getPosition() > -107) {
JKleinRot 32:80fc2de5b511 895 referentie_arm1 = referentie_arm1 - 287.0/200.0; //Referentie arm 1 loopt af in een seconden naar de gewenste waarde
JKleinRot 26:438a498e5526 896 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 897 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 898 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 899 }
JKleinRot 29:f95f0cc84349 900
JKleinRot 32:80fc2de5b511 901 if(puls_motor_arm1.getPosition() <= -107) {
JKleinRot 32:80fc2de5b511 902 referentie_arm1 = -107;
JKleinRot 27:5ac1fc1005d7 903 }
JKleinRot 25:71e52c56be13 904
JKleinRot 25:71e52c56be13 905 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 906 referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde
JKleinRot 26:438a498e5526 907 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 908 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 909 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 910 }
JKleinRot 29:f95f0cc84349 911 ticker_motor_arm2_pid.detach();
JKleinRot 32:80fc2de5b511 912 pwm_to_motor_arm2 = 0.8;
JKleinRot 29:f95f0cc84349 913 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 29:f95f0cc84349 914 dir_motor_arm2.write(0);
JKleinRot 32:80fc2de5b511 915 while(puls_motor_arm2.getPosition() > -306) {
JKleinRot 25:71e52c56be13 916
JKleinRot 26:438a498e5526 917 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 918 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 919 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 920 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 921 }
JKleinRot 29:f95f0cc84349 922 pwm_to_motor_arm2 = 0.5;
JKleinRot 29:f95f0cc84349 923 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 29:f95f0cc84349 924 dir_motor_arm2.write(1);
JKleinRot 29:f95f0cc84349 925 while(puls_motor_arm2.getPosition() <= 211) {
JKleinRot 25:71e52c56be13 926
JKleinRot 29:f95f0cc84349 927 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 29:f95f0cc84349 928 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 29:f95f0cc84349 929 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 29:f95f0cc84349 930 }
JKleinRot 29:f95f0cc84349 931 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 29:f95f0cc84349 932 pc.printf("staat stil");
JKleinRot 25:71e52c56be13 933
JKleinRot 29:f95f0cc84349 934 state = THUISPOSITIE_RECHTS; //Door naar de volgende state
JKleinRot 29:f95f0cc84349 935
JKleinRot 19:38c9d177b6ee 936 }
JKleinRot 29:f95f0cc84349 937
JKleinRot 29:f95f0cc84349 938
JKleinRot 19:38c9d177b6ee 939 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 940 }
JKleinRot 19:38c9d177b6ee 941
JKleinRot 25:71e52c56be13 942 case BBBT: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 943 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 944 lcd.printf(" BBBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 945 pc.printf("BBBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 946 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 947 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 948
JKleinRot 19:38c9d177b6ee 949 while(state == BBBT) {
JKleinRot 26:438a498e5526 950 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 951 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 952 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 953 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 954 }
JKleinRot 19:38c9d177b6ee 955 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 956 }
JKleinRot 19:38c9d177b6ee 957
JKleinRot 25:71e52c56be13 958 case BBTB: { //Motoraansturing voor doel rechtsmidden
JKleinRot 17:c694a0e63a89 959 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 960 lcd.printf(" BBTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 961 pc.printf("BBTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 962
JKleinRot 19:38c9d177b6ee 963 while(state == BBTB) {
JKleinRot 25:71e52c56be13 964 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 965 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 966
JKleinRot 32:80fc2de5b511 967 while(puls_motor_arm1.getPosition() > -107) {
JKleinRot 32:80fc2de5b511 968 referentie_arm1 = referentie_arm1 - 287.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 969 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 970 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 971 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 972 }
JKleinRot 29:f95f0cc84349 973
JKleinRot 32:80fc2de5b511 974 if(puls_motor_arm1.getPosition() <= -107) {
JKleinRot 32:80fc2de5b511 975 referentie_arm1 = -107;
JKleinRot 27:5ac1fc1005d7 976 }
JKleinRot 25:71e52c56be13 977
JKleinRot 25:71e52c56be13 978 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 979 referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 980 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 981 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 982 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 983 }
JKleinRot 30:f79cf70e2917 984 ticker_motor_arm2_pid.detach();
JKleinRot 32:80fc2de5b511 985 pwm_to_motor_arm2 = 0.9;
JKleinRot 30:f79cf70e2917 986 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 987 dir_motor_arm2.write(0);
JKleinRot 32:80fc2de5b511 988 while(puls_motor_arm2.getPosition() > -306) {
JKleinRot 25:71e52c56be13 989
JKleinRot 26:438a498e5526 990 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 991 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 992 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 993 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 994 }
JKleinRot 30:f79cf70e2917 995 pwm_to_motor_arm2 = 0.5;
JKleinRot 30:f79cf70e2917 996 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 997 dir_motor_arm2.write(1);
JKleinRot 30:f79cf70e2917 998 while(puls_motor_arm2.getPosition() <= 211) {
JKleinRot 25:71e52c56be13 999
JKleinRot 30:f79cf70e2917 1000 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 30:f79cf70e2917 1001 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 30:f79cf70e2917 1002 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 30:f79cf70e2917 1003 }
JKleinRot 30:f79cf70e2917 1004 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 30:f79cf70e2917 1005 state = THUISPOSITIE_RECHTS; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1006
JKleinRot 30:f79cf70e2917 1007
JKleinRot 19:38c9d177b6ee 1008 }
JKleinRot 19:38c9d177b6ee 1009 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1010 }
JKleinRot 19:38c9d177b6ee 1011
JKleinRot 25:71e52c56be13 1012 case BBTT: { //Motoraansturing voor doel rechtsboven
JKleinRot 17:c694a0e63a89 1013 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1014 lcd.printf(" BBTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1015 pc.printf("BBTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 1016
JKleinRot 19:38c9d177b6ee 1017 while(state == BBTT) {
JKleinRot 25:71e52c56be13 1018 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 1019 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1020
JKleinRot 32:80fc2de5b511 1021 while(puls_motor_arm1.getPosition() > -107) {
JKleinRot 32:80fc2de5b511 1022 referentie_arm1 = referentie_arm1 - 287.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1023 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1024 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1025 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1026 }
JKleinRot 29:f95f0cc84349 1027
JKleinRot 32:80fc2de5b511 1028 if(puls_motor_arm1.getPosition() <= -107) {
JKleinRot 32:80fc2de5b511 1029 referentie_arm1 = -107;
JKleinRot 27:5ac1fc1005d7 1030 }
JKleinRot 25:71e52c56be13 1031
JKleinRot 25:71e52c56be13 1032 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 1033 referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1034 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1035 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1036 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1037 }
JKleinRot 30:f79cf70e2917 1038 ticker_motor_arm2_pid.detach();
JKleinRot 30:f79cf70e2917 1039 pwm_to_motor_arm2 = 1;
JKleinRot 30:f79cf70e2917 1040 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1041 dir_motor_arm2.write(0);
JKleinRot 32:80fc2de5b511 1042 while(puls_motor_arm2.getPosition() > -306) {
JKleinRot 25:71e52c56be13 1043
JKleinRot 26:438a498e5526 1044 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1045 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1046 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1047 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1048 }
JKleinRot 30:f79cf70e2917 1049 pwm_to_motor_arm2 = 0.5;
JKleinRot 30:f79cf70e2917 1050 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1051 dir_motor_arm2.write(1);
JKleinRot 30:f79cf70e2917 1052 while(puls_motor_arm2.getPosition() <= 211) {
JKleinRot 25:71e52c56be13 1053
JKleinRot 30:f79cf70e2917 1054 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 30:f79cf70e2917 1055 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 30:f79cf70e2917 1056 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 30:f79cf70e2917 1057 }
JKleinRot 30:f79cf70e2917 1058 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 30:f79cf70e2917 1059 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 30:f79cf70e2917 1060 state = THUISPOSITIE_RECHTS; //Door naar de volgende state
JKleinRot 30:f79cf70e2917 1061 }
JKleinRot 25:71e52c56be13 1062
JKleinRot 19:38c9d177b6ee 1063 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1064 }
JKleinRot 19:38c9d177b6ee 1065
JKleinRot 25:71e52c56be13 1066 case BTBB: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1067 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1068 lcd.printf(" BTBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1069 pc.printf("BTBB\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1070 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1071 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1072
JKleinRot 19:38c9d177b6ee 1073 while(state == BTBB) {
JKleinRot 26:438a498e5526 1074 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1075 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1076 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1077 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1078 }
JKleinRot 19:38c9d177b6ee 1079 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1080 }
JKleinRot 19:38c9d177b6ee 1081
JKleinRot 25:71e52c56be13 1082 case BTBT: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1083 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1084 lcd.printf(" BTBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1085 pc.printf("BTBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1086 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1087 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1088
JKleinRot 19:38c9d177b6ee 1089 while(state == BTBT) {
JKleinRot 26:438a498e5526 1090 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1091 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1092 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1093 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1094 }
JKleinRot 19:38c9d177b6ee 1095 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1096 }
JKleinRot 19:38c9d177b6ee 1097
JKleinRot 25:71e52c56be13 1098 case BTTB: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1099 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1100 lcd.printf(" BTTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1101 pc.printf("BTTB\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1102 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1103 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1104
JKleinRot 19:38c9d177b6ee 1105 while(state == BTTB) {
JKleinRot 26:438a498e5526 1106 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1107 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1108 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1109 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1110 }
JKleinRot 19:38c9d177b6ee 1111 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1112 }
JKleinRot 19:38c9d177b6ee 1113
JKleinRot 25:71e52c56be13 1114 case BTTT: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1115 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1116 lcd.printf(" BTTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1117 pc.printf("BTTT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1118 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1119 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1120
JKleinRot 19:38c9d177b6ee 1121 while(state == BTTT) {
JKleinRot 26:438a498e5526 1122 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1123 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1124 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1125 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1126 }
JKleinRot 19:38c9d177b6ee 1127 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1128 }
JKleinRot 19:38c9d177b6ee 1129
JKleinRot 25:71e52c56be13 1130 case TBBB: { //Motoraansturing voor doel middenonder
JKleinRot 17:c694a0e63a89 1131 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1132 lcd.printf(" TBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1133 pc.printf("TBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 1134
JKleinRot 19:38c9d177b6ee 1135 while(state == TBBB) {
JKleinRot 19:38c9d177b6ee 1136 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 19:38c9d177b6ee 1137 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 19:38c9d177b6ee 1138
JKleinRot 32:80fc2de5b511 1139 while(puls_motor_arm1.getPosition() > 25) {
JKleinRot 32:80fc2de5b511 1140 referentie_arm1 = referentie_arm1 - 155.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1141 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1142 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1143 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 23:5267c928ae2b 1144 }
JKleinRot 29:f95f0cc84349 1145
JKleinRot 32:80fc2de5b511 1146 if(puls_motor_arm1.getPosition() <= 25) {
JKleinRot 32:80fc2de5b511 1147 referentie_arm1 = 25;
JKleinRot 27:5ac1fc1005d7 1148 }
JKleinRot 19:38c9d177b6ee 1149
JKleinRot 23:5267c928ae2b 1150 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1151 referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1152 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1153 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1154 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 23:5267c928ae2b 1155 }
JKleinRot 30:f79cf70e2917 1156 ticker_motor_arm2_pid.detach();
JKleinRot 32:80fc2de5b511 1157 pwm_to_motor_arm2 = 0.8;
JKleinRot 30:f79cf70e2917 1158 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1159 dir_motor_arm2.write(0);
JKleinRot 23:5267c928ae2b 1160
JKleinRot 32:80fc2de5b511 1161 while(puls_motor_arm2.getPosition() > -370) {
JKleinRot 23:5267c928ae2b 1162
JKleinRot 26:438a498e5526 1163 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1164 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1165 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1166 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1167 }
JKleinRot 30:f79cf70e2917 1168 pwm_to_motor_arm2 = 0.5;
JKleinRot 30:f79cf70e2917 1169 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1170 dir_motor_arm2.write(1);
JKleinRot 23:5267c928ae2b 1171
JKleinRot 30:f79cf70e2917 1172 while(puls_motor_arm2.getPosition() <= 167) {
JKleinRot 30:f79cf70e2917 1173 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 30:f79cf70e2917 1174 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 30:f79cf70e2917 1175 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 30:f79cf70e2917 1176 }
JKleinRot 30:f79cf70e2917 1177 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 30:f79cf70e2917 1178 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 30:f79cf70e2917 1179 state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
JKleinRot 23:5267c928ae2b 1180
JKleinRot 19:38c9d177b6ee 1181 }
JKleinRot 24:a1fdc830f96c 1182 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1183 }
JKleinRot 23:5267c928ae2b 1184
JKleinRot 25:71e52c56be13 1185 case TBBT: { //Geen motoraansturing
JKleinRot 25:71e52c56be13 1186 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 25:71e52c56be13 1187 lcd.printf(" TBBT "); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 1188 pc.printf("TBBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1189 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1190 lcd.cls(); //LCD scherm leegmaken
JKleinRot 25:71e52c56be13 1191
JKleinRot 25:71e52c56be13 1192 while(state == TBBT) {
JKleinRot 26:438a498e5526 1193 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1194 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1195 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1196 state = WACHT; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1197 }
JKleinRot 25:71e52c56be13 1198 break; //Stop met alle acties in deze case
JKleinRot 25:71e52c56be13 1199 }
JKleinRot 25:71e52c56be13 1200
JKleinRot 25:71e52c56be13 1201 case TBTB: { //Motoraansturing voor doel midden
JKleinRot 25:71e52c56be13 1202 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 25:71e52c56be13 1203 lcd.printf(" TBTB "); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 1204 pc.printf("TBTB\n\r"); //Controle naar pc sturen
JKleinRot 25:71e52c56be13 1205
JKleinRot 25:71e52c56be13 1206 while(state == TBTB) {
JKleinRot 25:71e52c56be13 1207 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 1208 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1209
JKleinRot 32:80fc2de5b511 1210 while(puls_motor_arm1.getPosition() > 25) {
JKleinRot 32:80fc2de5b511 1211 referentie_arm1 = referentie_arm1 - 155.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1212 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1213 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1214 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1215 }
JKleinRot 29:f95f0cc84349 1216
JKleinRot 32:80fc2de5b511 1217 if(puls_motor_arm1.getPosition() <= 25) {
JKleinRot 32:80fc2de5b511 1218 referentie_arm1 = 25;
JKleinRot 27:5ac1fc1005d7 1219 }
JKleinRot 25:71e52c56be13 1220
JKleinRot 25:71e52c56be13 1221 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1222 referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie
JKleinRot 26:438a498e5526 1223 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1224 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1225 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1226 }
JKleinRot 30:f79cf70e2917 1227 ticker_motor_arm2_pid.detach();
JKleinRot 32:80fc2de5b511 1228 pwm_to_motor_arm2 = 0.9;
JKleinRot 30:f79cf70e2917 1229 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1230 dir_motor_arm2.write(0);
JKleinRot 25:71e52c56be13 1231
JKleinRot 32:80fc2de5b511 1232 while(puls_motor_arm2.getPosition() > -370) {
JKleinRot 25:71e52c56be13 1233
JKleinRot 26:438a498e5526 1234 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1235 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1236 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1237 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1238 }
JKleinRot 30:f79cf70e2917 1239 pwm_to_motor_arm2 = 0.5;
JKleinRot 30:f79cf70e2917 1240 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1241 dir_motor_arm2.write(1);
JKleinRot 25:71e52c56be13 1242
JKleinRot 30:f79cf70e2917 1243 while(puls_motor_arm2.getPosition() <= 167) {
JKleinRot 30:f79cf70e2917 1244 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 30:f79cf70e2917 1245 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 30:f79cf70e2917 1246 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 30:f79cf70e2917 1247 }
JKleinRot 30:f79cf70e2917 1248 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 30:f79cf70e2917 1249 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 30:f79cf70e2917 1250 state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1251
JKleinRot 30:f79cf70e2917 1252
JKleinRot 25:71e52c56be13 1253 }
JKleinRot 25:71e52c56be13 1254 break; //Stop met alle acties in deze case
JKleinRot 25:71e52c56be13 1255 }
JKleinRot 25:71e52c56be13 1256
JKleinRot 25:71e52c56be13 1257 case TBTT: { //Motoraansturing voor doel middenboven
JKleinRot 25:71e52c56be13 1258 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 25:71e52c56be13 1259 lcd.printf(" TBTT "); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 1260 pc.printf("TBTT\n\r"); //Controle naar pc sturen
JKleinRot 25:71e52c56be13 1261
JKleinRot 25:71e52c56be13 1262 while(state == TBTT) {
JKleinRot 25:71e52c56be13 1263 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 1264 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1265
JKleinRot 32:80fc2de5b511 1266 while(puls_motor_arm1.getPosition() > 25) {
JKleinRot 32:80fc2de5b511 1267 referentie_arm1 = referentie_arm1 - 155.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste positie
JKleinRot 26:438a498e5526 1268 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1269 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1270 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1271 }
JKleinRot 29:f95f0cc84349 1272
JKleinRot 32:80fc2de5b511 1273 if(puls_motor_arm1.getPosition() <= 25) {
JKleinRot 32:80fc2de5b511 1274 referentie_arm1 = 25;
JKleinRot 27:5ac1fc1005d7 1275 }
JKleinRot 25:71e52c56be13 1276
JKleinRot 25:71e52c56be13 1277 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1278 referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie
JKleinRot 26:438a498e5526 1279 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1280 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1281 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1282 }
JKleinRot 30:f79cf70e2917 1283 ticker_motor_arm2_pid.detach();
JKleinRot 30:f79cf70e2917 1284 pwm_to_motor_arm2 = 1;
JKleinRot 30:f79cf70e2917 1285 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1286 dir_motor_arm2.write(0);
JKleinRot 25:71e52c56be13 1287
JKleinRot 32:80fc2de5b511 1288 while(puls_motor_arm2.getPosition() > -370) {
JKleinRot 25:71e52c56be13 1289
JKleinRot 26:438a498e5526 1290 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1291 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1292 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1293 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1294 }
JKleinRot 30:f79cf70e2917 1295 pwm_to_motor_arm2 = 0.5;
JKleinRot 30:f79cf70e2917 1296 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 30:f79cf70e2917 1297 dir_motor_arm2.write(1);
JKleinRot 25:71e52c56be13 1298
JKleinRot 30:f79cf70e2917 1299 while(puls_motor_arm2.getPosition() <= 167) {
JKleinRot 30:f79cf70e2917 1300 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 30:f79cf70e2917 1301 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 30:f79cf70e2917 1302 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 30:f79cf70e2917 1303 }
JKleinRot 30:f79cf70e2917 1304 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 30:f79cf70e2917 1305 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 30:f79cf70e2917 1306 state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1307
JKleinRot 30:f79cf70e2917 1308
JKleinRot 25:71e52c56be13 1309 }
JKleinRot 25:71e52c56be13 1310 break; //Stop met alle acties in deze case
JKleinRot 25:71e52c56be13 1311 }
JKleinRot 25:71e52c56be13 1312
JKleinRot 25:71e52c56be13 1313 case TTBB: { //Motoraansturing voor doel linksonder
JKleinRot 24:a1fdc830f96c 1314 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1315 lcd.printf(" TTBB "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1316 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1317
JKleinRot 24:a1fdc830f96c 1318 while(state == TTBB) {
JKleinRot 24:a1fdc830f96c 1319 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 24:a1fdc830f96c 1320 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1321
JKleinRot 31:36fe36657f8d 1322 ticker_motor_arm2_pid.detach();
JKleinRot 32:80fc2de5b511 1323 pwm_to_motor_arm2 = 0.8;
JKleinRot 31:36fe36657f8d 1324 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 31:36fe36657f8d 1325 dir_motor_arm2.write(0);
JKleinRot 24:a1fdc830f96c 1326
JKleinRot 32:80fc2de5b511 1327 while(puls_motor_arm2.getPosition() > -414) {
JKleinRot 24:a1fdc830f96c 1328
JKleinRot 31:36fe36657f8d 1329 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 31:36fe36657f8d 1330 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 31:36fe36657f8d 1331 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 31:36fe36657f8d 1332 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 31:36fe36657f8d 1333 }
JKleinRot 31:36fe36657f8d 1334 pwm_to_motor_arm2 = 0.5;
JKleinRot 31:36fe36657f8d 1335 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 31:36fe36657f8d 1336 dir_motor_arm2.write(1);
JKleinRot 24:a1fdc830f96c 1337
JKleinRot 32:80fc2de5b511 1338 while(puls_motor_arm2.getPosition()<= 123) {
JKleinRot 31:36fe36657f8d 1339 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 31:36fe36657f8d 1340 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 31:36fe36657f8d 1341 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 31:36fe36657f8d 1342 }
JKleinRot 31:36fe36657f8d 1343 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 31:36fe36657f8d 1344 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 31:36fe36657f8d 1345 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1346
JKleinRot 31:36fe36657f8d 1347
JKleinRot 24:a1fdc830f96c 1348 }
JKleinRot 24:a1fdc830f96c 1349 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1350 }
JKleinRot 23:5267c928ae2b 1351
JKleinRot 25:71e52c56be13 1352 case TTBT: { //Geen motoraansturing
JKleinRot 24:a1fdc830f96c 1353 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1354 lcd.printf(" TTBT "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1355 pc.printf("TTBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1356 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1357 lcd.cls(); //LCD scherm leegmaken
JKleinRot 24:a1fdc830f96c 1358
JKleinRot 24:a1fdc830f96c 1359 while(state == TTBT) {
JKleinRot 26:438a498e5526 1360 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1361 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1362 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1363 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1364 }
JKleinRot 24:a1fdc830f96c 1365 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1366 }
JKleinRot 24:a1fdc830f96c 1367
JKleinRot 25:71e52c56be13 1368 case TTTB: { //Motoraansturing voor doel linksmidden
JKleinRot 24:a1fdc830f96c 1369 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1370 lcd.printf(" TTTB "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1371 pc.printf("TTTB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1372
JKleinRot 24:a1fdc830f96c 1373 while(state == TTTB) {
JKleinRot 24:a1fdc830f96c 1374
JKleinRot 24:a1fdc830f96c 1375 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 24:a1fdc830f96c 1376 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1377
JKleinRot 31:36fe36657f8d 1378 ticker_motor_arm2_pid.detach();
JKleinRot 31:36fe36657f8d 1379 pwm_to_motor_arm2 = 1;
JKleinRot 31:36fe36657f8d 1380 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 31:36fe36657f8d 1381 dir_motor_arm2.write(0);
JKleinRot 24:a1fdc830f96c 1382
JKleinRot 32:80fc2de5b511 1383 while(puls_motor_arm2.getPosition() > -414) {
JKleinRot 24:a1fdc830f96c 1384
JKleinRot 31:36fe36657f8d 1385 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 31:36fe36657f8d 1386 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 31:36fe36657f8d 1387 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 31:36fe36657f8d 1388 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 31:36fe36657f8d 1389 }
JKleinRot 31:36fe36657f8d 1390 pwm_to_motor_arm2 = 0.5;
JKleinRot 31:36fe36657f8d 1391 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 31:36fe36657f8d 1392 dir_motor_arm2.write(1);
JKleinRot 24:a1fdc830f96c 1393
JKleinRot 32:80fc2de5b511 1394 while(puls_motor_arm2.getPosition()<= 123) {
JKleinRot 31:36fe36657f8d 1395 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 31:36fe36657f8d 1396 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 31:36fe36657f8d 1397 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 31:36fe36657f8d 1398 }
JKleinRot 31:36fe36657f8d 1399 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 31:36fe36657f8d 1400 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 31:36fe36657f8d 1401 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1402
JKleinRot 31:36fe36657f8d 1403
JKleinRot 23:5267c928ae2b 1404 }
JKleinRot 19:38c9d177b6ee 1405
JKleinRot 24:a1fdc830f96c 1406 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1407 }
JKleinRot 24:a1fdc830f96c 1408
JKleinRot 25:71e52c56be13 1409 case TTTT: { //Motoraansturing voor doel linksboven
JKleinRot 24:a1fdc830f96c 1410 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1411 lcd.printf(" TTTT "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1412 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1413
JKleinRot 24:a1fdc830f96c 1414 while(state == TTTT) {
JKleinRot 24:a1fdc830f96c 1415 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 24:a1fdc830f96c 1416 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1417
JKleinRot 31:36fe36657f8d 1418 ticker_motor_arm2_pid.detach();
JKleinRot 31:36fe36657f8d 1419 pwm_to_motor_arm2 = 1;
JKleinRot 31:36fe36657f8d 1420 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 31:36fe36657f8d 1421 dir_motor_arm2.write(0);
JKleinRot 24:a1fdc830f96c 1422
JKleinRot 32:80fc2de5b511 1423 while(puls_motor_arm2.getPosition() > -414) {
JKleinRot 24:a1fdc830f96c 1424
JKleinRot 31:36fe36657f8d 1425 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 31:36fe36657f8d 1426 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 31:36fe36657f8d 1427 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 31:36fe36657f8d 1428 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 31:36fe36657f8d 1429 }
JKleinRot 31:36fe36657f8d 1430 pwm_to_motor_arm2 = 0.5;
JKleinRot 31:36fe36657f8d 1431 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 31:36fe36657f8d 1432 dir_motor_arm2.write(1);
JKleinRot 24:a1fdc830f96c 1433
JKleinRot 32:80fc2de5b511 1434 while(puls_motor_arm2.getPosition()<= 123) {
JKleinRot 31:36fe36657f8d 1435 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 31:36fe36657f8d 1436 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 31:36fe36657f8d 1437 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1438 }
JKleinRot 31:36fe36657f8d 1439 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 31:36fe36657f8d 1440 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 31:36fe36657f8d 1441 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1442 }
JKleinRot 27:5ac1fc1005d7 1443 break;
JKleinRot 24:a1fdc830f96c 1444 }
JKleinRot 24:a1fdc830f96c 1445
JKleinRot 26:438a498e5526 1446 case THUISPOSITIE_MIDDEN: { //Terug naar gekalibreerde positie
JKleinRot 24:a1fdc830f96c 1447 while(puls_motor_arm2.getPosition() > 123) {
JKleinRot 25:71e52c56be13 1448 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 1449 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1450
JKleinRot 26:438a498e5526 1451 referentie_arm2 = referentie_arm2 - 44.0/200.0; //Referentie arm 2 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1452 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1453 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1454 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1455 }
JKleinRot 24:a1fdc830f96c 1456
JKleinRot 32:80fc2de5b511 1457 while(puls_motor_arm1.getPosition() < 203) {
JKleinRot 25:71e52c56be13 1458 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 1459 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1460
JKleinRot 32:80fc2de5b511 1461 referentie_arm1 = referentie_arm1 + 155.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1462 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1463 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1464 pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1465 }
JKleinRot 24:a1fdc830f96c 1466
JKleinRot 26:438a498e5526 1467 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1468 break;
JKleinRot 24:a1fdc830f96c 1469 }
JKleinRot 24:a1fdc830f96c 1470
JKleinRot 26:438a498e5526 1471 case THUISPOSITIE_RECHTS: { //Terug naar gekalibreerde positie
JKleinRot 25:71e52c56be13 1472 while(puls_motor_arm2.getPosition() > 123) {
JKleinRot 25:71e52c56be13 1473 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 1474 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1475
JKleinRot 26:438a498e5526 1476 referentie_arm2 = referentie_arm2 - 88.0/200.0; //Referentie arm 2 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1477 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1478 pc.printf("get position %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1479 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1480 }
JKleinRot 25:71e52c56be13 1481
JKleinRot 32:80fc2de5b511 1482 while(puls_motor_arm1.getPosition() < 203) {
JKleinRot 25:71e52c56be13 1483 while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 25:71e52c56be13 1484 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1485
JKleinRot 32:80fc2de5b511 1486 referentie_arm1 = referentie_arm1 + 287.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1487 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1488 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1489 pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1490 }
JKleinRot 27:5ac1fc1005d7 1491 state = WACHT;
JKleinRot 27:5ac1fc1005d7 1492 break;
JKleinRot 25:71e52c56be13 1493 }
JKleinRot 25:71e52c56be13 1494
JKleinRot 26:438a498e5526 1495 case WACHT: { //Even wachten en weer terug naar start
JKleinRot 26:438a498e5526 1496 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1497 lcd.printf(" WACHT "); //Tekst op LCD scherm
JKleinRot 28:e2f5cee5e73b 1498 wait(5); //Vijf seconden wachten
JKleinRot 26:438a498e5526 1499 state = START; //Terug naar state START
JKleinRot 27:5ac1fc1005d7 1500 break;
JKleinRot 26:438a498e5526 1501 }
JKleinRot 23:5267c928ae2b 1502
JKleinRot 26:438a498e5526 1503 default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 26:438a498e5526 1504 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 23:5267c928ae2b 1505 }
JKleinRot 9:454e7da8ab65 1506 }
JKleinRot 20:1cb0cf0d49ac 1507 }
JKleinRot 24:a1fdc830f96c 1508 }
JKleinRot 26:438a498e5526 1509
JKleinRot 27:5ac1fc1005d7 1510