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@29:f95f0cc84349, 2014-11-03 (annotated)

Committer:
JKleinRot
Date:
Mon Nov 03 23:03:59 2014 +0000
Revision:
29:f95f0cc84349
Parent:
28:e2f5cee5e73b
Child:
30:f79cf70e2917
Hardere snelheid bij BBBB;

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 25:71e52c56be13 249 referentie_arm1 = referentie_arm1 + 180.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 25:71e52c56be13 255 if (puls_motor_arm1.getPosition() >= 180) { //Als het gewenste aantal pulsen behaald is
JKleinRot 25:71e52c56be13 256 referentie_arm1 = 180; //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 29:f95f0cc84349 451 state = BBBB;
JKleinRot 29:f95f0cc84349 452 break;
JKleinRot 17:c694a0e63a89 453 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 454 lcd.printf(" B "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 455 pc.printf("B\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 456
JKleinRot 27:5ac1fc1005d7 457 EMG.reset();
JKleinRot 25:71e52c56be13 458 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 459
JKleinRot 25:71e52c56be13 460 while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten
JKleinRot 22:838a17065bc7 461 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 462 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 463
JKleinRot 25:71e52c56be13 464 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 465 }
JKleinRot 23:5267c928ae2b 466
JKleinRot 19:38c9d177b6ee 467 while(state == B) {
JKleinRot 19:38c9d177b6ee 468 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 469 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 470
JKleinRot 25:71e52c56be13 471 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 23:5267c928ae2b 472
JKleinRot 25:71e52c56be13 473 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 474 state = BB; //Ga door naar state BB
JKleinRot 19:38c9d177b6ee 475 }
JKleinRot 21:f4e9f6c28de1 476 if(xtf>= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 477 state = BT; //Ga door naar state BT
JKleinRot 19:38c9d177b6ee 478 }
JKleinRot 19:38c9d177b6ee 479 }
JKleinRot 19:38c9d177b6ee 480 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 481 }
JKleinRot 19:38c9d177b6ee 482
JKleinRot 25:71e52c56be13 483 case T: { //Tweede keuze maken voor doel
JKleinRot 19:38c9d177b6ee 484 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 485 lcd.printf(" T "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 486 pc.printf("T\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 487
JKleinRot 27:5ac1fc1005d7 488 EMG.reset();
JKleinRot 25:71e52c56be13 489 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 490
JKleinRot 26:438a498e5526 491 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 492 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 493 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 494
JKleinRot 25:71e52c56be13 495 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 496 }
JKleinRot 19:38c9d177b6ee 497
JKleinRot 19:38c9d177b6ee 498 while(state == T) {
JKleinRot 17:c694a0e63a89 499 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 500 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 501
JKleinRot 25:71e52c56be13 502 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 16:c34c5d9dfe1a 503
JKleinRot 25:71e52c56be13 504 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 505 state = TB; //Ga door naar state TB
JKleinRot 16:c34c5d9dfe1a 506 }
JKleinRot 25:71e52c56be13 507 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 508 state = TT; //Ga door naar state TT
JKleinRot 16:c34c5d9dfe1a 509 }
JKleinRot 16:c34c5d9dfe1a 510 }
JKleinRot 17:c694a0e63a89 511 break; //Stop met alle acties in deze case
JKleinRot 16:c34c5d9dfe1a 512 }
JKleinRot 16:c34c5d9dfe1a 513
JKleinRot 25:71e52c56be13 514 case BB: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 515 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 516 lcd.printf(" BB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 517 pc.printf("BB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 518
JKleinRot 25:71e52c56be13 519 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 520 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 521
JKleinRot 26:438a498e5526 522 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 523 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 524 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 525
JKleinRot 25:71e52c56be13 526 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 527 }
JKleinRot 16:c34c5d9dfe1a 528
JKleinRot 19:38c9d177b6ee 529 while(state == BB) {
JKleinRot 17:c694a0e63a89 530 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 531 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 16:c34c5d9dfe1a 532
JKleinRot 25:71e52c56be13 533 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 16:c34c5d9dfe1a 534
JKleinRot 25:71e52c56be13 535 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 536 state = BBB; //Ga door naar state BBB
JKleinRot 16:c34c5d9dfe1a 537 }
JKleinRot 25:71e52c56be13 538 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 539 state = BBT; //Ga door naar state BBT
JKleinRot 16:c34c5d9dfe1a 540 }
JKleinRot 16:c34c5d9dfe1a 541 }
JKleinRot 17:c694a0e63a89 542 break; //Stop met alle acties in deze case
JKleinRot 16:c34c5d9dfe1a 543 }
JKleinRot 19:38c9d177b6ee 544
JKleinRot 25:71e52c56be13 545 case BT: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 546 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 547 lcd.printf(" BT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 548 pc.printf("BT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 549
JKleinRot 25:71e52c56be13 550 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 551 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 552
JKleinRot 26:438a498e5526 553 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 554 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 555 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 556
JKleinRot 25:71e52c56be13 557 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 558 }
JKleinRot 15:3ebd0e666a9c 559
JKleinRot 19:38c9d177b6ee 560 while(state == BT) {
JKleinRot 17:c694a0e63a89 561 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 562 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 563
JKleinRot 22:838a17065bc7 564 EMG_meten();
JKleinRot 17:c694a0e63a89 565
JKleinRot 25:71e52c56be13 566 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 567 state = BTB; //Ga door naar state BTB
JKleinRot 17:c694a0e63a89 568 }
JKleinRot 25:71e52c56be13 569 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 570 state = BTT; //Ga door naar state BTT
JKleinRot 17:c694a0e63a89 571 }
JKleinRot 17:c694a0e63a89 572 }
JKleinRot 19:38c9d177b6ee 573 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 574 }
JKleinRot 19:38c9d177b6ee 575
JKleinRot 25:71e52c56be13 576 case TB: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 577 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 578 lcd.printf(" TB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 579 pc.printf("TB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 580
JKleinRot 25:71e52c56be13 581 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 582 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 583
JKleinRot 26:438a498e5526 584 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 585 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 586 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 587
JKleinRot 25:71e52c56be13 588 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 589 }
JKleinRot 17:c694a0e63a89 590
JKleinRot 19:38c9d177b6ee 591 while(state == TB) {
JKleinRot 17:c694a0e63a89 592 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 593 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 594
JKleinRot 25:71e52c56be13 595 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 596
JKleinRot 25:71e52c56be13 597 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 598 state = TBB; //Ga door naar state TBB
JKleinRot 17:c694a0e63a89 599 }
JKleinRot 25:71e52c56be13 600 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 601 state = TBT; //Ga door naar state TBT
JKleinRot 17:c694a0e63a89 602 }
JKleinRot 17:c694a0e63a89 603 }
JKleinRot 19:38c9d177b6ee 604 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 605 }
JKleinRot 17:c694a0e63a89 606
JKleinRot 25:71e52c56be13 607 case TT: { //Derde keuze maken voor doel
JKleinRot 17:c694a0e63a89 608 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 609 lcd.printf(" TT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 610 pc.printf("TT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 611
JKleinRot 25:71e52c56be13 612 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 613 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 614
JKleinRot 26:438a498e5526 615 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 616 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 617 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 618
JKleinRot 25:71e52c56be13 619 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 620 }
JKleinRot 17:c694a0e63a89 621
JKleinRot 19:38c9d177b6ee 622 while(state == TT) {
JKleinRot 17:c694a0e63a89 623 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 624 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 625
JKleinRot 25:71e52c56be13 626 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 627
JKleinRot 25:71e52c56be13 628 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 629 state = TTB; //Ga door naar state TTB
JKleinRot 17:c694a0e63a89 630 }
JKleinRot 25:71e52c56be13 631 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 632 state = TTT; //Ga door naar state TTT
JKleinRot 17:c694a0e63a89 633 }
JKleinRot 17:c694a0e63a89 634 }
JKleinRot 19:38c9d177b6ee 635 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 636 }
JKleinRot 19:38c9d177b6ee 637
JKleinRot 25:71e52c56be13 638 case BBB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 639 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 640 lcd.printf(" BBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 641 pc.printf("BBB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 642
JKleinRot 25:71e52c56be13 643 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 644 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 645
JKleinRot 26:438a498e5526 646 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 647 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 648 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 649
JKleinRot 25:71e52c56be13 650 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 651 }
JKleinRot 17:c694a0e63a89 652
JKleinRot 19:38c9d177b6ee 653 while(state == BBB) {
JKleinRot 17:c694a0e63a89 654 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 655 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 656
JKleinRot 25:71e52c56be13 657 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 658
JKleinRot 25:71e52c56be13 659 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 660 state = BBBB; //Ga door naar state BBBB
JKleinRot 17:c694a0e63a89 661 }
JKleinRot 25:71e52c56be13 662 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 663 state = BBBT; //Ga door naar state BBBT
JKleinRot 17:c694a0e63a89 664 }
JKleinRot 17:c694a0e63a89 665 }
JKleinRot 19:38c9d177b6ee 666 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 667 }
JKleinRot 17:c694a0e63a89 668
JKleinRot 19:38c9d177b6ee 669
JKleinRot 25:71e52c56be13 670 case BBT: { //Vierde keuze maken in doel
JKleinRot 17:c694a0e63a89 671 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 672 lcd.printf(" BBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 673 pc.printf("BBT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 674
JKleinRot 25:71e52c56be13 675 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 676 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 677
JKleinRot 26:438a498e5526 678 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 679 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 680 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 681
JKleinRot 25:71e52c56be13 682 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 683 }
JKleinRot 17:c694a0e63a89 684
JKleinRot 19:38c9d177b6ee 685 while(state == BBT) {
JKleinRot 17:c694a0e63a89 686 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 687 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 688
JKleinRot 25:71e52c56be13 689 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 690
JKleinRot 25:71e52c56be13 691 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 692 state = BBTB; //Ga door naar state BBTB
JKleinRot 17:c694a0e63a89 693 }
JKleinRot 25:71e52c56be13 694 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 695 state = BBTT; //Ga door naar state BBTT
JKleinRot 17:c694a0e63a89 696 }
JKleinRot 17:c694a0e63a89 697 }
JKleinRot 19:38c9d177b6ee 698 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 699 }
JKleinRot 19:38c9d177b6ee 700
JKleinRot 25:71e52c56be13 701 case BTB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 702 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 703 lcd.printf(" BTB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 704 pc.printf("BTB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 705
JKleinRot 25:71e52c56be13 706 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 707 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 708
JKleinRot 26:438a498e5526 709 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 710 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 711 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 712
JKleinRot 25:71e52c56be13 713 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 714 }
JKleinRot 17:c694a0e63a89 715
JKleinRot 19:38c9d177b6ee 716 while(state == BTB) {
JKleinRot 17:c694a0e63a89 717 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 718 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 719
JKleinRot 25:71e52c56be13 720 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 721
JKleinRot 25:71e52c56be13 722 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 723 state = BTBB; //Ga door naar state BTBB
JKleinRot 17:c694a0e63a89 724 }
JKleinRot 25:71e52c56be13 725 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 726 state = BTBT; //Ga door naar state BTBT
JKleinRot 17:c694a0e63a89 727 }
JKleinRot 17:c694a0e63a89 728 }
JKleinRot 19:38c9d177b6ee 729 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 730 }
JKleinRot 19:38c9d177b6ee 731
JKleinRot 25:71e52c56be13 732 case BTT: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 733 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 734 lcd.printf(" BTT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 735 pc.printf("BTT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 736
JKleinRot 25:71e52c56be13 737 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 738 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 739
JKleinRot 26:438a498e5526 740 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 741 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 742 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 743
JKleinRot 25:71e52c56be13 744 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 745 }
JKleinRot 17:c694a0e63a89 746
JKleinRot 19:38c9d177b6ee 747 while(state == BTT) {
JKleinRot 17:c694a0e63a89 748 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 749 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 750
JKleinRot 25:71e52c56be13 751 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 752
JKleinRot 25:71e52c56be13 753 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 754 state = BTTB; //Ga door naar state BTTB
JKleinRot 17:c694a0e63a89 755 }
JKleinRot 25:71e52c56be13 756 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 757 state = BTTT; //Ga door naar state BTTT
JKleinRot 17:c694a0e63a89 758 }
JKleinRot 17:c694a0e63a89 759 }
JKleinRot 19:38c9d177b6ee 760 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 761 }
JKleinRot 19:38c9d177b6ee 762
JKleinRot 25:71e52c56be13 763 case TBB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 764 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 765 lcd.printf(" TBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 766 pc.printf("TBB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 767
JKleinRot 25:71e52c56be13 768 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 769 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 770
JKleinRot 26:438a498e5526 771 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 772 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 773 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 774
JKleinRot 25:71e52c56be13 775 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 776 }
JKleinRot 17:c694a0e63a89 777
JKleinRot 19:38c9d177b6ee 778 while(state == TBB) {
JKleinRot 17:c694a0e63a89 779 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 780 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 781
JKleinRot 25:71e52c56be13 782 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 783
JKleinRot 25:71e52c56be13 784 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 785 state = TBBB; //Ga door naar state TBBB
JKleinRot 17:c694a0e63a89 786 }
JKleinRot 25:71e52c56be13 787 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 788 state = TBBT; //Ga door naar state TBBT
JKleinRot 17:c694a0e63a89 789 }
JKleinRot 17:c694a0e63a89 790 }
JKleinRot 19:38c9d177b6ee 791 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 792 }
JKleinRot 19:38c9d177b6ee 793
JKleinRot 25:71e52c56be13 794 case TBT: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 795 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 796 lcd.printf(" TBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 797 pc.printf("TBT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 798
JKleinRot 25:71e52c56be13 799 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 800 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 801
JKleinRot 26:438a498e5526 802 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 803 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 804 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 805
JKleinRot 25:71e52c56be13 806 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 807 }
JKleinRot 17:c694a0e63a89 808
JKleinRot 19:38c9d177b6ee 809 while(state == TBT) {
JKleinRot 17:c694a0e63a89 810 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 811 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 812
JKleinRot 25:71e52c56be13 813 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 814
JKleinRot 26:438a498e5526 815 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 816 state = TBTB; //Ga door naar state TBTB
JKleinRot 17:c694a0e63a89 817 }
JKleinRot 26:438a498e5526 818 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 819 state = TBTT; //Ga door naar state TBTT
JKleinRot 17:c694a0e63a89 820 }
JKleinRot 17:c694a0e63a89 821 }
JKleinRot 19:38c9d177b6ee 822 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 823 }
JKleinRot 19:38c9d177b6ee 824
JKleinRot 25:71e52c56be13 825 case TTB: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 826 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 22:838a17065bc7 827 lcd.printf(" TTB "); //Tekst op LCD scherm
JKleinRot 22:838a17065bc7 828 pc.printf("TTB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 829
JKleinRot 25:71e52c56be13 830 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 831 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 832
JKleinRot 26:438a498e5526 833 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 834 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 835 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 836
JKleinRot 25:71e52c56be13 837 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 838 }
JKleinRot 17:c694a0e63a89 839
JKleinRot 22:838a17065bc7 840 while(state == TTB) {
JKleinRot 17:c694a0e63a89 841 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 842 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 843
JKleinRot 25:71e52c56be13 844 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 845
JKleinRot 26:438a498e5526 846 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 847 state = TTBB; //Ga door naar state TTBB
JKleinRot 17:c694a0e63a89 848 }
JKleinRot 26:438a498e5526 849 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 850 state = TTBT; //Ga door naar state TTBT
JKleinRot 17:c694a0e63a89 851 }
JKleinRot 17:c694a0e63a89 852 }
JKleinRot 19:38c9d177b6ee 853 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 854 }
JKleinRot 19:38c9d177b6ee 855
JKleinRot 25:71e52c56be13 856 case TTT: { //Vierde keuze maken voor doel
JKleinRot 17:c694a0e63a89 857 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 858 lcd.printf(" TTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 859 pc.printf("TTT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 860
JKleinRot 25:71e52c56be13 861 EMG.reset(); //Timer resetten
JKleinRot 25:71e52c56be13 862 EMG.start(); //Timer aanzetten voor EMG meten
JKleinRot 23:5267c928ae2b 863
JKleinRot 26:438a498e5526 864 while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten
JKleinRot 22:838a17065bc7 865 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 866 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 867
JKleinRot 25:71e52c56be13 868 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 22:838a17065bc7 869 }
JKleinRot 17:c694a0e63a89 870
JKleinRot 19:38c9d177b6ee 871 while(state == TTT) {
JKleinRot 17:c694a0e63a89 872 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 873 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 874
JKleinRot 25:71e52c56be13 875 EMG_meten(); //EMG meten van biceps en triceps
JKleinRot 17:c694a0e63a89 876
JKleinRot 26:438a498e5526 877 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 878 state = TTTB; //Ga door naar state TTTB
JKleinRot 17:c694a0e63a89 879 }
JKleinRot 26:438a498e5526 880 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 881 state = TTTT; //Ga door naar state TTTT
JKleinRot 17:c694a0e63a89 882 }
JKleinRot 17:c694a0e63a89 883 }
JKleinRot 19:38c9d177b6ee 884 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 885 }
JKleinRot 19:38c9d177b6ee 886
JKleinRot 25:71e52c56be13 887 case BBBB: { //Motoraansturing voor doel rechtsonder
JKleinRot 17:c694a0e63a89 888 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 889 lcd.printf(" BBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 890 pc.printf("BBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 891
JKleinRot 19:38c9d177b6ee 892 while(state == BBBB) {
JKleinRot 25:71e52c56be13 893 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 894 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 895
JKleinRot 25:71e52c56be13 896 while(puls_motor_arm1.getPosition() > -84) {
JKleinRot 26:438a498e5526 897 referentie_arm1 = referentie_arm1 - 264.0/200.0; //Referentie arm 1 loopt af in een seconden naar de gewenste waarde
JKleinRot 26:438a498e5526 898 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 899 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 900 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 901 }
JKleinRot 29:f95f0cc84349 902
JKleinRot 29:f95f0cc84349 903 if(puls_motor_arm1.getPosition() <= -84) {
JKleinRot 27:5ac1fc1005d7 904 referentie_arm1 = -84;
JKleinRot 27:5ac1fc1005d7 905 }
JKleinRot 25:71e52c56be13 906
JKleinRot 29:f95f0cc84349 907
JKleinRot 29:f95f0cc84349 908
JKleinRot 25:71e52c56be13 909 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 910 referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde
JKleinRot 26:438a498e5526 911 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 912 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 913 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 914 }
JKleinRot 29:f95f0cc84349 915 ticker_motor_arm2_pid.detach();
JKleinRot 29:f95f0cc84349 916 pwm_to_motor_arm2 = 1;
JKleinRot 29:f95f0cc84349 917 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 29:f95f0cc84349 918 dir_motor_arm2.write(0);
JKleinRot 25:71e52c56be13 919 while(puls_motor_arm2.getPosition() > 36) {
JKleinRot 25:71e52c56be13 920
JKleinRot 26:438a498e5526 921 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 922 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 923 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 924 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 925 }
JKleinRot 29:f95f0cc84349 926 pwm_to_motor_arm2 = 0.5;
JKleinRot 29:f95f0cc84349 927 pwm_motor_arm2.write(pwm_to_motor_arm2);
JKleinRot 29:f95f0cc84349 928 dir_motor_arm2.write(1);
JKleinRot 29:f95f0cc84349 929 while(puls_motor_arm2.getPosition() <= 211) {
JKleinRot 25:71e52c56be13 930
JKleinRot 29:f95f0cc84349 931 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 29:f95f0cc84349 932 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 29:f95f0cc84349 933 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 29:f95f0cc84349 934 }
JKleinRot 29:f95f0cc84349 935 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
JKleinRot 29:f95f0cc84349 936 pc.printf("staat stil");
JKleinRot 25:71e52c56be13 937
JKleinRot 29:f95f0cc84349 938 state = THUISPOSITIE_RECHTS; //Door naar de volgende state
JKleinRot 29:f95f0cc84349 939
JKleinRot 19:38c9d177b6ee 940 }
JKleinRot 29:f95f0cc84349 941
JKleinRot 29:f95f0cc84349 942
JKleinRot 19:38c9d177b6ee 943 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 944 }
JKleinRot 19:38c9d177b6ee 945
JKleinRot 25:71e52c56be13 946 case BBBT: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 947 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 948 lcd.printf(" BBBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 949 pc.printf("BBBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 950 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 951 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 952
JKleinRot 19:38c9d177b6ee 953 while(state == BBBT) {
JKleinRot 26:438a498e5526 954 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 955 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 956 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 957 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 958 }
JKleinRot 19:38c9d177b6ee 959 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 960 }
JKleinRot 19:38c9d177b6ee 961
JKleinRot 25:71e52c56be13 962 case BBTB: { //Motoraansturing voor doel rechtsmidden
JKleinRot 17:c694a0e63a89 963 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 964 lcd.printf(" BBTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 965 pc.printf("BBTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 966
JKleinRot 19:38c9d177b6ee 967 while(state == BBTB) {
JKleinRot 25:71e52c56be13 968 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 969 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 970
JKleinRot 25:71e52c56be13 971 while(puls_motor_arm1.getPosition() > -84) {
JKleinRot 26:438a498e5526 972 referentie_arm1 = referentie_arm1 - 264.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 973 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 974 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 975 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 976 }
JKleinRot 29:f95f0cc84349 977
JKleinRot 29:f95f0cc84349 978 if(puls_motor_arm1.getPosition() <= -84) {
JKleinRot 27:5ac1fc1005d7 979 referentie_arm1 = -84;
JKleinRot 27:5ac1fc1005d7 980 }
JKleinRot 25:71e52c56be13 981
JKleinRot 25:71e52c56be13 982 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 983 referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 984 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 985 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 986 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 987 }
JKleinRot 25:71e52c56be13 988
JKleinRot 25:71e52c56be13 989 while(puls_motor_arm2.getPosition() > 36) {
JKleinRot 26:438a498e5526 990 referentie_arm2 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch af
JKleinRot 26:438a498e5526 991 t = t + 0.005; //Tijd loopt op met 0.005 per flag
JKleinRot 25:71e52c56be13 992
JKleinRot 26:438a498e5526 993 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 994 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 995 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 996 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 997 }
JKleinRot 25:71e52c56be13 998
JKleinRot 25:71e52c56be13 999 if(puls_motor_arm2.getPosition() <= 36) {
JKleinRot 26:438a498e5526 1000 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1001 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 1002 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 25:71e52c56be13 1003
JKleinRot 26:438a498e5526 1004 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1005 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1006 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1007 }
JKleinRot 25:71e52c56be13 1008
JKleinRot 25:71e52c56be13 1009 if(puls_motor_arm2.getPosition() >= 211) {
JKleinRot 26:438a498e5526 1010 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1011 state = THUISPOSITIE_RECHTS; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1012 }
JKleinRot 25:71e52c56be13 1013 }
JKleinRot 19:38c9d177b6ee 1014 }
JKleinRot 19:38c9d177b6ee 1015 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1016 }
JKleinRot 19:38c9d177b6ee 1017
JKleinRot 25:71e52c56be13 1018 case BBTT: { //Motoraansturing voor doel rechtsboven
JKleinRot 17:c694a0e63a89 1019 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1020 lcd.printf(" BBTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1021 pc.printf("BBTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 1022
JKleinRot 19:38c9d177b6ee 1023 while(state == BBTT) {
JKleinRot 25:71e52c56be13 1024 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 1025 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1026
JKleinRot 25:71e52c56be13 1027 while(puls_motor_arm1.getPosition() > -84) {
JKleinRot 27:5ac1fc1005d7 1028 referentie_arm1 = referentie_arm1 - 259.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1029 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1030 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1031 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1032 }
JKleinRot 29:f95f0cc84349 1033
JKleinRot 29:f95f0cc84349 1034 if(puls_motor_arm1.getPosition() <= -84) {
JKleinRot 27:5ac1fc1005d7 1035 referentie_arm1 = -84;
JKleinRot 27:5ac1fc1005d7 1036 }
JKleinRot 25:71e52c56be13 1037
JKleinRot 25:71e52c56be13 1038 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 1039 referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1040 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1041 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1042 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1043 }
JKleinRot 25:71e52c56be13 1044
JKleinRot 25:71e52c56be13 1045 while(puls_motor_arm2.getPosition() > 36) {
JKleinRot 26:438a498e5526 1046 referentie_arm2 = -0.5 * 200000 * t * t; //Referentie arm 2 loopt parabolisch af
JKleinRot 26:438a498e5526 1047 t = t + 0.005; //Tijd loopt op met 0.005 iedere flag
JKleinRot 25:71e52c56be13 1048
JKleinRot 26:438a498e5526 1049 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1050 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1051 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1052 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1053 }
JKleinRot 25:71e52c56be13 1054
JKleinRot 25:71e52c56be13 1055 if(puls_motor_arm2.getPosition() <= 36) {
JKleinRot 26:438a498e5526 1056 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1057 while(puls_motor_arm2.getPosition() < 211) {
JKleinRot 26:438a498e5526 1058 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 25:71e52c56be13 1059
JKleinRot 26:438a498e5526 1060 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1061 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1062 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1063 }
JKleinRot 25:71e52c56be13 1064
JKleinRot 25:71e52c56be13 1065 if(puls_motor_arm2.getPosition() >= 211) {
JKleinRot 26:438a498e5526 1066 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1067 state = THUISPOSITIE_RECHTS; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1068 }
JKleinRot 25:71e52c56be13 1069 }
JKleinRot 19:38c9d177b6ee 1070 }
JKleinRot 19:38c9d177b6ee 1071 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1072 }
JKleinRot 19:38c9d177b6ee 1073
JKleinRot 25:71e52c56be13 1074 case BTBB: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1075 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1076 lcd.printf(" BTBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1077 pc.printf("BTBB\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1078 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1079 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1080
JKleinRot 19:38c9d177b6ee 1081 while(state == BTBB) {
JKleinRot 26:438a498e5526 1082 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1083 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1084 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1085 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1086 }
JKleinRot 19:38c9d177b6ee 1087 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1088 }
JKleinRot 19:38c9d177b6ee 1089
JKleinRot 25:71e52c56be13 1090 case BTBT: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1091 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1092 lcd.printf(" BTBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1093 pc.printf("BTBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1094 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1095 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1096
JKleinRot 19:38c9d177b6ee 1097 while(state == BTBT) {
JKleinRot 26:438a498e5526 1098 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1099 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1100 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1101 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1102 }
JKleinRot 19:38c9d177b6ee 1103 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1104 }
JKleinRot 19:38c9d177b6ee 1105
JKleinRot 25:71e52c56be13 1106 case BTTB: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1107 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1108 lcd.printf(" BTTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1109 pc.printf("BTTB\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1110 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1111 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1112
JKleinRot 19:38c9d177b6ee 1113 while(state == BTTB) {
JKleinRot 26:438a498e5526 1114 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1115 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1116 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1117 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1118 }
JKleinRot 19:38c9d177b6ee 1119 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1120 }
JKleinRot 19:38c9d177b6ee 1121
JKleinRot 25:71e52c56be13 1122 case BTTT: { //Geen motoraansturing
JKleinRot 17:c694a0e63a89 1123 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1124 lcd.printf(" BTTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1125 pc.printf("BTTT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1126 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1127 lcd.cls(); //LCD scherm leegmaken
JKleinRot 17:c694a0e63a89 1128
JKleinRot 19:38c9d177b6ee 1129 while(state == BTTT) {
JKleinRot 26:438a498e5526 1130 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1131 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1132 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1133 state = WACHT; //Door naar de volgende state
JKleinRot 19:38c9d177b6ee 1134 }
JKleinRot 19:38c9d177b6ee 1135 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 1136 }
JKleinRot 19:38c9d177b6ee 1137
JKleinRot 25:71e52c56be13 1138 case TBBB: { //Motoraansturing voor doel middenonder
JKleinRot 17:c694a0e63a89 1139 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1140 lcd.printf(" TBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1141 pc.printf("TBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 1142
JKleinRot 19:38c9d177b6ee 1143 while(state == TBBB) {
JKleinRot 19:38c9d177b6ee 1144 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 1145 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 19:38c9d177b6ee 1146
JKleinRot 23:5267c928ae2b 1147 while(puls_motor_arm1.getPosition() > 48) {
JKleinRot 26:438a498e5526 1148 referentie_arm1 = referentie_arm1 - 132.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1149 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1150 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1151 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 23:5267c928ae2b 1152 }
JKleinRot 29:f95f0cc84349 1153
JKleinRot 29:f95f0cc84349 1154 if(puls_motor_arm1.getPosition() <= 48) {
JKleinRot 27:5ac1fc1005d7 1155 referentie_arm1 = 48;
JKleinRot 27:5ac1fc1005d7 1156 }
JKleinRot 19:38c9d177b6ee 1157
JKleinRot 23:5267c928ae2b 1158 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1159 referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1160 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1161 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1162 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 23:5267c928ae2b 1163 }
JKleinRot 23:5267c928ae2b 1164
JKleinRot 25:71e52c56be13 1165 while(puls_motor_arm2.getPosition() > -8) {
JKleinRot 26:438a498e5526 1166 referentie_arm2 = -0.5 * 100000 * t * t; //Referentie arm 2 loopt parabolisch af
JKleinRot 26:438a498e5526 1167 t = t + 0.005; //Tijd loopt op met 0.005 per flag
JKleinRot 23:5267c928ae2b 1168
JKleinRot 26:438a498e5526 1169 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1170 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1171 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1172 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1173 }
JKleinRot 23:5267c928ae2b 1174
JKleinRot 25:71e52c56be13 1175 if(puls_motor_arm2.getPosition() <= -8) {
JKleinRot 26:438a498e5526 1176 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1177 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1178 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 23:5267c928ae2b 1179
JKleinRot 26:438a498e5526 1180 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1181 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1182 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 23:5267c928ae2b 1183 }
JKleinRot 23:5267c928ae2b 1184
JKleinRot 25:71e52c56be13 1185 if(puls_motor_arm2.getPosition() >= 167) {
JKleinRot 26:438a498e5526 1186 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1187 state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
JKleinRot 21:f4e9f6c28de1 1188 }
JKleinRot 19:38c9d177b6ee 1189 }
JKleinRot 19:38c9d177b6ee 1190 }
JKleinRot 24:a1fdc830f96c 1191 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1192 }
JKleinRot 23:5267c928ae2b 1193
JKleinRot 25:71e52c56be13 1194 case TBBT: { //Geen motoraansturing
JKleinRot 25:71e52c56be13 1195 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 25:71e52c56be13 1196 lcd.printf(" TBBT "); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 1197 pc.printf("TBBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1198 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1199 lcd.cls(); //LCD scherm leegmaken
JKleinRot 25:71e52c56be13 1200
JKleinRot 25:71e52c56be13 1201 while(state == TBBT) {
JKleinRot 26:438a498e5526 1202 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1203 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1204 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1205 state = WACHT; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1206 }
JKleinRot 25:71e52c56be13 1207 break; //Stop met alle acties in deze case
JKleinRot 25:71e52c56be13 1208 }
JKleinRot 25:71e52c56be13 1209
JKleinRot 25:71e52c56be13 1210 case TBTB: { //Motoraansturing voor doel midden
JKleinRot 25:71e52c56be13 1211 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 25:71e52c56be13 1212 lcd.printf(" TBTB "); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 1213 pc.printf("TBTB\n\r"); //Controle naar pc sturen
JKleinRot 25:71e52c56be13 1214
JKleinRot 25:71e52c56be13 1215 while(state == TBTB) {
JKleinRot 25:71e52c56be13 1216 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 1217 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1218
JKleinRot 25:71e52c56be13 1219 while(puls_motor_arm1.getPosition() > 48) {
JKleinRot 26:438a498e5526 1220 referentie_arm1 = referentie_arm1 - 132.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1221 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1222 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1223 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1224 }
JKleinRot 29:f95f0cc84349 1225
JKleinRot 29:f95f0cc84349 1226 if(puls_motor_arm1.getPosition() <= 48) {
JKleinRot 27:5ac1fc1005d7 1227 referentie_arm1 = 48;
JKleinRot 27:5ac1fc1005d7 1228 }
JKleinRot 25:71e52c56be13 1229
JKleinRot 25:71e52c56be13 1230 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1231 referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie
JKleinRot 26:438a498e5526 1232 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1233 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1234 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1235 }
JKleinRot 25:71e52c56be13 1236
JKleinRot 25:71e52c56be13 1237 while(puls_motor_arm2.getPosition() > -8) {
JKleinRot 26:438a498e5526 1238 referentie_arm2 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch op
JKleinRot 26:438a498e5526 1239 t = t + 0.005; //Tijd loopt op met 0.005 per flag
JKleinRot 25:71e52c56be13 1240
JKleinRot 26:438a498e5526 1241 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1242 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1243 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1244 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1245 }
JKleinRot 25:71e52c56be13 1246
JKleinRot 25:71e52c56be13 1247 if(puls_motor_arm2.getPosition() <= -8) {
JKleinRot 26:438a498e5526 1248 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1249 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1250 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie
JKleinRot 25:71e52c56be13 1251
JKleinRot 26:438a498e5526 1252 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1253 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1254 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1255 }
JKleinRot 25:71e52c56be13 1256
JKleinRot 25:71e52c56be13 1257 if(puls_motor_arm2.getPosition() >= 167) {
JKleinRot 26:438a498e5526 1258 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1259 state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1260 }
JKleinRot 25:71e52c56be13 1261 }
JKleinRot 25:71e52c56be13 1262 }
JKleinRot 25:71e52c56be13 1263 break; //Stop met alle acties in deze case
JKleinRot 25:71e52c56be13 1264 }
JKleinRot 25:71e52c56be13 1265
JKleinRot 25:71e52c56be13 1266 case TBTT: { //Motoraansturing voor doel middenboven
JKleinRot 25:71e52c56be13 1267 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 25:71e52c56be13 1268 lcd.printf(" TBTT "); //Tekst op LCD scherm
JKleinRot 25:71e52c56be13 1269 pc.printf("TBTT\n\r"); //Controle naar pc sturen
JKleinRot 25:71e52c56be13 1270
JKleinRot 25:71e52c56be13 1271 while(state == TBTT) {
JKleinRot 25:71e52c56be13 1272 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 1273 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1274
JKleinRot 25:71e52c56be13 1275 while(puls_motor_arm1.getPosition() > 48) {
JKleinRot 26:438a498e5526 1276 referentie_arm1 = referentie_arm1 - 132.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste positie
JKleinRot 26:438a498e5526 1277 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1278 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1279 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1280 }
JKleinRot 29:f95f0cc84349 1281
JKleinRot 29:f95f0cc84349 1282 if(puls_motor_arm1.getPosition() <= 48) {
JKleinRot 27:5ac1fc1005d7 1283 referentie_arm1 = 48;
JKleinRot 27:5ac1fc1005d7 1284 }
JKleinRot 25:71e52c56be13 1285
JKleinRot 25:71e52c56be13 1286 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1287 referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie
JKleinRot 26:438a498e5526 1288 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1289 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1290 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1291 }
JKleinRot 25:71e52c56be13 1292
JKleinRot 25:71e52c56be13 1293 while(puls_motor_arm2.getPosition() > -8) {
JKleinRot 26:438a498e5526 1294 referentie_arm2 = -0.5 * 200000 * t * t; //Referentie arm 2 loopt parabolisch af
JKleinRot 26:438a498e5526 1295 t = t + 0.005; //Tijd loopt op met 0.005 per flag
JKleinRot 25:71e52c56be13 1296
JKleinRot 26:438a498e5526 1297 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1298 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1299 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1300 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 25:71e52c56be13 1301 }
JKleinRot 25:71e52c56be13 1302
JKleinRot 25:71e52c56be13 1303 if(puls_motor_arm2.getPosition() <= -8) {
JKleinRot 26:438a498e5526 1304 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1305 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 26:438a498e5526 1306 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 25:71e52c56be13 1307
JKleinRot 26:438a498e5526 1308 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1309 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1310 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1311 }
JKleinRot 25:71e52c56be13 1312
JKleinRot 25:71e52c56be13 1313 if(puls_motor_arm2.getPosition() >= 167) {
JKleinRot 26:438a498e5526 1314 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1315 state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
JKleinRot 25:71e52c56be13 1316 }
JKleinRot 25:71e52c56be13 1317 }
JKleinRot 25:71e52c56be13 1318 }
JKleinRot 25:71e52c56be13 1319 break; //Stop met alle acties in deze case
JKleinRot 25:71e52c56be13 1320 }
JKleinRot 25:71e52c56be13 1321
JKleinRot 25:71e52c56be13 1322 case TTBB: { //Motoraansturing voor doel linksonder
JKleinRot 24:a1fdc830f96c 1323 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1324 lcd.printf(" TTBB "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1325 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1326
JKleinRot 24:a1fdc830f96c 1327 while(state == TTBB) {
JKleinRot 24:a1fdc830f96c 1328 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 1329 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1330
JKleinRot 26:438a498e5526 1331 referentie_arm2 = -0.5 * 100000 * t * t; //Referentie arm 2 loopt parabolisch af
JKleinRot 26:438a498e5526 1332 t = t + 0.005; //Tijd loopt op met 0.005 per flag
JKleinRot 24:a1fdc830f96c 1333
JKleinRot 26:438a498e5526 1334 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1335 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1336 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1337 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 24:a1fdc830f96c 1338
JKleinRot 25:71e52c56be13 1339 if(puls_motor_arm2.getPosition() <= -175) {
JKleinRot 26:438a498e5526 1340 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1341 while(puls_motor_arm2.getPosition() < 0) {
JKleinRot 26:438a498e5526 1342 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 24:a1fdc830f96c 1343
JKleinRot 26:438a498e5526 1344 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1345 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1346 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1347 }
JKleinRot 24:a1fdc830f96c 1348
JKleinRot 25:71e52c56be13 1349 if(puls_motor_arm2.getPosition() >= 0) {
JKleinRot 26:438a498e5526 1350 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1351 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1352 }
JKleinRot 23:5267c928ae2b 1353 }
JKleinRot 24:a1fdc830f96c 1354 }
JKleinRot 24:a1fdc830f96c 1355 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1356 }
JKleinRot 23:5267c928ae2b 1357
JKleinRot 25:71e52c56be13 1358 case TTBT: { //Geen motoraansturing
JKleinRot 24:a1fdc830f96c 1359 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1360 lcd.printf(" TTBT "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1361 pc.printf("TTBT\n\r"); //Controle naar pc sturen
JKleinRot 26:438a498e5526 1362 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1363 lcd.cls(); //LCD scherm leegmaken
JKleinRot 24:a1fdc830f96c 1364
JKleinRot 24:a1fdc830f96c 1365 while(state == TTBT) {
JKleinRot 26:438a498e5526 1366 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1367 lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm
JKleinRot 26:438a498e5526 1368 wait(1); //Een seconde wachten
JKleinRot 26:438a498e5526 1369 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1370 }
JKleinRot 24:a1fdc830f96c 1371 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1372 }
JKleinRot 24:a1fdc830f96c 1373
JKleinRot 25:71e52c56be13 1374 case TTTB: { //Motoraansturing voor doel linksmidden
JKleinRot 24:a1fdc830f96c 1375 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1376 lcd.printf(" TTTB "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1377 pc.printf("TTTB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1378
JKleinRot 24:a1fdc830f96c 1379 while(state == TTTB) {
JKleinRot 24:a1fdc830f96c 1380
JKleinRot 24:a1fdc830f96c 1381 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 1382 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1383
JKleinRot 26:438a498e5526 1384 referentie_arm2 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch af
JKleinRot 26:438a498e5526 1385 t = t + 0.005; //Tijd loopt op met 0.005 per flag
JKleinRot 24:a1fdc830f96c 1386
JKleinRot 26:438a498e5526 1387 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1388 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1389 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1390 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 24:a1fdc830f96c 1391
JKleinRot 25:71e52c56be13 1392 if(puls_motor_arm2.getPosition() <= -175) {
JKleinRot 26:438a498e5526 1393 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1394 while(puls_motor_arm2.getPosition() < 0) {
JKleinRot 26:438a498e5526 1395 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 24:a1fdc830f96c 1396
JKleinRot 26:438a498e5526 1397 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1398 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1399 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1400 }
JKleinRot 24:a1fdc830f96c 1401
JKleinRot 25:71e52c56be13 1402 if(puls_motor_arm2.getPosition() >= 0) {
JKleinRot 26:438a498e5526 1403 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1404 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1405 }
JKleinRot 23:5267c928ae2b 1406 }
JKleinRot 23:5267c928ae2b 1407 }
JKleinRot 19:38c9d177b6ee 1408
JKleinRot 24:a1fdc830f96c 1409 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1410 }
JKleinRot 24:a1fdc830f96c 1411
JKleinRot 25:71e52c56be13 1412 case TTTT: { //Motoraansturing voor doel linksboven
JKleinRot 24:a1fdc830f96c 1413 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1414 lcd.printf(" TTTT "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1415 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1416
JKleinRot 24:a1fdc830f96c 1417 while(state == TTTT) {
JKleinRot 24:a1fdc830f96c 1418 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 1419 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1420
JKleinRot 26:438a498e5526 1421 referentie_arm2 = -0.5 * 200000 * t * t; //Referentie arm 2 loopt parabolisch af
JKleinRot 26:438a498e5526 1422 t = t + 0.005; //Tijd loopt op met 0.005 per flag
JKleinRot 28:e2f5cee5e73b 1423 pwm_motor_arm1 = 0.075;
JKleinRot 24:a1fdc830f96c 1424
JKleinRot 26:438a498e5526 1425 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1426 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1427 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 26:438a498e5526 1428 pc.printf("t is %f\n\r", t); //T naar pc sturen
JKleinRot 24:a1fdc830f96c 1429
JKleinRot 25:71e52c56be13 1430 if(puls_motor_arm2.getPosition() <= -175) {
JKleinRot 26:438a498e5526 1431 wait(1); //Een seconde wachten
JKleinRot 25:71e52c56be13 1432 while(puls_motor_arm2.getPosition() < 0) {
JKleinRot 26:438a498e5526 1433 referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
JKleinRot 24:a1fdc830f96c 1434
JKleinRot 26:438a498e5526 1435 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1436 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1437 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1438 }
JKleinRot 24:a1fdc830f96c 1439
JKleinRot 25:71e52c56be13 1440 if(puls_motor_arm2.getPosition() >= 0) {
JKleinRot 26:438a498e5526 1441 pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
JKleinRot 26:438a498e5526 1442 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1443 }
JKleinRot 24:a1fdc830f96c 1444 }
JKleinRot 24:a1fdc830f96c 1445 }
JKleinRot 27:5ac1fc1005d7 1446 break;
JKleinRot 24:a1fdc830f96c 1447 }
JKleinRot 24:a1fdc830f96c 1448
JKleinRot 26:438a498e5526 1449 case THUISPOSITIE_MIDDEN: { //Terug naar gekalibreerde positie
JKleinRot 24:a1fdc830f96c 1450 while(puls_motor_arm2.getPosition() > 123) {
JKleinRot 25:71e52c56be13 1451 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 1452 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1453
JKleinRot 26:438a498e5526 1454 referentie_arm2 = referentie_arm2 - 44.0/200.0; //Referentie arm 2 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1455 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 28:e2f5cee5e73b 1456 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1457 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1458 }
JKleinRot 24:a1fdc830f96c 1459
JKleinRot 24:a1fdc830f96c 1460 while(puls_motor_arm1.getPosition() < 180) {
JKleinRot 25:71e52c56be13 1461 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 1462 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1463
JKleinRot 26:438a498e5526 1464 referentie_arm1 = referentie_arm1 + 132.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1465 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1466 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1467 pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 24:a1fdc830f96c 1468 }
JKleinRot 24:a1fdc830f96c 1469
JKleinRot 26:438a498e5526 1470 state = WACHT; //Door naar de volgende state
JKleinRot 24:a1fdc830f96c 1471 break;
JKleinRot 24:a1fdc830f96c 1472 }
JKleinRot 24:a1fdc830f96c 1473
JKleinRot 26:438a498e5526 1474 case THUISPOSITIE_RECHTS: { //Terug naar gekalibreerde positie
JKleinRot 25:71e52c56be13 1475 while(puls_motor_arm2.getPosition() > 123) {
JKleinRot 25:71e52c56be13 1476 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 1477 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1478
JKleinRot 26:438a498e5526 1479 referentie_arm2 = referentie_arm2 - 88.0/200.0; //Referentie arm 2 loopt af in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1480 pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1481 pc.printf("get position %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1482 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1483 }
JKleinRot 25:71e52c56be13 1484
JKleinRot 27:5ac1fc1005d7 1485 while(puls_motor_arm1.getPosition() < 175) {
JKleinRot 25:71e52c56be13 1486 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 1487 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 25:71e52c56be13 1488
JKleinRot 27:5ac1fc1005d7 1489 referentie_arm1 = referentie_arm1 + 259.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde
JKleinRot 26:438a498e5526 1490 pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen
JKleinRot 26:438a498e5526 1491 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen
JKleinRot 26:438a498e5526 1492 pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen
JKleinRot 25:71e52c56be13 1493 }
JKleinRot 27:5ac1fc1005d7 1494 state = WACHT;
JKleinRot 27:5ac1fc1005d7 1495 break;
JKleinRot 25:71e52c56be13 1496 }
JKleinRot 25:71e52c56be13 1497
JKleinRot 26:438a498e5526 1498 case WACHT: { //Even wachten en weer terug naar start
JKleinRot 26:438a498e5526 1499 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 26:438a498e5526 1500 lcd.printf(" WACHT "); //Tekst op LCD scherm
JKleinRot 28:e2f5cee5e73b 1501 wait(5); //Vijf seconden wachten
JKleinRot 26:438a498e5526 1502 state = START; //Terug naar state START
JKleinRot 27:5ac1fc1005d7 1503 break;
JKleinRot 26:438a498e5526 1504 }
JKleinRot 23:5267c928ae2b 1505
JKleinRot 26:438a498e5526 1506 default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 26:438a498e5526 1507 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 23:5267c928ae2b 1508 }
JKleinRot 9:454e7da8ab65 1509 }
JKleinRot 20:1cb0cf0d49ac 1510 }
JKleinRot 24:a1fdc830f96c 1511 }
JKleinRot 26:438a498e5526 1512
JKleinRot 27:5ac1fc1005d7 1513