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@13:95a4bb9daf63, 2014-11-03 (annotated)

Committer:
phgbartels
Date:
Mon Nov 03 21:31:16 2014 +0000
Revision:
13:95a4bb9daf63
Parent:
12:3fec73bc3318
Child:
14:b1de410384c2
Werken script, translerende arm doet het!

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 #include "TextLCD.h"
phgbartels 0:2386012c6594 14 #include "mbed.h"
phgbartels 0:2386012c6594 15 #include "encoder.h"
lauradeheus 6:a7379a681adf 16 #include "HIDScope.h"
phgbartels 0:2386012c6594 17 #include "PwmOut.h"
lauradeheus 6:a7379a681adf 18 #include "arm_math.h"
phgbartels 11:687aa4656a6e 19 #include "MODSERIAL.h"
phgbartels 0:2386012c6594 20
lauradeheus 8:f733c6a27c15 21 /*definieren pinnen Motoren*/
phgbartels 0:2386012c6594 22 #define M1_PWM PTA5
phgbartels 0:2386012c6594 23 #define M1_DIR PTA4
phgbartels 0:2386012c6594 24 #define M2_PWM PTC8
phgbartels 0:2386012c6594 25 #define M2_DIR PTC9
phgbartels 0:2386012c6594 26 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
phgbartels 0:2386012c6594 27 #define TSAMP 0.005
lauradeheus 8:f733c6a27c15 28 #define K_P (80000)
phgbartels 13:95a4bb9daf63 29 #define K_P_motor2 (75)
phgbartels 13:95a4bb9daf63 30 #define K_D_motor2 (0.01)
lauradeheus 8:f733c6a27c15 31 #define K_I (0.01)
lauradeheus 8:f733c6a27c15 32 #define K_D (0.01)
phgbartels 0:2386012c6594 33 #define I_LIMIT 100.
phgbartels 0:2386012c6594 34 #define lengte_arm 0.5
phgbartels 0:2386012c6594 35
phgbartels 0:2386012c6594 36 /*
phgbartels 0:2386012c6594 37 Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden.
phgbartels 0:2386012c6594 38 */
phgbartels 0:2386012c6594 39 TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7
phgbartels 0:2386012c6594 40 Encoder motor1(PTD3,PTD1);
phgbartels 11:687aa4656a6e 41 Encoder motor2(PTD5, PTD0);
phgbartels 0:2386012c6594 42 PwmOut pwm_motor1(M1_PWM);
phgbartels 11:687aa4656a6e 43 PwmOut pwm_motor2(M2_PWM);
phgbartels 0:2386012c6594 44 DigitalOut motordir1(M1_DIR);
phgbartels 11:687aa4656a6e 45 DigitalOut motordir2(M2_DIR);
phgbartels 0:2386012c6594 46 DigitalOut LEDGREEN(LED_GREEN);
phgbartels 11:687aa4656a6e 47 DigitalOut LEDRED(LED_RED);
phgbartels 11:687aa4656a6e 48 MODSERIAL pc(USBTX,USBRX);
lauradeheus 6:a7379a681adf 49 HIDScope scope(3);
lauradeheus 8:f733c6a27c15 50 AnalogIn emg(PTB1);
phgbartels 12:3fec73bc3318 51
phgbartels 12:3fec73bc3318 52 Timer statetimer;
phgbartels 0:2386012c6594 53 /*
phgbartels 0:2386012c6594 54 definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
phgbartels 0:2386012c6594 55 */
phgbartels 0:2386012c6594 56 Ticker statemachine;
phgbartels 0:2386012c6594 57 Ticker screen;
lauradeheus 6:a7379a681adf 58 arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz
lauradeheus 6:a7379a681adf 59 arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz
lauradeheus 6:a7379a681adf 60 arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz
lauradeheus 6:a7379a681adf 61 arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
phgbartels 0:2386012c6594 62 int previous_herhalingen = 0;
phgbartels 1:b08ac32d1ddc 63 int current_herhalingen = 0;
phgbartels 11:687aa4656a6e 64 int current_herhalingen_1 = 0;
phgbartels 11:687aa4656a6e 65 int previous_herhalingen_1 = 0;
phgbartels 0:2386012c6594 66 int previous_pos_motor1 = 0;
phgbartels 13:95a4bb9daf63 67 int previous_pos_motor2 = 0;
phgbartels 0:2386012c6594 68 int current_pos_motor1;
phgbartels 11:687aa4656a6e 69 int current_pos_motor2;
phgbartels 5:4842219cb77c 70 int EMG = 1;
phgbartels 10:cd89569cd847 71 int aantal_pieken = 0;
lauradeheus 6:a7379a681adf 72 int doel;
lauradeheus 7:7e3e183bf063 73 int doel_richting;
lauradeheus 7:7e3e183bf063 74 int doel_hoogte;
phgbartels 13:95a4bb9daf63 75 int puls_richting1 = 4000;
lauradeheus 9:7e9b63fe8988 76 int puls_richting2;
phgbartels 13:95a4bb9daf63 77 int puls_richting3 = 4000;
lauradeheus 6:a7379a681adf 78 bool aanspan;
phgbartels 0:2386012c6594 79 void clamp(float * in, float min, float max);
phgbartels 0:2386012c6594 80 float pid(float setpoint, float measurement);
phgbartels 13:95a4bb9daf63 81 float pid_motor2(float setpoint, float measurement);
phgbartels 0:2386012c6594 82 float pos_motor1_rad;
phgbartels 13:95a4bb9daf63 83 float pos_motor2_rad;
phgbartels 0:2386012c6594 84 float PWM1_percentage = 0;
phgbartels 13:95a4bb9daf63 85 float PWM2_percentage = 0;
phgbartels 11:687aa4656a6e 86 float PWM1;
phgbartels 0:2386012c6594 87 float PWM2;
phgbartels 1:b08ac32d1ddc 88 float setpoint = 0;
lauradeheus 6:a7379a681adf 89 float prev_setpoint = 0;
lauradeheus 6:a7379a681adf 90 float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
lauradeheus 6:a7379a681adf 91 float lowpass_1_states[4];
lauradeheus 6:a7379a681adf 92 float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684};
lauradeheus 6:a7379a681adf 93 float lowpass_2_states[4];
lauradeheus 6:a7379a681adf 94 float highpass_const[] = {0.638945525159022 , -1.277891050318045 , 0.638945525159022 , 1.142980502539901 , -0.412801598096189};
lauradeheus 6:a7379a681adf 95 float highpass_states[4];
lauradeheus 6:a7379a681adf 96 float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362};
lauradeheus 6:a7379a681adf 97 float notch_states[4];
lauradeheus 6:a7379a681adf 98 float emg_filtered;
lauradeheus 6:a7379a681adf 99 float emg_max = 0;
lauradeheus 6:a7379a681adf 100 float emg_treshhold_laag = 0;
lauradeheus 6:a7379a681adf 101 float emg_treshhold_hoog = 0;
phgbartels 11:687aa4656a6e 102 float marge = 0;
lauradeheus 9:7e9b63fe8988 103 float PWM_richting1;
lauradeheus 9:7e9b63fe8988 104 float PWM_richting2;
lauradeheus 9:7e9b63fe8988 105 float PWM_richting3;
phgbartels 13:95a4bb9daf63 106 float position2_setpoint;
phgbartels 0:2386012c6594 107
phgbartels 13:95a4bb9daf63 108 enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES
phgbartels 0:2386012c6594 109 state_t state = RUST;
phgbartels 0:2386012c6594 110
phgbartels 11:687aa4656a6e 111 void rust()
phgbartels 11:687aa4656a6e 112 {
phgbartels 11:687aa4656a6e 113 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 114 previous_herhalingen = current_herhalingen;
lauradeheus 8:f733c6a27c15 115 }//void rust
phgbartels 11:687aa4656a6e 116
phgbartels 11:687aa4656a6e 117 void pieken_tellen()
phgbartels 11:687aa4656a6e 118 {
phgbartels 11:687aa4656a6e 119 if (emg_filtered>=emg_treshhold_hoog) {
lauradeheus 6:a7379a681adf 120 aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
lauradeheus 8:f733c6a27c15 121 }//if
phgbartels 11:687aa4656a6e 122 if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt
lauradeheus 6:a7379a681adf 123 aanspan=false;
lauradeheus 6:a7379a681adf 124 aantal_pieken++;
phgbartels 10:cd89569cd847 125 doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1;
phgbartels 11:687aa4656a6e 126
lauradeheus 8:f733c6a27c15 127 }//if
lauradeheus 8:f733c6a27c15 128 }//void pieken_tellen
lauradeheus 6:a7379a681adf 129
phgbartels 11:687aa4656a6e 130 void emg_filtering()
phgbartels 11:687aa4656a6e 131 {
lauradeheus 6:a7379a681adf 132 uint16_t emg_value;
lauradeheus 6:a7379a681adf 133 float emg_value_f32;
lauradeheus 6:a7379a681adf 134 emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
lauradeheus 6:a7379a681adf 135 emg_value_f32 = emg.read();
phgbartels 11:687aa4656a6e 136
lauradeheus 6:a7379a681adf 137 arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
lauradeheus 6:a7379a681adf 138 arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
lauradeheus 6:a7379a681adf 139 arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
lauradeheus 6:a7379a681adf 140 emg_filtered = fabs(emg_filtered);
lauradeheus 6:a7379a681adf 141 arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
phgbartels 10:cd89569cd847 142 //scope.set(0,emg_value); //uint value
phgbartels 10:cd89569cd847 143 //scope.set(1,emg_filtered); //processed float
phgbartels 11:687aa4656a6e 144 if(state!=EMG_KALIBRATIE) {
lauradeheus 6:a7379a681adf 145 pieken_tellen();
lauradeheus 8:f733c6a27c15 146 }//if
phgbartels 10:cd89569cd847 147 //pc.printf("%d\n\r",doel);
phgbartels 11:687aa4656a6e 148 scope.set(0, doel);
phgbartels 10:cd89569cd847 149 scope.set(1, aantal_pieken);
lauradeheus 9:7e9b63fe8988 150 scope.send();
lauradeheus 8:f733c6a27c15 151 }//void emg_filtering()
lauradeheus 6:a7379a681adf 152
phgbartels 11:687aa4656a6e 153 void emg_max_meting()
phgbartels 11:687aa4656a6e 154 {
lauradeheus 6:a7379a681adf 155 emg_filtering();
phgbartels 11:687aa4656a6e 156 if (emg_filtered>=emg_max) {
lauradeheus 6:a7379a681adf 157 emg_max=emg_filtered;
lauradeheus 8:f733c6a27c15 158 }//if
phgbartels 11:687aa4656a6e 159 emg_treshhold_laag = 0.4*emg_max;
phgbartels 11:687aa4656a6e 160 emg_treshhold_hoog = 0.7*emg_max;
lauradeheus 8:f733c6a27c15 161 }//void emg_max_meting
lauradeheus 6:a7379a681adf 162
phgbartels 11:687aa4656a6e 163 void akkoord_geven()
phgbartels 11:687aa4656a6e 164 {
lauradeheus 9:7e9b63fe8988 165 emg_filtering();
lauradeheus 9:7e9b63fe8988 166 }
lauradeheus 9:7e9b63fe8988 167
phgbartels 11:687aa4656a6e 168 void emg_kalibratie()
phgbartels 11:687aa4656a6e 169 {
lauradeheus 7:7e3e183bf063 170 //if(emg_filtered>=0.05){//Deze if-loop alleen zodat het nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten.
phgbartels 11:687aa4656a6e 171 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 172 previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 173 //}
phgbartels 11:687aa4656a6e 174 if(current_herhalingen<=1000) {
lauradeheus 6:a7379a681adf 175 emg_max_meting();
lauradeheus 8:f733c6a27c15 176 }//if
lauradeheus 8:f733c6a27c15 177 }//void emg_kalibratie
phgbartels 0:2386012c6594 178
phgbartels 11:687aa4656a6e 179 void arm_kalibratie()
phgbartels 11:687aa4656a6e 180 {
lauradeheus 7:7e3e183bf063 181 //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
lauradeheus 7:7e3e183bf063 182 motor1.setPosition(0);
phgbartels 13:95a4bb9daf63 183 //motor2.setPosition(0);
phgbartels 11:687aa4656a6e 184 akkoord_geven();
lauradeheus 8:f733c6a27c15 185 }//void arm_kalibratie
lauradeheus 7:7e3e183bf063 186
phgbartels 11:687aa4656a6e 187 void doel_bepaling()
phgbartels 11:687aa4656a6e 188 {
phgbartels 11:687aa4656a6e 189 if(200<=current_herhalingen && current_herhalingen <1200) {
lauradeheus 7:7e3e183bf063 190 emg_filtering();
phgbartels 10:cd89569cd847 191 doel = aantal_pieken-(((aantal_pieken-1)/3)*3);
lauradeheus 8:f733c6a27c15 192 }//if
phgbartels 11:687aa4656a6e 193 else if(current_herhalingen == 1200 && state==METEN_HOOGTE) {
lauradeheus 9:7e9b63fe8988 194 doel_hoogte = doel;
lauradeheus 9:7e9b63fe8988 195 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 196 doel = 0;
phgbartels 11:687aa4656a6e 197 } else if(current_herhalingen == 1200 && state==METEN_RICHTING) {
lauradeheus 9:7e9b63fe8988 198 doel_richting = doel;
lauradeheus 9:7e9b63fe8988 199 aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
phgbartels 10:cd89569cd847 200 doel = 0;
phgbartels 11:687aa4656a6e 201 } else if(1200<current_herhalingen && current_herhalingen<=2200) {
phgbartels 11:687aa4656a6e 202 akkoord_geven();
lauradeheus 8:f733c6a27c15 203 }//else if
phgbartels 11:687aa4656a6e 204 else {
phgbartels 11:687aa4656a6e 205 }//else
lauradeheus 8:f733c6a27c15 206 }//void doel_bepaling
phgbartels 0:2386012c6594 207
phgbartels 11:687aa4656a6e 208 void meten_hoogte()
phgbartels 11:687aa4656a6e 209 {
phgbartels 11:687aa4656a6e 210 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 211 previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 212 doel_bepaling();
lauradeheus 8:f733c6a27c15 213 }//void meten_hoogte
lauradeheus 7:7e3e183bf063 214
phgbartels 11:687aa4656a6e 215 void meten_richting()
phgbartels 11:687aa4656a6e 216 {
phgbartels 11:687aa4656a6e 217 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 218 previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 219 doel_bepaling();
lauradeheus 8:f733c6a27c15 220 }//void meten_richting
phgbartels 0:2386012c6594 221
phgbartels 11:687aa4656a6e 222 void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge)
phgbartels 11:687aa4656a6e 223 {
phgbartels 0:2386012c6594 224 current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
phgbartels 0:2386012c6594 225 pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
phgbartels 0:2386012c6594 226 previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde.
phgbartels 11:687aa4656a6e 227
lauradeheus 9:7e9b63fe8988 228 //nu gaan we snelheid volgen d.m.v. positie regeling
phgbartels 11:687aa4656a6e 229 if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) { //if position error < ...rad, then stop.
phgbartels 11:687aa4656a6e 230 speed_radpersecond = 0;
phgbartels 4:377ddd65e4a6 231 setpoint = pos_motor1_rad;
phgbartels 11:687aa4656a6e 232 current_herhalingen = previous_herhalingen + 1;
phgbartels 2:de7b6c1d67c4 233 previous_herhalingen = current_herhalingen;
phgbartels 11:687aa4656a6e 234 //pc.printf("stop\n\r");
phgbartels 5:4842219cb77c 235 PWM1_percentage = 0;
lauradeheus 8:f733c6a27c15 236 }//if
phgbartels 11:687aa4656a6e 237 else if(pos_motor1_rad - position_setpoint_rad < 0) {
phgbartels 5:4842219cb77c 238 setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
phgbartels 11:687aa4656a6e 239 PWM1_percentage = pid(setpoint, pos_motor1_rad);
lauradeheus 8:f733c6a27c15 240 }//else if
phgbartels 11:687aa4656a6e 241 else {
phgbartels 5:4842219cb77c 242 setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
phgbartels 11:687aa4656a6e 243 PWM1_percentage = pid(setpoint, pos_motor1_rad);
lauradeheus 8:f733c6a27c15 244 }//else
phgbartels 11:687aa4656a6e 245 //pc.printf("%f\n\r",PWM1_percentage);
phgbartels 11:687aa4656a6e 246
phgbartels 11:687aa4656a6e 247 if (PWM1_percentage < -100) {
phgbartels 0:2386012c6594 248 PWM1_percentage = -100;
phgbartels 13:95a4bb9daf63 249 } else if (PWM1_percentage >100) {
phgbartels 0:2386012c6594 250 PWM1_percentage =100;
phgbartels 13:95a4bb9daf63 251 }
phgbartels 11:687aa4656a6e 252
phgbartels 11:687aa4656a6e 253 if(PWM1_percentage < 0) {
phgbartels 0:2386012c6594 254 motordir1 = 1;
lauradeheus 8:f733c6a27c15 255 }//if
phgbartels 11:687aa4656a6e 256 else {
phgbartels 0:2386012c6594 257 motordir1 = 0;
lauradeheus 8:f733c6a27c15 258 }//else
phgbartels 11:687aa4656a6e 259
lauradeheus 9:7e9b63fe8988 260 pwm_motor1.write(abs(PWM1_percentage/100.));
phgbartels 0:2386012c6594 261 prev_setpoint = setpoint;
lauradeheus 8:f733c6a27c15 262 }//void GotoPosition
phgbartels 2:de7b6c1d67c4 263
phgbartels 0:2386012c6594 264 float pid(float setpoint, float measurement)
phgbartels 0:2386012c6594 265 {
phgbartels 0:2386012c6594 266 float error;
phgbartels 0:2386012c6594 267 static float prev_error = 0;
phgbartels 0:2386012c6594 268 float out_p = 0;
phgbartels 0:2386012c6594 269 static float out_i = 0;
phgbartels 0:2386012c6594 270 float out_d = 0;
phgbartels 0:2386012c6594 271 error = (setpoint-measurement);
phgbartels 11:687aa4656a6e 272 out_p = error*K_P;
phgbartels 0:2386012c6594 273 out_i += error*K_I;
phgbartels 0:2386012c6594 274 out_d = (error-prev_error)*K_D;
phgbartels 0:2386012c6594 275 clamp(&out_i,-I_LIMIT,I_LIMIT);
phgbartels 0:2386012c6594 276 prev_error = error;
phgbartels 0:2386012c6594 277 return out_p + out_i + out_d;
lauradeheus 8:f733c6a27c15 278 }//float pid
phgbartels 0:2386012c6594 279
phgbartels 13:95a4bb9daf63 280 void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2)
phgbartels 13:95a4bb9daf63 281 {
phgbartels 13:95a4bb9daf63 282 if (setpoint < position2_setpoint_rad) {
phgbartels 13:95a4bb9daf63 283 setpoint = prev_setpoint +( 0.0005 * speed2_radpersecond);
phgbartels 13:95a4bb9daf63 284 if (setpoint > position2_setpoint_rad) {
phgbartels 13:95a4bb9daf63 285 setpoint = position2_setpoint_rad;
phgbartels 13:95a4bb9daf63 286 }
phgbartels 13:95a4bb9daf63 287 }
phgbartels 13:95a4bb9daf63 288
phgbartels 13:95a4bb9daf63 289 else if (setpoint > position2_setpoint_rad) {
phgbartels 13:95a4bb9daf63 290 setpoint = prev_setpoint - (0.0005 * speed2_radpersecond);
phgbartels 13:95a4bb9daf63 291 if (setpoint < position2_setpoint_rad) {
phgbartels 13:95a4bb9daf63 292 setpoint = position2_setpoint_rad;
phgbartels 13:95a4bb9daf63 293 }
phgbartels 13:95a4bb9daf63 294 } else if (setpoint == position2_setpoint_rad) {
phgbartels 13:95a4bb9daf63 295 current_herhalingen = previous_herhalingen + 1;
phgbartels 13:95a4bb9daf63 296 previous_herhalingen = current_herhalingen;
phgbartels 13:95a4bb9daf63 297 }
phgbartels 13:95a4bb9daf63 298
phgbartels 13:95a4bb9daf63 299 current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
phgbartels 13:95a4bb9daf63 300 pc.printf("c: %d\r\n", motor2.getPosition());//current_pos_motor2);
phgbartels 13:95a4bb9daf63 301 pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
phgbartels 13:95a4bb9daf63 302 PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad);
phgbartels 13:95a4bb9daf63 303
phgbartels 13:95a4bb9daf63 304 if (PWM2_percentage < -100) {
phgbartels 13:95a4bb9daf63 305 PWM2_percentage = -100;
phgbartels 13:95a4bb9daf63 306 } else if (PWM2_percentage >100) {
phgbartels 13:95a4bb9daf63 307 PWM2_percentage =100;
phgbartels 13:95a4bb9daf63 308 }
phgbartels 13:95a4bb9daf63 309
phgbartels 13:95a4bb9daf63 310 if(PWM2_percentage < 0) {
phgbartels 13:95a4bb9daf63 311 motordir2 = 1;
phgbartels 13:95a4bb9daf63 312 }//if
phgbartels 13:95a4bb9daf63 313 else {
phgbartels 13:95a4bb9daf63 314 motordir2 = 0;
phgbartels 13:95a4bb9daf63 315 }//else
phgbartels 13:95a4bb9daf63 316
phgbartels 13:95a4bb9daf63 317 pwm_motor2.write(abs(PWM2_percentage/100.));
phgbartels 13:95a4bb9daf63 318 prev_setpoint = setpoint;
phgbartels 13:95a4bb9daf63 319 }
phgbartels 13:95a4bb9daf63 320
phgbartels 13:95a4bb9daf63 321 float pid_motor2(float setpoint, float measurement)
phgbartels 13:95a4bb9daf63 322 {
phgbartels 13:95a4bb9daf63 323 float error;
phgbartels 13:95a4bb9daf63 324 static float prev_error = 0;
phgbartels 13:95a4bb9daf63 325 float out_p = 0;
phgbartels 13:95a4bb9daf63 326 float out_d = 0;
phgbartels 13:95a4bb9daf63 327 error = (setpoint-measurement);
phgbartels 13:95a4bb9daf63 328 out_p = error*K_P_motor2;
phgbartels 13:95a4bb9daf63 329 out_d = (error-prev_error)*K_D_motor2;
phgbartels 13:95a4bb9daf63 330 prev_error = error;
phgbartels 13:95a4bb9daf63 331 return out_p + out_d;
phgbartels 13:95a4bb9daf63 332 }//float pid
phgbartels 13:95a4bb9daf63 333
lauradeheus 8:f733c6a27c15 334 void clamp(float* in, float min, float max)
phgbartels 0:2386012c6594 335 {
phgbartels 11:687aa4656a6e 336 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min;
lauradeheus 8:f733c6a27c15 337 }//void clamp
phgbartels 11:687aa4656a6e 338
phgbartels 0:2386012c6594 339 void statemachinefunction()
phgbartels 0:2386012c6594 340 {
phgbartels 12:3fec73bc3318 341 statetimer.reset();
phgbartels 12:3fec73bc3318 342 //pc.printf(".");
phgbartels 0:2386012c6594 343 switch(state) {
phgbartels 0:2386012c6594 344 case RUST: {
phgbartels 0:2386012c6594 345 rust();
phgbartels 0:2386012c6594 346 /*voorwaarde wanneer hij door kan naar de volgende case*/
phgbartels 11:687aa4656a6e 347 if (current_herhalingen == 100 && EMG == 1) {
phgbartels 0:2386012c6594 348 current_herhalingen = 0;
phgbartels 0:2386012c6594 349 previous_herhalingen = 0;
lauradeheus 9:7e9b63fe8988 350 state = EMG_KALIBRATIE;
lauradeheus 9:7e9b63fe8988 351 EMG = 0; //door EMG op 0 te zetten word deze loop nooit meer doorlopen, daarna zal altijd else worden uitgevoerd. Wat ook gelijk het kalibreren van de arm overslaat. Men kan na 1 keer kalibreren dus vaker achter elkaar schieten
phgbartels 11:687aa4656a6e 352 }//if (current_herhalingen == 100 && EMG == 1)
phgbartels 11:687aa4656a6e 353 else if(current_herhalingen == 100) {
phgbartels 11:687aa4656a6e 354 current_herhalingen = 0;
phgbartels 11:687aa4656a6e 355 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 356 state = METEN_HOOGTE;
lauradeheus 9:7e9b63fe8988 357 }//else
phgbartels 1:b08ac32d1ddc 358 break;
phgbartels 11:687aa4656a6e 359 }//case RUST:
phgbartels 0:2386012c6594 360
phgbartels 11:687aa4656a6e 361 case EMG_KALIBRATIE: {
phgbartels 0:2386012c6594 362 emg_kalibratie();
phgbartels 11:687aa4656a6e 363 if (current_herhalingen >=1000) { /*waarom >= en niet ==?*/
lauradeheus 7:7e3e183bf063 364 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 365 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 366 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 367 doel = 0;
lauradeheus 9:7e9b63fe8988 368 state = ARM_KALIBRATIE;
phgbartels 11:687aa4656a6e 369 }//if (current_herhalingen >=1000)
lauradeheus 7:7e3e183bf063 370 break;
lauradeheus 8:f733c6a27c15 371 }//case EMG_KALIBRATIE
phgbartels 11:687aa4656a6e 372
phgbartels 11:687aa4656a6e 373 case ARM_KALIBRATIE: {
lauradeheus 9:7e9b63fe8988 374 arm_kalibratie();
phgbartels 11:687aa4656a6e 375 if (aantal_pieken == 1) {
phgbartels 0:2386012c6594 376 current_herhalingen = 0;
phgbartels 0:2386012c6594 377 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 378 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 379 doel = 0;
phgbartels 11:687aa4656a6e 380 state = METEN_HOOGTE;
phgbartels 11:687aa4656a6e 381 }//if (current_herhalingen == 100)
phgbartels 11:687aa4656a6e 382 break;
phgbartels 11:687aa4656a6e 383 }//case ARM_KALIBRATIE:
phgbartels 11:687aa4656a6e 384
phgbartels 11:687aa4656a6e 385 case METEN_HOOGTE: {
phgbartels 11:687aa4656a6e 386 meten_hoogte();
phgbartels 11:687aa4656a6e 387 if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) {
lauradeheus 7:7e3e183bf063 388 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 389 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 390 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 391 doel = 0;
phgbartels 11:687aa4656a6e 392 //doel_hoogte = 0;
phgbartels 11:687aa4656a6e 393 state = METEN_RICHTING;
phgbartels 11:687aa4656a6e 394 }//if (current_herhalingen == 2800 && aantal_pieken == 1)
phgbartels 11:687aa4656a6e 395 else if (current_herhalingen == 2200) {
phgbartels 11:687aa4656a6e 396 current_herhalingen = 0;
phgbartels 11:687aa4656a6e 397 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 398 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 399 doel = 0;
lauradeheus 9:7e9b63fe8988 400 state = METEN_HOOGTE;
lauradeheus 8:f733c6a27c15 401 }///else
phgbartels 1:b08ac32d1ddc 402 break;
lauradeheus 8:f733c6a27c15 403 }//case METEN_HOOGTE
phgbartels 0:2386012c6594 404
phgbartels 11:687aa4656a6e 405 case METEN_RICHTING: {
phgbartels 0:2386012c6594 406 meten_richting();
phgbartels 11:687aa4656a6e 407 if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) {
phgbartels 0:2386012c6594 408 current_herhalingen = 0;
phgbartels 0:2386012c6594 409 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 410 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 411 doel = 0;
lauradeheus 9:7e9b63fe8988 412 state = INSTELLEN_RICHTING;
phgbartels 11:687aa4656a6e 413 }//if (current_herhalingen == 2800 && aantal_pieken == 1)
phgbartels 11:687aa4656a6e 414 else if (current_herhalingen == 2200) {
lauradeheus 7:7e3e183bf063 415 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 416 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 417 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 418 doel = 0;
lauradeheus 9:7e9b63fe8988 419 state = METEN_RICHTING;
lauradeheus 9:7e9b63fe8988 420 }///else
phgbartels 1:b08ac32d1ddc 421 break;
lauradeheus 8:f733c6a27c15 422 }//case METEN_RICHTING
phgbartels 0:2386012c6594 423
phgbartels 13:95a4bb9daf63 424
phgbartels 13:95a4bb9daf63 425 case TEST: {
phgbartels 13:95a4bb9daf63 426 state = RUST;
phgbartels 13:95a4bb9daf63 427 break;
phgbartels 13:95a4bb9daf63 428 }
phgbartels 13:95a4bb9daf63 429
phgbartels 11:687aa4656a6e 430 case INSTELLEN_RICHTING: {
phgbartels 13:95a4bb9daf63 431 //instellen_richting();
phgbartels 13:95a4bb9daf63 432 if (doel_richting == 1) {
phgbartels 13:95a4bb9daf63 433 position2_setpoint = 0;
phgbartels 13:95a4bb9daf63 434 } else if (doel_richting ==2) {
phgbartels 13:95a4bb9daf63 435 position2_setpoint = 14.5;
phgbartels 13:95a4bb9daf63 436 } else {
phgbartels 13:95a4bb9daf63 437 position2_setpoint = 25;
phgbartels 13:95a4bb9daf63 438 }
phgbartels 13:95a4bb9daf63 439
phgbartels 13:95a4bb9daf63 440
phgbartels 13:95a4bb9daf63 441 translatie(position2_setpoint, 10, 0.1);
phgbartels 13:95a4bb9daf63 442
phgbartels 13:95a4bb9daf63 443 if (current_herhalingen >= 400) {
phgbartels 13:95a4bb9daf63 444 current_herhalingen = 0;
phgbartels 13:95a4bb9daf63 445 previous_herhalingen = 0;
lauradeheus 9:7e9b63fe8988 446 doel_richting = 0;
phgbartels 13:95a4bb9daf63 447 state = RETURN2RUST;
phgbartels 13:95a4bb9daf63 448 }//if (current_herhalingen == 100
phgbartels 11:687aa4656a6e 449 break;
lauradeheus 8:f733c6a27c15 450 }//case INSTELLEN_RICHTING
phgbartels 0:2386012c6594 451
phgbartels 11:687aa4656a6e 452 case SLAAN: {
lauradeheus 8:f733c6a27c15 453 GotoPosition(1.5 ,8, 0.1);
phgbartels 11:687aa4656a6e 454 if (current_herhalingen == 400) {
phgbartels 0:2386012c6594 455 current_herhalingen = 0;
phgbartels 0:2386012c6594 456 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 457 prev_setpoint =0;
phgbartels 2:de7b6c1d67c4 458 setpoint =0;
lauradeheus 9:7e9b63fe8988 459 state = RETURN2RUST;
phgbartels 11:687aa4656a6e 460 }//if (current_herhalingen == 100)
phgbartels 11:687aa4656a6e 461 break;
lauradeheus 8:f733c6a27c15 462 }//case SLAAN
phgbartels 0:2386012c6594 463
phgbartels 11:687aa4656a6e 464 case RETURN2RUST: {
phgbartels 13:95a4bb9daf63 465 translatie(0,10,0.1);
phgbartels 13:95a4bb9daf63 466 /*GotoPosition(0,4, 0.05);
lauradeheus 9:7e9b63fe8988 467 doel_richting = 0;
phgbartels 11:687aa4656a6e 468 doel_hoogte = 0;
phgbartels 11:687aa4656a6e 469 if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) {
lauradeheus 9:7e9b63fe8988 470 state = RUST;
lauradeheus 9:7e9b63fe8988 471 current_herhalingen = 0;
lauradeheus 9:7e9b63fe8988 472 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 473 current_herhalingen = 0;
lauradeheus 9:7e9b63fe8988 474 current_herhalingen = 0;
phgbartels 11:687aa4656a6e 475 }//if (current_herhalingen == 100)
phgbartels 13:95a4bb9daf63 476 */
phgbartels 1:b08ac32d1ddc 477 break;
lauradeheus 8:f733c6a27c15 478 }// case RETURN2RUST
phgbartels 11:687aa4656a6e 479
phgbartels 0:2386012c6594 480 default: {
phgbartels 0:2386012c6594 481 state = RUST;
lauradeheus 8:f733c6a27c15 482 }//default
phgbartels 0:2386012c6594 483
phgbartels 0:2386012c6594 484 }//switch(state)
phgbartels 0:2386012c6594 485 }//void statemachinefunction
phgbartels 0:2386012c6594 486
phgbartels 0:2386012c6594 487
phgbartels 11:687aa4656a6e 488 void screenupdate()
phgbartels 11:687aa4656a6e 489 {
phgbartels 11:687aa4656a6e 490 if(state==RUST) {
phgbartels 11:687aa4656a6e 491 lcd.cls();
phgbartels 0:2386012c6594 492 lcd.locate(0,0);
phgbartels 5:4842219cb77c 493 lcd.printf("V.I.C.T.O.R.Y."); //regel 1 LCD scherm
phgbartels 0:2386012c6594 494 lcd.locate(0,1);
phgbartels 0:2386012c6594 495 lcd.printf(" GROEP 7 ");
lauradeheus 8:f733c6a27c15 496 }//if(state==RUST)
phgbartels 11:687aa4656a6e 497
phgbartels 11:687aa4656a6e 498 else if(state==EMG_KALIBRATIE) {
lauradeheus 6:a7379a681adf 499 lcd.cls();
lauradeheus 6:a7379a681adf 500 lcd.locate(0,0);
lauradeheus 6:a7379a681adf 501 lcd.printf("Max. aanspannen");
phgbartels 11:687aa4656a6e 502 if(current_herhalingen<=200) {
phgbartels 11:687aa4656a6e 503 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 504 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 505 }//if(current_herhalingen<=200)
phgbartels 11:687aa4656a6e 506 else if(current_herhalingen<=400) {
phgbartels 11:687aa4656a6e 507 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 508 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 509 }//else if(current_herhalingen<=400)
phgbartels 11:687aa4656a6e 510 else if(current_herhalingen<=600) {
phgbartels 11:687aa4656a6e 511 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 512 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 513 }//else if(current_herhalingen<=600)
phgbartels 11:687aa4656a6e 514 else if(current_herhalingen<=800) {
phgbartels 11:687aa4656a6e 515 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 516 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 517 }//else if(current_herhalingen<=800)
phgbartels 11:687aa4656a6e 518 else if(current_herhalingen<=1000) {
phgbartels 11:687aa4656a6e 519 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 520 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 521 }//else if(current_herhalingen<=1000)
lauradeheus 8:f733c6a27c15 522 }//else if(state==EMG_KALIBRATIE)
phgbartels 11:687aa4656a6e 523
phgbartels 11:687aa4656a6e 524 else if(state==ARM_KALIBRATIE) {
lauradeheus 9:7e9b63fe8988 525 lcd.cls();
lauradeheus 9:7e9b63fe8988 526 lcd.locate(0,0);
lauradeheus 9:7e9b63fe8988 527 lcd.printf("Set arm to zero");
phgbartels 11:687aa4656a6e 528 lcd.locate(0,1);
lauradeheus 9:7e9b63fe8988 529 lcd.printf("Klaar? Span aan");
lauradeheus 9:7e9b63fe8988 530 }//else if(state==ARM_KALIBRATIE)
phgbartels 11:687aa4656a6e 531
phgbartels 11:687aa4656a6e 532 else if(state==METEN_HOOGTE) {
lauradeheus 7:7e3e183bf063 533 lcd.cls();
phgbartels 11:687aa4656a6e 534 if(current_herhalingen<=200) {
lauradeheus 7:7e3e183bf063 535 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 536 lcd.printf("Hoogte bepalen:");
lauradeheus 7:7e3e183bf063 537 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 538 lcd.printf("span aan per vak");
lauradeheus 8:f733c6a27c15 539 }//if(current_herhalingen<=200){
phgbartels 11:687aa4656a6e 540 else if(200<=current_herhalingen && current_herhalingen<1200) {
lauradeheus 7:7e3e183bf063 541 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 542 lcd.printf("Vak %d",doel);
phgbartels 11:687aa4656a6e 543 if(current_herhalingen<=400) {
phgbartels 11:687aa4656a6e 544 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 545 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 546 }//if(current_herhalingen<=400)
phgbartels 11:687aa4656a6e 547 else if(current_herhalingen<=600) {
phgbartels 11:687aa4656a6e 548 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 549 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 550 }//else if(current_herhalingen<=600)
phgbartels 11:687aa4656a6e 551 else if(current_herhalingen<=800) {
phgbartels 11:687aa4656a6e 552 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 553 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 554 }//else if(current_herhalingen<=800)
phgbartels 11:687aa4656a6e 555 else if(current_herhalingen<=1000) {
phgbartels 11:687aa4656a6e 556 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 557 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 558 }//else if(current_herhalingen<=1000)
phgbartels 11:687aa4656a6e 559 else if(current_herhalingen<1200) {
phgbartels 11:687aa4656a6e 560 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 561 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 562 }//else if(current_herhalingen<=1200)
lauradeheus 8:f733c6a27c15 563 }//else if(200<=current_herhalingen<=1200)
phgbartels 11:687aa4656a6e 564 else if(current_herhalingen<=2200) {
lauradeheus 7:7e3e183bf063 565 lcd.locate(0,0);
lauradeheus 9:7e9b63fe8988 566 lcd.printf("Vak %d akkoord?",doel_hoogte);
lauradeheus 7:7e3e183bf063 567 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 568 lcd.printf("Span aan");
lauradeheus 8:f733c6a27c15 569 }//else if(current_herhalingen<=1600){
phgbartels 11:687aa4656a6e 570 else {
lauradeheus 7:7e3e183bf063 571 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 572 lcd.printf("Opnieuw hoogte");
lauradeheus 7:7e3e183bf063 573 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 574 lcd.printf("bepalen");
lauradeheus 8:f733c6a27c15 575 }//else{
lauradeheus 8:f733c6a27c15 576 }//else if(state==METEN_HOOGTE){
phgbartels 11:687aa4656a6e 577
phgbartels 11:687aa4656a6e 578 else if(state==METEN_RICHTING) {
lauradeheus 7:7e3e183bf063 579 lcd.cls();
phgbartels 11:687aa4656a6e 580 if(current_herhalingen<=200) {
lauradeheus 7:7e3e183bf063 581 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 582 lcd.printf("Richting bepalen:");
lauradeheus 7:7e3e183bf063 583 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 584 lcd.printf("span aan per vak");
lauradeheus 8:f733c6a27c15 585 }//if(current_herhalingen<=200)
phgbartels 11:687aa4656a6e 586 else if(200<=current_herhalingen && current_herhalingen<1200) {
lauradeheus 7:7e3e183bf063 587 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 588 lcd.printf("Vak %d",doel);
phgbartels 11:687aa4656a6e 589 if(current_herhalingen<=400) {
phgbartels 11:687aa4656a6e 590 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 591 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 592 }//if(current_herhalingen<=400)
phgbartels 11:687aa4656a6e 593 else if(current_herhalingen<=600) {
phgbartels 11:687aa4656a6e 594 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 595 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 596 }//else if(current_herhalingen<=600)
phgbartels 11:687aa4656a6e 597 else if(current_herhalingen<=800) {
phgbartels 11:687aa4656a6e 598 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 599 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 600 }//else if(current_herhalingen<=800)
phgbartels 11:687aa4656a6e 601 else if(current_herhalingen<=1000) {
phgbartels 11:687aa4656a6e 602 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 603 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 604 }//else if(current_herhalingen<=1000)
phgbartels 11:687aa4656a6e 605 else if(current_herhalingen<1200) {
phgbartels 11:687aa4656a6e 606 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 607 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 608 }//else if(current_herhalingen<=1200)
lauradeheus 8:f733c6a27c15 609 }//else if(200<=current_herhalingen<=1200)
phgbartels 11:687aa4656a6e 610 else if(current_herhalingen<=2200) {
lauradeheus 7:7e3e183bf063 611 lcd.locate(0,0);
lauradeheus 9:7e9b63fe8988 612 lcd.printf("Vak %d akkoord?",doel_richting);
lauradeheus 7:7e3e183bf063 613 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 614 lcd.printf("Span aan");
lauradeheus 8:f733c6a27c15 615 }//else if(current_herhalingen<=1600)
phgbartels 11:687aa4656a6e 616 else {
lauradeheus 7:7e3e183bf063 617 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 618 lcd.printf("Opnieuw richting");
lauradeheus 7:7e3e183bf063 619 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 620 lcd.printf("bepalen");
lauradeheus 8:f733c6a27c15 621 }//else
lauradeheus 8:f733c6a27c15 622 }//else if(state==METEN_RICHTING){
phgbartels 11:687aa4656a6e 623
phgbartels 11:687aa4656a6e 624 else if(state==INSTELLEN_RICHTING) {
phgbartels 10:cd89569cd847 625 lcd.cls();
phgbartels 10:cd89569cd847 626 lcd.locate(0,0);
phgbartels 10:cd89569cd847 627 lcd.printf("Instellen hoek");
phgbartels 10:cd89569cd847 628 lcd.locate(0,1);
phgbartels 12:3fec73bc3318 629 lcd.printf("Even wachten...");
phgbartels 10:cd89569cd847 630 }//else if(state==INSTELLEN_RICHTING){
phgbartels 11:687aa4656a6e 631
phgbartels 11:687aa4656a6e 632 else if(state==SLAAN) {
phgbartels 10:cd89569cd847 633 lcd.cls();
phgbartels 10:cd89569cd847 634 lcd.locate(0,0);
phgbartels 10:cd89569cd847 635 lcd.printf("Slaan, pas op");
phgbartels 10:cd89569cd847 636 lcd.locate(0,1);
phgbartels 10:cd89569cd847 637 lcd.printf("Let's pray");
phgbartels 10:cd89569cd847 638 }//else if(state==INSTELLEN_RICHTING){
phgbartels 11:687aa4656a6e 639
phgbartels 11:687aa4656a6e 640 else if(state==RETURN2RUST) {
phgbartels 10:cd89569cd847 641 lcd.cls();
phgbartels 10:cd89569cd847 642 lcd.locate(0,0);
phgbartels 10:cd89569cd847 643 lcd.printf("Terug naar");
phgbartels 10:cd89569cd847 644 lcd.locate(0,1);
phgbartels 10:cd89569cd847 645 lcd.printf("0-positie");
phgbartels 10:cd89569cd847 646 }//else if(state==INSTELLEN_RICHTING){
phgbartels 11:687aa4656a6e 647
phgbartels 11:687aa4656a6e 648 else {
phgbartels 0:2386012c6594 649 lcd.cls();
phgbartels 0:2386012c6594 650 lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
lauradeheus 8:f733c6a27c15 651 }//else{
phgbartels 0:2386012c6594 652 }
phgbartels 0:2386012c6594 653
phgbartels 11:687aa4656a6e 654 int main()
phgbartels 11:687aa4656a6e 655 {
phgbartels 13:95a4bb9daf63 656 pwm_motor1.period_us(100);
phgbartels 13:95a4bb9daf63 657 pwm_motor2.period_us(100);
phgbartels 4:377ddd65e4a6 658 pc.baud(115200);
phgbartels 12:3fec73bc3318 659 statetimer.start();
lauradeheus 6:a7379a681adf 660 arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
lauradeheus 6:a7379a681adf 661 arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
lauradeheus 6:a7379a681adf 662 arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
lauradeheus 6:a7379a681adf 663 arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
phgbartels 13:95a4bb9daf63 664 state = TEST;
phgbartels 4:377ddd65e4a6 665 statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
phgbartels 12:3fec73bc3318 666 //screen.attach(&screenupdate, 0.2);
phgbartels 13:95a4bb9daf63 667 while(1) {
phgbartels 12:3fec73bc3318 668 screenupdate();
phgbartels 12:3fec73bc3318 669 wait(0.2);
phgbartels 12:3fec73bc3318 670 };
phgbartels 0:2386012c6594 671 }