Mainscript inclusief PID (uitgevoerd middag 31-10-2014)
Dependencies: Encoder HIDScope TextLCD mbed
Fork of Main-script_groep7 by
main.cpp@6:565b4fc42323, 2014-10-31 (annotated)
- Committer:
- phgbartels
- Date:
- Fri Oct 31 11:23:37 2014 +0000
- Revision:
- 6:565b4fc42323
- Parent:
- 5:4842219cb77c
- Child:
- 7:f016dd2e8ea9
Werkend script, nu nog waarden vari?ren
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
phgbartels | 0:2386012c6594 | 1 | /********************************************/ |
phgbartels | 0:2386012c6594 | 2 | /* */ |
phgbartels | 0:2386012c6594 | 3 | /* BRONCODE GROEP 7, MODULE 9, 2014 */ |
phgbartels | 0:2386012c6594 | 4 | /* *-THE SLAP-* */ |
phgbartels | 0:2386012c6594 | 5 | /* */ |
phgbartels | 0:2386012c6594 | 6 | /* -Anne ten Dam */ |
phgbartels | 0:2386012c6594 | 7 | /* -Laura de Heus */ |
phgbartels | 0:2386012c6594 | 8 | /* -Moniek Strijdveen */ |
phgbartels | 0:2386012c6594 | 9 | /* -Bart Arendshorst */ |
phgbartels | 0:2386012c6594 | 10 | /* -Peter Bartels */ |
phgbartels | 0:2386012c6594 | 11 | /********************************************/ |
phgbartels | 0:2386012c6594 | 12 | |
phgbartels | 0:2386012c6594 | 13 | /* |
phgbartels | 0:2386012c6594 | 14 | #include voor alle libraries |
phgbartels | 0:2386012c6594 | 15 | */ |
phgbartels | 0:2386012c6594 | 16 | #include "TextLCD.h" |
phgbartels | 0:2386012c6594 | 17 | #include "mbed.h" |
phgbartels | 0:2386012c6594 | 18 | #include "encoder.h" |
phgbartels | 1:b08ac32d1ddc | 19 | //#include "HIDScope.h" |
phgbartels | 0:2386012c6594 | 20 | #include "PwmOut.h" |
phgbartels | 0:2386012c6594 | 21 | |
phgbartels | 0:2386012c6594 | 22 | /* |
phgbartels | 0:2386012c6594 | 23 | #define vaste waarden |
phgbartels | 0:2386012c6594 | 24 | */ |
phgbartels | 0:2386012c6594 | 25 | /*definieren pinnen Motor 1*/ |
phgbartels | 0:2386012c6594 | 26 | #define M1_PWM PTA5 |
phgbartels | 0:2386012c6594 | 27 | #define M1_DIR PTA4 |
phgbartels | 0:2386012c6594 | 28 | #define M2_PWM PTC8 |
phgbartels | 0:2386012c6594 | 29 | #define M2_DIR PTC9 |
phgbartels | 0:2386012c6594 | 30 | /*Definieren minimale waarden PWM per motor*/ |
phgbartels | 0:2386012c6594 | 31 | #define PWM1_min_50 0.529 /*met koppelstuk!*/ |
phgbartels | 0:2386012c6594 | 32 | #define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/ |
phgbartels | 0:2386012c6594 | 33 | /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/ |
phgbartels | 0:2386012c6594 | 34 | #define TSAMP 0.005 |
phgbartels | 5:4842219cb77c | 35 | #define K_P (5) |
phgbartels | 5:4842219cb77c | 36 | #define K_I (0.1 * TSAMP) |
phgbartels | 5:4842219cb77c | 37 | #define K_D (0) |
phgbartels | 0:2386012c6594 | 38 | //#define K_D (0.0005 /TSAMP) |
phgbartels | 0:2386012c6594 | 39 | #define I_LIMIT 100. |
phgbartels | 0:2386012c6594 | 40 | #define PI 3.1415926535897 |
phgbartels | 0:2386012c6594 | 41 | #define lengte_arm 0.5 |
phgbartels | 0:2386012c6594 | 42 | |
phgbartels | 0:2386012c6594 | 43 | /* |
phgbartels | 0:2386012c6594 | 44 | Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden. |
phgbartels | 0:2386012c6594 | 45 | */ |
phgbartels | 0:2386012c6594 | 46 | TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7 |
phgbartels | 0:2386012c6594 | 47 | Encoder motor1(PTD3,PTD1); |
phgbartels | 0:2386012c6594 | 48 | Encoder motor2(PTD5, PTD0); |
phgbartels | 0:2386012c6594 | 49 | PwmOut pwm_motor1(M1_PWM); |
phgbartels | 0:2386012c6594 | 50 | PwmOut pwm_motor2(M2_PWM); |
phgbartels | 0:2386012c6594 | 51 | DigitalOut motordir1(M1_DIR); |
phgbartels | 0:2386012c6594 | 52 | DigitalOut motordir2(M2_DIR); |
phgbartels | 0:2386012c6594 | 53 | DigitalOut LEDGREEN(LED_GREEN); |
phgbartels | 0:2386012c6594 | 54 | DigitalOut LEDRED(LED_RED); |
phgbartels | 4:377ddd65e4a6 | 55 | Serial pc(USBTX,USBRX); |
phgbartels | 0:2386012c6594 | 56 | /* |
phgbartels | 0:2386012c6594 | 57 | definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde |
phgbartels | 0:2386012c6594 | 58 | */ |
phgbartels | 0:2386012c6594 | 59 | Ticker statemachine; |
phgbartels | 0:2386012c6594 | 60 | Ticker screen; |
phgbartels | 0:2386012c6594 | 61 | int previous_herhalingen = 0; |
phgbartels | 1:b08ac32d1ddc | 62 | int current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 63 | int PWM2_percentage = 100; |
phgbartels | 0:2386012c6594 | 64 | int aantal_rotaties_1 = 10; |
phgbartels | 0:2386012c6594 | 65 | int aantalcr_1 = 1600; |
phgbartels | 0:2386012c6594 | 66 | int aantalcr_2 = 960; |
phgbartels | 0:2386012c6594 | 67 | int beginpos_motor1; |
phgbartels | 0:2386012c6594 | 68 | int beginpos_motor1_new; |
phgbartels | 0:2386012c6594 | 69 | int beginpos_motor2; |
phgbartels | 0:2386012c6594 | 70 | int beginpos_motor2_new; |
phgbartels | 0:2386012c6594 | 71 | int previous_pos_motor1 = 0; |
phgbartels | 0:2386012c6594 | 72 | int current_pos_motor1; |
phgbartels | 5:4842219cb77c | 73 | int EMG = 1; |
phgbartels | 0:2386012c6594 | 74 | int delta_pos_motor1_puls; |
phgbartels | 0:2386012c6594 | 75 | void clamp(float * in, float min, float max); |
phgbartels | 0:2386012c6594 | 76 | volatile bool looptimerflag; |
phgbartels | 2:de7b6c1d67c4 | 77 | int16_t gewenste_snelheid = 2; |
phgbartels | 2:de7b6c1d67c4 | 78 | int16_t gewenste_snelheid_rad = 4; |
phgbartels | 2:de7b6c1d67c4 | 79 | int16_t gewenste_snelheid_rad_return = 1; |
phgbartels | 0:2386012c6594 | 80 | float pid(float setpoint, float measurement); |
phgbartels | 0:2386012c6594 | 81 | float pos_motor1_rad; |
phgbartels | 0:2386012c6594 | 82 | float PWM1_percentage = 0; |
phgbartels | 0:2386012c6594 | 83 | float delta_pos_motor1_rad; |
phgbartels | 0:2386012c6594 | 84 | float snelheid_motor1_rad; |
phgbartels | 0:2386012c6594 | 85 | float snelheid_arm_ms; |
phgbartels | 0:2386012c6594 | 86 | float PWM1; |
phgbartels | 0:2386012c6594 | 87 | float PWM2; |
phgbartels | 0:2386012c6594 | 88 | float Speed_motor1; |
phgbartels | 0:2386012c6594 | 89 | float Speed_motor1rad; |
phgbartels | 1:b08ac32d1ddc | 90 | float setpoint = 0; |
phgbartels | 1:b08ac32d1ddc | 91 | float prev_setpoint = 0; |
phgbartels | 6:565b4fc42323 | 92 | float hoek_gewenst = 0; |
phgbartels | 6:565b4fc42323 | 93 | float hoek_nul = 0; |
phgbartels | 6:565b4fc42323 | 94 | float snelheid_slaan = 8; |
phgbartels | 6:565b4fc42323 | 95 | float snelheid_naar_nul = 3; |
phgbartels | 0:2386012c6594 | 96 | |
phgbartels | 1:b08ac32d1ddc | 97 | //HIDScope scope(6); |
phgbartels | 0:2386012c6594 | 98 | |
phgbartels | 0:2386012c6594 | 99 | enum state_t {RUST, ARM_KALIBRATIE, EMG_KALIBRATIE, METEN_RICHTING, METEN_HOOGTE, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES |
phgbartels | 0:2386012c6594 | 100 | state_t state = RUST; |
phgbartels | 0:2386012c6594 | 101 | |
phgbartels | 0:2386012c6594 | 102 | //functies die vanuit de statemachinefunction aangeroepen kunnen worden |
phgbartels | 0:2386012c6594 | 103 | void rust() { |
phgbartels | 0:2386012c6594 | 104 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 105 | } |
phgbartels | 0:2386012c6594 | 106 | |
phgbartels | 0:2386012c6594 | 107 | void arm_kalibratie() { |
phgbartels | 0:2386012c6594 | 108 | //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop |
phgbartels | 0:2386012c6594 | 109 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 110 | motor1.setPosition(0); |
phgbartels | 1:b08ac32d1ddc | 111 | motor2.setPosition(0); |
phgbartels | 0:2386012c6594 | 112 | } |
phgbartels | 0:2386012c6594 | 113 | |
phgbartels | 0:2386012c6594 | 114 | void emg_kalibratie() { |
phgbartels | 0:2386012c6594 | 115 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 116 | } |
phgbartels | 0:2386012c6594 | 117 | |
phgbartels | 0:2386012c6594 | 118 | void meten_richting() { |
phgbartels | 0:2386012c6594 | 119 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 120 | } |
phgbartels | 0:2386012c6594 | 121 | |
phgbartels | 0:2386012c6594 | 122 | void meten_hoogte() { |
phgbartels | 0:2386012c6594 | 123 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 124 | } |
phgbartels | 0:2386012c6594 | 125 | |
phgbartels | 0:2386012c6594 | 126 | void instellen_richting() { |
phgbartels | 0:2386012c6594 | 127 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 128 | } |
phgbartels | 0:2386012c6594 | 129 | |
phgbartels | 3:156c3e536ed4 | 130 | void GotoPosition (float position_setpoint_rad, float speed_radpersecond) { |
phgbartels | 5:4842219cb77c | 131 | |
phgbartels | 0:2386012c6594 | 132 | current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is |
phgbartels | 5:4842219cb77c | 133 | //delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls) |
phgbartels | 0:2386012c6594 | 134 | pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar |
phgbartels | 5:4842219cb77c | 135 | //delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad) |
phgbartels | 2:de7b6c1d67c4 | 136 | //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen |
phgbartels | 2:de7b6c1d67c4 | 137 | //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen |
phgbartels | 1:b08ac32d1ddc | 138 | //scope.set(0, snelheid_motor1_rad); |
phgbartels | 0:2386012c6594 | 139 | |
phgbartels | 0:2386012c6594 | 140 | previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde. |
phgbartels | 0:2386012c6594 | 141 | |
phgbartels | 0:2386012c6594 | 142 | //nu gaan we positie regelen i.p.v. snelheid. |
phgbartels | 5:4842219cb77c | 143 | |
phgbartels | 5:4842219cb77c | 144 | if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop. |
phgbartels | 0:2386012c6594 | 145 | { |
phgbartels | 3:156c3e536ed4 | 146 | speed_radpersecond = 0; |
phgbartels | 4:377ddd65e4a6 | 147 | setpoint = pos_motor1_rad; |
phgbartels | 2:de7b6c1d67c4 | 148 | current_herhalingen = previous_herhalingen + 1; |
phgbartels | 2:de7b6c1d67c4 | 149 | previous_herhalingen = current_herhalingen; |
phgbartels | 4:377ddd65e4a6 | 150 | pc.printf("stop\n\r"); |
phgbartels | 5:4842219cb77c | 151 | PWM1_percentage = 0; |
phgbartels | 0:2386012c6594 | 152 | } |
phgbartels | 4:377ddd65e4a6 | 153 | else if(pos_motor1_rad - position_setpoint_rad < 0) |
phgbartels | 4:377ddd65e4a6 | 154 | { |
phgbartels | 5:4842219cb77c | 155 | setpoint = prev_setpoint +( TSAMP * speed_radpersecond); |
phgbartels | 5:4842219cb77c | 156 | PWM1_percentage = pid(setpoint, pos_motor1_rad); |
phgbartels | 4:377ddd65e4a6 | 157 | } |
phgbartels | 3:156c3e536ed4 | 158 | else |
phgbartels | 4:377ddd65e4a6 | 159 | { |
phgbartels | 5:4842219cb77c | 160 | setpoint = prev_setpoint -( TSAMP * speed_radpersecond); |
phgbartels | 5:4842219cb77c | 161 | PWM1_percentage = pid(setpoint, pos_motor1_rad); |
phgbartels | 4:377ddd65e4a6 | 162 | } |
phgbartels | 0:2386012c6594 | 163 | /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/ |
phgbartels | 5:4842219cb77c | 164 | pc.printf("%f\n\r",PWM1_percentage); |
phgbartels | 4:377ddd65e4a6 | 165 | |
phgbartels | 5:4842219cb77c | 166 | |
phgbartels | 1:b08ac32d1ddc | 167 | //scope.set(1, setpoint-pos_motor1_rad); |
phgbartels | 0:2386012c6594 | 168 | |
phgbartels | 0:2386012c6594 | 169 | if (PWM1_percentage < -100) |
phgbartels | 0:2386012c6594 | 170 | { |
phgbartels | 0:2386012c6594 | 171 | PWM1_percentage = -100; |
phgbartels | 0:2386012c6594 | 172 | } |
phgbartels | 0:2386012c6594 | 173 | else if (PWM1_percentage >100) |
phgbartels | 0:2386012c6594 | 174 | { |
phgbartels | 0:2386012c6594 | 175 | PWM1_percentage =100; |
phgbartels | 0:2386012c6594 | 176 | } |
phgbartels | 0:2386012c6594 | 177 | |
phgbartels | 4:377ddd65e4a6 | 178 | if(PWM1_percentage < 0) |
phgbartels | 0:2386012c6594 | 179 | { |
phgbartels | 0:2386012c6594 | 180 | motordir1 = 1; |
phgbartels | 0:2386012c6594 | 181 | } |
phgbartels | 0:2386012c6594 | 182 | else |
phgbartels | 0:2386012c6594 | 183 | { |
phgbartels | 0:2386012c6594 | 184 | motordir1 = 0; |
phgbartels | 0:2386012c6594 | 185 | } |
phgbartels | 0:2386012c6594 | 186 | |
phgbartels | 4:377ddd65e4a6 | 187 | pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); |
phgbartels | 1:b08ac32d1ddc | 188 | //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); |
phgbartels | 0:2386012c6594 | 189 | prev_setpoint = setpoint; |
phgbartels | 2:de7b6c1d67c4 | 190 | //scope.send(); |
phgbartels | 2:de7b6c1d67c4 | 191 | } |
phgbartels | 2:de7b6c1d67c4 | 192 | |
phgbartels | 3:156c3e536ed4 | 193 | |
phgbartels | 0:2386012c6594 | 194 | |
phgbartels | 0:2386012c6594 | 195 | float pid(float setpoint, float measurement) |
phgbartels | 0:2386012c6594 | 196 | { |
phgbartels | 0:2386012c6594 | 197 | float error; |
phgbartels | 0:2386012c6594 | 198 | static float prev_error = 0; |
phgbartels | 0:2386012c6594 | 199 | float out_p = 0; |
phgbartels | 0:2386012c6594 | 200 | static float out_i = 0; |
phgbartels | 0:2386012c6594 | 201 | float out_d = 0; |
phgbartels | 0:2386012c6594 | 202 | error = (setpoint-measurement); |
phgbartels | 0:2386012c6594 | 203 | out_p = error*K_P; |
phgbartels | 0:2386012c6594 | 204 | out_i += error*K_I; |
phgbartels | 0:2386012c6594 | 205 | out_d = (error-prev_error)*K_D; |
phgbartels | 0:2386012c6594 | 206 | clamp(&out_i,-I_LIMIT,I_LIMIT); |
phgbartels | 0:2386012c6594 | 207 | prev_error = error; |
phgbartels | 1:b08ac32d1ddc | 208 | //scope.set(2, out_p); |
phgbartels | 1:b08ac32d1ddc | 209 | //scope.set(3, out_i); |
phgbartels | 1:b08ac32d1ddc | 210 | //scope.set(4, out_d); |
phgbartels | 0:2386012c6594 | 211 | return out_p + out_i + out_d; |
phgbartels | 0:2386012c6594 | 212 | } |
phgbartels | 0:2386012c6594 | 213 | |
phgbartels | 0:2386012c6594 | 214 | void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op |
phgbartels | 0:2386012c6594 | 215 | // maar de locatie van de variabele. |
phgbartels | 0:2386012c6594 | 216 | { |
phgbartels | 0:2386012c6594 | 217 | *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c |
phgbartels | 0:2386012c6594 | 218 | // *in = het getal dat staat op locatie van in --> waarde van new_pwm |
phgbartels | 0:2386012c6594 | 219 | } |
phgbartels | 0:2386012c6594 | 220 | |
phgbartels | 0:2386012c6594 | 221 | void statemachinefunction() |
phgbartels | 0:2386012c6594 | 222 | { |
phgbartels | 0:2386012c6594 | 223 | switch(state) { |
phgbartels | 0:2386012c6594 | 224 | case RUST: { |
phgbartels | 0:2386012c6594 | 225 | rust(); |
phgbartels | 0:2386012c6594 | 226 | /*voorwaarde wanneer hij door kan naar de volgende case*/ |
phgbartels | 5:4842219cb77c | 227 | if (current_herhalingen == 100 && EMG == 1) |
phgbartels | 0:2386012c6594 | 228 | { |
phgbartels | 0:2386012c6594 | 229 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 230 | previous_herhalingen = 0; |
phgbartels | 1:b08ac32d1ddc | 231 | state = ARM_KALIBRATIE; |
phgbartels | 5:4842219cb77c | 232 | EMG = 0; |
phgbartels | 0:2386012c6594 | 233 | } |
phgbartels | 1:b08ac32d1ddc | 234 | break; |
phgbartels | 0:2386012c6594 | 235 | /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/ |
phgbartels | 0:2386012c6594 | 236 | //if (metingstatus<5); |
phgbartels | 0:2386012c6594 | 237 | // state = ARMKALIBRATIE; |
phgbartels | 0:2386012c6594 | 238 | //if (metingstatus==5); |
phgbartels | 0:2386012c6594 | 239 | // state = METEN_RICHTING; |
phgbartels | 0:2386012c6594 | 240 | //break; |
phgbartels | 0:2386012c6594 | 241 | //} |
phgbartels | 0:2386012c6594 | 242 | } |
phgbartels | 0:2386012c6594 | 243 | |
phgbartels | 0:2386012c6594 | 244 | case ARM_KALIBRATIE: |
phgbartels | 0:2386012c6594 | 245 | { |
phgbartels | 0:2386012c6594 | 246 | arm_kalibratie(); |
phgbartels | 5:4842219cb77c | 247 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 248 | { |
phgbartels | 0:2386012c6594 | 249 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 250 | previous_herhalingen = 0; |
phgbartels | 1:b08ac32d1ddc | 251 | state = EMG_KALIBRATIE; |
phgbartels | 2:de7b6c1d67c4 | 252 | motor1.setPosition(0); |
phgbartels | 2:de7b6c1d67c4 | 253 | motor2.setPosition(0); |
phgbartels | 2:de7b6c1d67c4 | 254 | pwm_motor1.period_us(100); |
phgbartels | 2:de7b6c1d67c4 | 255 | pwm_motor2.period_us(100); |
phgbartels | 0:2386012c6594 | 256 | } |
phgbartels | 1:b08ac32d1ddc | 257 | break; |
phgbartels | 0:2386012c6594 | 258 | } |
phgbartels | 0:2386012c6594 | 259 | |
phgbartels | 0:2386012c6594 | 260 | case EMG_KALIBRATIE: |
phgbartels | 0:2386012c6594 | 261 | { |
phgbartels | 0:2386012c6594 | 262 | emg_kalibratie(); |
phgbartels | 5:4842219cb77c | 263 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 264 | { |
phgbartels | 0:2386012c6594 | 265 | state = METEN_RICHTING; |
phgbartels | 0:2386012c6594 | 266 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 267 | previous_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 268 | } |
phgbartels | 1:b08ac32d1ddc | 269 | break; |
phgbartels | 0:2386012c6594 | 270 | } |
phgbartels | 0:2386012c6594 | 271 | |
phgbartels | 0:2386012c6594 | 272 | case METEN_RICHTING: |
phgbartels | 0:2386012c6594 | 273 | { |
phgbartels | 0:2386012c6594 | 274 | meten_richting(); |
phgbartels | 5:4842219cb77c | 275 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 276 | { |
phgbartels | 0:2386012c6594 | 277 | state = METEN_HOOGTE; |
phgbartels | 0:2386012c6594 | 278 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 279 | previous_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 280 | } |
phgbartels | 1:b08ac32d1ddc | 281 | break; |
phgbartels | 0:2386012c6594 | 282 | } |
phgbartels | 0:2386012c6594 | 283 | |
phgbartels | 0:2386012c6594 | 284 | case METEN_HOOGTE: |
phgbartels | 0:2386012c6594 | 285 | { |
phgbartels | 0:2386012c6594 | 286 | meten_hoogte(); |
phgbartels | 5:4842219cb77c | 287 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 288 | { |
phgbartels | 0:2386012c6594 | 289 | state = INSTELLEN_RICHTING; |
phgbartels | 0:2386012c6594 | 290 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 291 | previous_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 292 | } |
phgbartels | 1:b08ac32d1ddc | 293 | break; |
phgbartels | 0:2386012c6594 | 294 | } |
phgbartels | 0:2386012c6594 | 295 | |
phgbartels | 0:2386012c6594 | 296 | case INSTELLEN_RICHTING: |
phgbartels | 0:2386012c6594 | 297 | { |
phgbartels | 0:2386012c6594 | 298 | instellen_richting(); |
phgbartels | 5:4842219cb77c | 299 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 300 | { |
phgbartels | 0:2386012c6594 | 301 | state = SLAAN; |
phgbartels | 0:2386012c6594 | 302 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 303 | previous_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 304 | } |
phgbartels | 2:de7b6c1d67c4 | 305 | break; |
phgbartels | 1:b08ac32d1ddc | 306 | |
phgbartels | 0:2386012c6594 | 307 | } |
phgbartels | 0:2386012c6594 | 308 | |
phgbartels | 0:2386012c6594 | 309 | case SLAAN: |
phgbartels | 0:2386012c6594 | 310 | { |
phgbartels | 6:565b4fc42323 | 311 | GotoPosition(hoek_gewenst , snelheid_slaan); |
phgbartels | 5:4842219cb77c | 312 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 313 | { |
phgbartels | 0:2386012c6594 | 314 | state = RETURN2RUST; |
phgbartels | 0:2386012c6594 | 315 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 316 | previous_herhalingen = 0; |
phgbartels | 2:de7b6c1d67c4 | 317 | prev_setpoint =0; |
phgbartels | 2:de7b6c1d67c4 | 318 | setpoint =0; |
phgbartels | 2:de7b6c1d67c4 | 319 | |
phgbartels | 0:2386012c6594 | 320 | } |
phgbartels | 2:de7b6c1d67c4 | 321 | break; |
phgbartels | 0:2386012c6594 | 322 | } |
phgbartels | 0:2386012c6594 | 323 | |
phgbartels | 0:2386012c6594 | 324 | case RETURN2RUST: |
phgbartels | 0:2386012c6594 | 325 | { |
phgbartels | 6:565b4fc42323 | 326 | GotoPosition(hoek_nul, snelheid_naar_nul); |
phgbartels | 5:4842219cb77c | 327 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 328 | { |
phgbartels | 0:2386012c6594 | 329 | state = RUST; |
phgbartels | 0:2386012c6594 | 330 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 331 | previous_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 332 | } |
phgbartels | 5:4842219cb77c | 333 | |
phgbartels | 1:b08ac32d1ddc | 334 | break; |
phgbartels | 0:2386012c6594 | 335 | } |
phgbartels | 0:2386012c6594 | 336 | |
phgbartels | 0:2386012c6594 | 337 | default: { |
phgbartels | 0:2386012c6594 | 338 | state = RUST; |
phgbartels | 0:2386012c6594 | 339 | } |
phgbartels | 0:2386012c6594 | 340 | |
phgbartels | 0:2386012c6594 | 341 | }//switch(state) |
phgbartels | 0:2386012c6594 | 342 | }//void statemachinefunction |
phgbartels | 0:2386012c6594 | 343 | |
phgbartels | 0:2386012c6594 | 344 | |
phgbartels | 0:2386012c6594 | 345 | void screenupdate(){ |
phgbartels | 0:2386012c6594 | 346 | if(state==RUST){ |
phgbartels | 0:2386012c6594 | 347 | lcd.cls(); |
phgbartels | 0:2386012c6594 | 348 | lcd.locate(0,0); |
phgbartels | 5:4842219cb77c | 349 | lcd.printf("V.I.C.T.O.R.Y."); //regel 1 LCD scherm |
phgbartels | 0:2386012c6594 | 350 | lcd.locate(0,1); |
phgbartels | 0:2386012c6594 | 351 | lcd.printf(" GROEP 7 "); |
phgbartels | 0:2386012c6594 | 352 | } |
phgbartels | 0:2386012c6594 | 353 | else{ |
phgbartels | 0:2386012c6594 | 354 | lcd.cls(); |
phgbartels | 0:2386012c6594 | 355 | lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen. |
phgbartels | 0:2386012c6594 | 356 | } |
phgbartels | 0:2386012c6594 | 357 | } |
phgbartels | 0:2386012c6594 | 358 | |
phgbartels | 0:2386012c6594 | 359 | int main() { |
phgbartels | 4:377ddd65e4a6 | 360 | pc.baud(115200); |
phgbartels | 4:377ddd65e4a6 | 361 | statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds) |
phgbartels | 0:2386012c6594 | 362 | screen.attach(&screenupdate, 0.2); |
phgbartels | 4:377ddd65e4a6 | 363 | while(1); |
phgbartels | 0:2386012c6594 | 364 | } |