Peter Bartels / Mbed 2 deprecated Main-script_groep7_V4

Laatste versie van ons script

Dependencies:   Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed

Fork of Main-script_groep7_V3 by Peter Bartels

main.cpp@2:de7b6c1d67c4, 2014-10-30 (annotated)

Committer:
phgbartels
Date:
Thu Oct 30 11:51:21 2014 +0000
Revision:
2:de7b6c1d67c4
Parent:
1:b08ac32d1ddc
Child:
3:156c3e536ed4
Added bracket + ....? ; Victor

Who changed what in which revision?

UserRevisionLine numberNew 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 0:2386012c6594 35 #define K_P (100)
phgbartels 0:2386012c6594 36 #define K_I (50 * TSAMP)
phgbartels 0:2386012c6594 37 #define K_D (1)
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 0:2386012c6594 55
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 0:2386012c6594 73 int delta_pos_motor1_puls;
phgbartels 0:2386012c6594 74 void clamp(float * in, float min, float max);
phgbartels 0:2386012c6594 75 volatile bool looptimerflag;
phgbartels 2:de7b6c1d67c4 76 int16_t gewenste_snelheid = 2;
phgbartels 2:de7b6c1d67c4 77 int16_t gewenste_snelheid_rad = 4;
phgbartels 2:de7b6c1d67c4 78 int16_t gewenste_snelheid_rad_return = 1;
phgbartels 0:2386012c6594 79 float pid(float setpoint, float measurement);
phgbartels 0:2386012c6594 80 float pos_motor1_rad;
phgbartels 0:2386012c6594 81 float PWM1_percentage = 0;
phgbartels 0:2386012c6594 82 float delta_pos_motor1_rad;
phgbartels 0:2386012c6594 83 float snelheid_motor1_rad;
phgbartels 0:2386012c6594 84 float snelheid_arm_ms;
phgbartels 0:2386012c6594 85 float PWM1;
phgbartels 0:2386012c6594 86 float PWM2;
phgbartels 0:2386012c6594 87 float Speed_motor1;
phgbartels 0:2386012c6594 88 float Speed_motor1rad;
phgbartels 1:b08ac32d1ddc 89 float setpoint = 0;
phgbartels 1:b08ac32d1ddc 90 float prev_setpoint = 0;
phgbartels 0:2386012c6594 91
phgbartels 1:b08ac32d1ddc 92 //HIDScope scope(6);
phgbartels 0:2386012c6594 93
phgbartels 0:2386012c6594 94 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 95 state_t state = RUST;
phgbartels 0:2386012c6594 96
phgbartels 0:2386012c6594 97 //functies die vanuit de statemachinefunction aangeroepen kunnen worden
phgbartels 0:2386012c6594 98 void rust() {
phgbartels 0:2386012c6594 99 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
phgbartels 0:2386012c6594 100 }
phgbartels 0:2386012c6594 101
phgbartels 0:2386012c6594 102 void arm_kalibratie() {
phgbartels 0:2386012c6594 103 //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 104 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
phgbartels 0:2386012c6594 105 motor1.setPosition(0);
phgbartels 1:b08ac32d1ddc 106 motor2.setPosition(0);
phgbartels 0:2386012c6594 107 }
phgbartels 0:2386012c6594 108
phgbartels 0:2386012c6594 109 void emg_kalibratie() {
phgbartels 0:2386012c6594 110 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
phgbartels 0:2386012c6594 111 }
phgbartels 0:2386012c6594 112
phgbartels 0:2386012c6594 113 void meten_richting() {
phgbartels 0:2386012c6594 114 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
phgbartels 0:2386012c6594 115 }
phgbartels 0:2386012c6594 116
phgbartels 0:2386012c6594 117 void meten_hoogte() {
phgbartels 0:2386012c6594 118 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
phgbartels 0:2386012c6594 119 }
phgbartels 0:2386012c6594 120
phgbartels 0:2386012c6594 121 void instellen_richting() {
phgbartels 0:2386012c6594 122 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
phgbartels 0:2386012c6594 123 }
phgbartels 0:2386012c6594 124
phgbartels 0:2386012c6594 125 void slaan () {
phgbartels 0:2386012c6594 126 current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
phgbartels 0:2386012c6594 127 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 128 pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
phgbartels 0:2386012c6594 129 delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
phgbartels 2:de7b6c1d67c4 130 //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
phgbartels 2:de7b6c1d67c4 131 //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 132 //scope.set(0, snelheid_motor1_rad);
phgbartels 0:2386012c6594 133
phgbartels 0:2386012c6594 134 previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde.
phgbartels 0:2386012c6594 135
phgbartels 0:2386012c6594 136 //nu gaan we positie regelen i.p.v. snelheid.
phgbartels 0:2386012c6594 137 if (current_pos_motor1 >= 400)
phgbartels 0:2386012c6594 138 {
phgbartels 0:2386012c6594 139 gewenste_snelheid_rad = 0;
phgbartels 2:de7b6c1d67c4 140 current_herhalingen = previous_herhalingen + 1;
phgbartels 2:de7b6c1d67c4 141 previous_herhalingen = current_herhalingen;
phgbartels 0:2386012c6594 142 }
phgbartels 0:2386012c6594 143
phgbartels 0:2386012c6594 144 setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad;
phgbartels 0:2386012c6594 145 /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
phgbartels 0:2386012c6594 146 PWM1_percentage = pid(setpoint, pos_motor1_rad);
phgbartels 1:b08ac32d1ddc 147 //scope.set(1, setpoint-pos_motor1_rad);
phgbartels 0:2386012c6594 148
phgbartels 0:2386012c6594 149 if (PWM1_percentage < -100)
phgbartels 0:2386012c6594 150 {
phgbartels 0:2386012c6594 151 PWM1_percentage = -100;
phgbartels 0:2386012c6594 152 }
phgbartels 0:2386012c6594 153 else if (PWM1_percentage >100)
phgbartels 0:2386012c6594 154 {
phgbartels 0:2386012c6594 155 PWM1_percentage =100;
phgbartels 0:2386012c6594 156 }
phgbartels 0:2386012c6594 157 else {}
phgbartels 0:2386012c6594 158
phgbartels 2:de7b6c1d67c4 159 if(PWM1_percentage > 0)
phgbartels 0:2386012c6594 160 {
phgbartels 0:2386012c6594 161 motordir1 = 1;
phgbartels 0:2386012c6594 162 }
phgbartels 0:2386012c6594 163 else
phgbartels 0:2386012c6594 164 {
phgbartels 0:2386012c6594 165 motordir1 = 0;
phgbartels 0:2386012c6594 166 }
phgbartels 0:2386012c6594 167
phgbartels 0:2386012c6594 168 pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
phgbartels 1:b08ac32d1ddc 169 //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
phgbartels 0:2386012c6594 170 prev_setpoint = setpoint;
phgbartels 2:de7b6c1d67c4 171 //scope.send();
phgbartels 2:de7b6c1d67c4 172 }
phgbartels 2:de7b6c1d67c4 173
phgbartels 2:de7b6c1d67c4 174 void return2rust () {
phgbartels 2:de7b6c1d67c4 175 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
phgbartels 2:de7b6c1d67c4 176 current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
phgbartels 2:de7b6c1d67c4 177 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 2:de7b6c1d67c4 178 pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
phgbartels 2:de7b6c1d67c4 179 delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
phgbartels 2:de7b6c1d67c4 180 //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
phgbartels 2:de7b6c1d67c4 181 //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 2:de7b6c1d67c4 182 //scope.set(0, snelheid_motor1_rad);
phgbartels 2:de7b6c1d67c4 183
phgbartels 2:de7b6c1d67c4 184 previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde.
phgbartels 2:de7b6c1d67c4 185
phgbartels 2:de7b6c1d67c4 186 //nu gaan we positie regelen i.p.v. snelheid.
phgbartels 2:de7b6c1d67c4 187 if (current_pos_motor1 <= 0)
phgbartels 2:de7b6c1d67c4 188 {
phgbartels 2:de7b6c1d67c4 189 gewenste_snelheid_rad_return = 0;
phgbartels 2:de7b6c1d67c4 190 }
phgbartels 2:de7b6c1d67c4 191
phgbartels 2:de7b6c1d67c4 192 setpoint = prev_setpoint + (TSAMP * gewenste_snelheid_rad_return);
phgbartels 2:de7b6c1d67c4 193 /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
phgbartels 2:de7b6c1d67c4 194 PWM1_percentage = pid(setpoint, pos_motor1_rad);
phgbartels 2:de7b6c1d67c4 195 //scope.set(1, setpoint-pos_motor1_rad);
phgbartels 2:de7b6c1d67c4 196
phgbartels 2:de7b6c1d67c4 197 if (PWM1_percentage < -100)
phgbartels 2:de7b6c1d67c4 198 {
phgbartels 2:de7b6c1d67c4 199 PWM1_percentage = -100;
phgbartels 2:de7b6c1d67c4 200 }
phgbartels 2:de7b6c1d67c4 201 else if (PWM1_percentage >100)
phgbartels 2:de7b6c1d67c4 202 {
phgbartels 2:de7b6c1d67c4 203 PWM1_percentage =100;
phgbartels 2:de7b6c1d67c4 204 }
phgbartels 2:de7b6c1d67c4 205 else {}
phgbartels 2:de7b6c1d67c4 206
phgbartels 2:de7b6c1d67c4 207 if(PWM1_percentage > 0)
phgbartels 2:de7b6c1d67c4 208 {
phgbartels 2:de7b6c1d67c4 209 motordir1 = 1;
phgbartels 2:de7b6c1d67c4 210 }
phgbartels 2:de7b6c1d67c4 211 else
phgbartels 2:de7b6c1d67c4 212 {
phgbartels 2:de7b6c1d67c4 213 motordir1 = 0;
phgbartels 2:de7b6c1d67c4 214 }
phgbartels 2:de7b6c1d67c4 215
phgbartels 2:de7b6c1d67c4 216 pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
phgbartels 2:de7b6c1d67c4 217 //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
phgbartels 2:de7b6c1d67c4 218 prev_setpoint = setpoint;
phgbartels 2:de7b6c1d67c4 219 //scope.send();
phgbartels 0:2386012c6594 220 }
phgbartels 0:2386012c6594 221
phgbartels 0:2386012c6594 222 float pid(float setpoint, float measurement)
phgbartels 0:2386012c6594 223 {
phgbartels 0:2386012c6594 224 float error;
phgbartels 0:2386012c6594 225 static float prev_error = 0;
phgbartels 0:2386012c6594 226 float out_p = 0;
phgbartels 0:2386012c6594 227 static float out_i = 0;
phgbartels 0:2386012c6594 228 float out_d = 0;
phgbartels 0:2386012c6594 229 error = (setpoint-measurement);
phgbartels 0:2386012c6594 230 out_p = error*K_P;
phgbartels 0:2386012c6594 231 out_i += error*K_I;
phgbartels 0:2386012c6594 232 out_d = (error-prev_error)*K_D;
phgbartels 0:2386012c6594 233 clamp(&out_i,-I_LIMIT,I_LIMIT);
phgbartels 0:2386012c6594 234 prev_error = error;
phgbartels 1:b08ac32d1ddc 235 //scope.set(2, out_p);
phgbartels 1:b08ac32d1ddc 236 //scope.set(3, out_i);
phgbartels 1:b08ac32d1ddc 237 //scope.set(4, out_d);
phgbartels 0:2386012c6594 238 return out_p + out_i + out_d;
phgbartels 0:2386012c6594 239 }
phgbartels 0:2386012c6594 240
phgbartels 0:2386012c6594 241 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 242 // maar de locatie van de variabele.
phgbartels 0:2386012c6594 243 {
phgbartels 0:2386012c6594 244 *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 245 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
phgbartels 0:2386012c6594 246 }
phgbartels 0:2386012c6594 247
phgbartels 0:2386012c6594 248 void statemachinefunction()
phgbartels 0:2386012c6594 249 {
phgbartels 0:2386012c6594 250 switch(state) {
phgbartels 0:2386012c6594 251 case RUST: {
phgbartels 0:2386012c6594 252 rust();
phgbartels 0:2386012c6594 253 /*voorwaarde wanneer hij door kan naar de volgende case*/
phgbartels 1:b08ac32d1ddc 254 if (current_herhalingen == 600)
phgbartels 0:2386012c6594 255 {
phgbartels 0:2386012c6594 256 current_herhalingen = 0;
phgbartels 0:2386012c6594 257 previous_herhalingen = 0;
phgbartels 1:b08ac32d1ddc 258 state = ARM_KALIBRATIE;
phgbartels 0:2386012c6594 259 }
phgbartels 1:b08ac32d1ddc 260 break;
phgbartels 0:2386012c6594 261 /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/
phgbartels 0:2386012c6594 262 //if (metingstatus<5);
phgbartels 0:2386012c6594 263 // state = ARMKALIBRATIE;
phgbartels 0:2386012c6594 264 //if (metingstatus==5);
phgbartels 0:2386012c6594 265 // state = METEN_RICHTING;
phgbartels 0:2386012c6594 266 //break;
phgbartels 0:2386012c6594 267 //}
phgbartels 0:2386012c6594 268 }
phgbartels 0:2386012c6594 269
phgbartels 0:2386012c6594 270 case ARM_KALIBRATIE:
phgbartels 0:2386012c6594 271 {
phgbartels 0:2386012c6594 272 arm_kalibratie();
phgbartels 0:2386012c6594 273 if (current_herhalingen == 200)
phgbartels 0:2386012c6594 274 {
phgbartels 0:2386012c6594 275 current_herhalingen = 0;
phgbartels 0:2386012c6594 276 previous_herhalingen = 0;
phgbartels 1:b08ac32d1ddc 277 state = EMG_KALIBRATIE;
phgbartels 2:de7b6c1d67c4 278 motor1.setPosition(0);
phgbartels 2:de7b6c1d67c4 279 motor2.setPosition(0);
phgbartels 2:de7b6c1d67c4 280 pwm_motor1.period_us(100);
phgbartels 2:de7b6c1d67c4 281 pwm_motor2.period_us(100);
phgbartels 0:2386012c6594 282 }
phgbartels 1:b08ac32d1ddc 283 break;
phgbartels 0:2386012c6594 284 }
phgbartels 0:2386012c6594 285
phgbartels 0:2386012c6594 286 case EMG_KALIBRATIE:
phgbartels 0:2386012c6594 287 {
phgbartels 0:2386012c6594 288 emg_kalibratie();
phgbartels 0:2386012c6594 289 if (current_herhalingen == 200)
phgbartels 0:2386012c6594 290 {
phgbartels 0:2386012c6594 291 state = METEN_RICHTING;
phgbartels 0:2386012c6594 292 current_herhalingen = 0;
phgbartels 0:2386012c6594 293 previous_herhalingen = 0;
phgbartels 0:2386012c6594 294 }
phgbartels 1:b08ac32d1ddc 295 break;
phgbartels 0:2386012c6594 296 }
phgbartels 0:2386012c6594 297
phgbartels 0:2386012c6594 298 case METEN_RICHTING:
phgbartels 0:2386012c6594 299 {
phgbartels 0:2386012c6594 300 meten_richting();
phgbartels 0:2386012c6594 301 if (current_herhalingen == 200)
phgbartels 0:2386012c6594 302 {
phgbartels 0:2386012c6594 303 state = METEN_HOOGTE;
phgbartels 0:2386012c6594 304 current_herhalingen = 0;
phgbartels 0:2386012c6594 305 previous_herhalingen = 0;
phgbartels 0:2386012c6594 306 }
phgbartels 1:b08ac32d1ddc 307 break;
phgbartels 0:2386012c6594 308 }
phgbartels 0:2386012c6594 309
phgbartels 0:2386012c6594 310 case METEN_HOOGTE:
phgbartels 0:2386012c6594 311 {
phgbartels 0:2386012c6594 312 meten_hoogte();
phgbartels 0:2386012c6594 313 if (current_herhalingen == 200)
phgbartels 0:2386012c6594 314 {
phgbartels 0:2386012c6594 315 state = INSTELLEN_RICHTING;
phgbartels 0:2386012c6594 316 current_herhalingen = 0;
phgbartels 0:2386012c6594 317 previous_herhalingen = 0;
phgbartels 0:2386012c6594 318 }
phgbartels 1:b08ac32d1ddc 319 break;
phgbartels 0:2386012c6594 320 }
phgbartels 0:2386012c6594 321
phgbartels 0:2386012c6594 322 case INSTELLEN_RICHTING:
phgbartels 0:2386012c6594 323 {
phgbartels 0:2386012c6594 324 instellen_richting();
phgbartels 0:2386012c6594 325 if (current_herhalingen == 200)
phgbartels 0:2386012c6594 326 {
phgbartels 0:2386012c6594 327 state = SLAAN;
phgbartels 0:2386012c6594 328 current_herhalingen = 0;
phgbartels 0:2386012c6594 329 previous_herhalingen = 0;
phgbartels 0:2386012c6594 330 }
phgbartels 2:de7b6c1d67c4 331 break;
phgbartels 1:b08ac32d1ddc 332
phgbartels 0:2386012c6594 333 }
phgbartels 0:2386012c6594 334
phgbartels 0:2386012c6594 335 case SLAAN:
phgbartels 0:2386012c6594 336 {
phgbartels 0:2386012c6594 337 slaan();
phgbartels 0:2386012c6594 338 if (current_herhalingen == 200)
phgbartels 0:2386012c6594 339 {
phgbartels 0:2386012c6594 340 state = RETURN2RUST;
phgbartels 0:2386012c6594 341 current_herhalingen = 0;
phgbartels 0:2386012c6594 342 previous_herhalingen = 0;
phgbartels 2:de7b6c1d67c4 343 prev_setpoint =0;
phgbartels 2:de7b6c1d67c4 344 setpoint =0;
phgbartels 2:de7b6c1d67c4 345
phgbartels 0:2386012c6594 346 }
phgbartels 2:de7b6c1d67c4 347 break;
phgbartels 0:2386012c6594 348 }
phgbartels 0:2386012c6594 349
phgbartels 0:2386012c6594 350 case RETURN2RUST:
phgbartels 0:2386012c6594 351 {
phgbartels 0:2386012c6594 352 return2rust();
phgbartels 2:de7b6c1d67c4 353 /*if (current_herhalingen == 200)
phgbartels 0:2386012c6594 354 {
phgbartels 0:2386012c6594 355 state = RUST;
phgbartels 0:2386012c6594 356 current_herhalingen = 0;
phgbartels 0:2386012c6594 357 previous_herhalingen = 0;
phgbartels 0:2386012c6594 358 }
phgbartels 2:de7b6c1d67c4 359 */
phgbartels 1:b08ac32d1ddc 360 break;
phgbartels 0:2386012c6594 361 }
phgbartels 0:2386012c6594 362
phgbartels 0:2386012c6594 363 default: {
phgbartels 0:2386012c6594 364 state = RUST;
phgbartels 0:2386012c6594 365 }
phgbartels 0:2386012c6594 366
phgbartels 0:2386012c6594 367 }//switch(state)
phgbartels 0:2386012c6594 368 }//void statemachinefunction
phgbartels 0:2386012c6594 369
phgbartels 0:2386012c6594 370
phgbartels 0:2386012c6594 371 void screenupdate(){
phgbartels 0:2386012c6594 372 if(state==RUST){
phgbartels 0:2386012c6594 373 lcd.cls();
phgbartels 0:2386012c6594 374 lcd.locate(0,0);
phgbartels 0:2386012c6594 375 lcd.printf("S.T.I.E.N.E.N."); //regel 1 LCD scherm
phgbartels 0:2386012c6594 376 lcd.locate(0,1);
phgbartels 0:2386012c6594 377 lcd.printf(" GROEP 7 ");
phgbartels 0:2386012c6594 378 }
phgbartels 0:2386012c6594 379 else{
phgbartels 0:2386012c6594 380 lcd.cls();
phgbartels 0:2386012c6594 381 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 382 }
phgbartels 0:2386012c6594 383 }
phgbartels 0:2386012c6594 384
phgbartels 0:2386012c6594 385 int main() {
phgbartels 0:2386012c6594 386 statemachine.attach(&statemachinefunction, 0.005); // the address of the function to be attached (flip) and the interval (2 seconds)
phgbartels 0:2386012c6594 387 screen.attach(&screenupdate, 0.2);
phgbartels 0:2386012c6594 388 }