Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
main.cpp@15:2d2278caf861, 2014-11-03 (annotated)
- Committer:
- phgbartels
- Date:
- Mon Nov 03 23:11:57 2014 +0000
- Revision:
- 15:2d2278caf861
- Parent:
- 14:b1de410384c2
- Child:
- 16:9e4f61aab351
Werken script, alles doet het met alle waarden
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 | #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 |
phgbartels | 15:2d2278caf861 | 28 | #define K_P (2000) |
phgbartels | 14:b1de410384c2 | 29 | #define K_I (0) |
phgbartels | 14:b1de410384c2 | 30 | #define K_D (0.1) |
phgbartels | 13:95a4bb9daf63 | 31 | #define K_P_motor2 (75) |
phgbartels | 13:95a4bb9daf63 | 32 | #define K_D_motor2 (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; |
lauradeheus | 6:a7379a681adf | 88 | float prev_setpoint = 0; |
lauradeheus | 6:a7379a681adf | 89 | float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203}; |
lauradeheus | 6:a7379a681adf | 90 | float lowpass_1_states[4]; |
lauradeheus | 6:a7379a681adf | 91 | float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684}; |
lauradeheus | 6:a7379a681adf | 92 | float lowpass_2_states[4]; |
lauradeheus | 6:a7379a681adf | 93 | float highpass_const[] = {0.638945525159022 , -1.277891050318045 , 0.638945525159022 , 1.142980502539901 , -0.412801598096189}; |
lauradeheus | 6:a7379a681adf | 94 | float highpass_states[4]; |
lauradeheus | 6:a7379a681adf | 95 | float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362}; |
lauradeheus | 6:a7379a681adf | 96 | float notch_states[4]; |
lauradeheus | 6:a7379a681adf | 97 | float emg_filtered; |
lauradeheus | 6:a7379a681adf | 98 | float emg_max = 0; |
lauradeheus | 6:a7379a681adf | 99 | float emg_treshhold_laag = 0; |
lauradeheus | 6:a7379a681adf | 100 | float emg_treshhold_hoog = 0; |
phgbartels | 11:687aa4656a6e | 101 | float marge = 0; |
lauradeheus | 9:7e9b63fe8988 | 102 | float PWM_richting1; |
lauradeheus | 9:7e9b63fe8988 | 103 | float PWM_richting2; |
lauradeheus | 9:7e9b63fe8988 | 104 | float PWM_richting3; |
phgbartels | 13:95a4bb9daf63 | 105 | float position2_setpoint; |
phgbartels | 0:2386012c6594 | 106 | |
phgbartels | 13:95a4bb9daf63 | 107 | 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 | 108 | state_t state = RUST; |
phgbartels | 0:2386012c6594 | 109 | |
phgbartels | 11:687aa4656a6e | 110 | void rust() |
phgbartels | 11:687aa4656a6e | 111 | { |
phgbartels | 11:687aa4656a6e | 112 | current_herhalingen = previous_herhalingen + 1; |
phgbartels | 11:687aa4656a6e | 113 | previous_herhalingen = current_herhalingen; |
lauradeheus | 8:f733c6a27c15 | 114 | }//void rust |
phgbartels | 11:687aa4656a6e | 115 | |
phgbartels | 11:687aa4656a6e | 116 | void pieken_tellen() |
phgbartels | 11:687aa4656a6e | 117 | { |
phgbartels | 11:687aa4656a6e | 118 | if (emg_filtered>=emg_treshhold_hoog) { |
lauradeheus | 6:a7379a681adf | 119 | aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is. |
lauradeheus | 8:f733c6a27c15 | 120 | }//if |
phgbartels | 11:687aa4656a6e | 121 | if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt |
lauradeheus | 6:a7379a681adf | 122 | aanspan=false; |
lauradeheus | 6:a7379a681adf | 123 | aantal_pieken++; |
phgbartels | 10:cd89569cd847 | 124 | doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1; |
phgbartels | 11:687aa4656a6e | 125 | |
lauradeheus | 8:f733c6a27c15 | 126 | }//if |
lauradeheus | 8:f733c6a27c15 | 127 | }//void pieken_tellen |
lauradeheus | 6:a7379a681adf | 128 | |
phgbartels | 11:687aa4656a6e | 129 | void emg_filtering() |
phgbartels | 11:687aa4656a6e | 130 | { |
lauradeheus | 6:a7379a681adf | 131 | uint16_t emg_value; |
lauradeheus | 6:a7379a681adf | 132 | float emg_value_f32; |
lauradeheus | 6:a7379a681adf | 133 | 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 | 134 | emg_value_f32 = emg.read(); |
phgbartels | 11:687aa4656a6e | 135 | |
lauradeheus | 6:a7379a681adf | 136 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 ); |
lauradeheus | 6:a7379a681adf | 137 | arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 ); |
lauradeheus | 6:a7379a681adf | 138 | arm_biquad_cascade_df1_f32(¬ch, &emg_filtered, &emg_filtered, 1); |
lauradeheus | 6:a7379a681adf | 139 | emg_filtered = fabs(emg_filtered); |
lauradeheus | 6:a7379a681adf | 140 | arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 ); |
phgbartels | 10:cd89569cd847 | 141 | //scope.set(0,emg_value); //uint value |
phgbartels | 10:cd89569cd847 | 142 | //scope.set(1,emg_filtered); //processed float |
phgbartels | 11:687aa4656a6e | 143 | if(state!=EMG_KALIBRATIE) { |
lauradeheus | 6:a7379a681adf | 144 | pieken_tellen(); |
lauradeheus | 8:f733c6a27c15 | 145 | }//if |
phgbartels | 10:cd89569cd847 | 146 | //pc.printf("%d\n\r",doel); |
phgbartels | 11:687aa4656a6e | 147 | scope.set(0, doel); |
phgbartels | 10:cd89569cd847 | 148 | scope.set(1, aantal_pieken); |
lauradeheus | 9:7e9b63fe8988 | 149 | scope.send(); |
lauradeheus | 8:f733c6a27c15 | 150 | }//void emg_filtering() |
lauradeheus | 6:a7379a681adf | 151 | |
phgbartels | 11:687aa4656a6e | 152 | void emg_max_meting() |
phgbartels | 11:687aa4656a6e | 153 | { |
lauradeheus | 6:a7379a681adf | 154 | emg_filtering(); |
phgbartels | 11:687aa4656a6e | 155 | if (emg_filtered>=emg_max) { |
lauradeheus | 6:a7379a681adf | 156 | emg_max=emg_filtered; |
lauradeheus | 8:f733c6a27c15 | 157 | }//if |
phgbartels | 11:687aa4656a6e | 158 | emg_treshhold_laag = 0.4*emg_max; |
phgbartels | 11:687aa4656a6e | 159 | emg_treshhold_hoog = 0.7*emg_max; |
lauradeheus | 8:f733c6a27c15 | 160 | }//void emg_max_meting |
lauradeheus | 6:a7379a681adf | 161 | |
phgbartels | 11:687aa4656a6e | 162 | void akkoord_geven() |
phgbartels | 11:687aa4656a6e | 163 | { |
lauradeheus | 9:7e9b63fe8988 | 164 | emg_filtering(); |
lauradeheus | 9:7e9b63fe8988 | 165 | } |
lauradeheus | 9:7e9b63fe8988 | 166 | |
phgbartels | 11:687aa4656a6e | 167 | void emg_kalibratie() |
phgbartels | 11:687aa4656a6e | 168 | { |
lauradeheus | 7:7e3e183bf063 | 169 | //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 | 170 | current_herhalingen = previous_herhalingen + 1; |
phgbartels | 11:687aa4656a6e | 171 | previous_herhalingen = current_herhalingen; |
lauradeheus | 7:7e3e183bf063 | 172 | //} |
phgbartels | 11:687aa4656a6e | 173 | if(current_herhalingen<=1000) { |
lauradeheus | 6:a7379a681adf | 174 | emg_max_meting(); |
lauradeheus | 8:f733c6a27c15 | 175 | }//if |
lauradeheus | 8:f733c6a27c15 | 176 | }//void emg_kalibratie |
phgbartels | 0:2386012c6594 | 177 | |
phgbartels | 11:687aa4656a6e | 178 | void arm_kalibratie() |
phgbartels | 11:687aa4656a6e | 179 | { |
lauradeheus | 7:7e3e183bf063 | 180 | //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 | 181 | motor1.setPosition(0); |
phgbartels | 13:95a4bb9daf63 | 182 | //motor2.setPosition(0); |
phgbartels | 11:687aa4656a6e | 183 | akkoord_geven(); |
lauradeheus | 8:f733c6a27c15 | 184 | }//void arm_kalibratie |
lauradeheus | 7:7e3e183bf063 | 185 | |
phgbartels | 11:687aa4656a6e | 186 | void doel_bepaling() |
phgbartels | 11:687aa4656a6e | 187 | { |
phgbartels | 11:687aa4656a6e | 188 | if(200<=current_herhalingen && current_herhalingen <1200) { |
lauradeheus | 7:7e3e183bf063 | 189 | emg_filtering(); |
phgbartels | 10:cd89569cd847 | 190 | doel = aantal_pieken-(((aantal_pieken-1)/3)*3); |
lauradeheus | 8:f733c6a27c15 | 191 | }//if |
phgbartels | 11:687aa4656a6e | 192 | else if(current_herhalingen == 1200 && state==METEN_HOOGTE) { |
lauradeheus | 9:7e9b63fe8988 | 193 | doel_hoogte = doel; |
lauradeheus | 9:7e9b63fe8988 | 194 | aantal_pieken = 0; |
phgbartels | 11:687aa4656a6e | 195 | doel = 0; |
phgbartels | 11:687aa4656a6e | 196 | } else if(current_herhalingen == 1200 && state==METEN_RICHTING) { |
lauradeheus | 9:7e9b63fe8988 | 197 | doel_richting = doel; |
lauradeheus | 9:7e9b63fe8988 | 198 | aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt. |
phgbartels | 10:cd89569cd847 | 199 | doel = 0; |
phgbartels | 11:687aa4656a6e | 200 | } else if(1200<current_herhalingen && current_herhalingen<=2200) { |
phgbartels | 11:687aa4656a6e | 201 | akkoord_geven(); |
lauradeheus | 8:f733c6a27c15 | 202 | }//else if |
phgbartels | 11:687aa4656a6e | 203 | else { |
phgbartels | 11:687aa4656a6e | 204 | }//else |
lauradeheus | 8:f733c6a27c15 | 205 | }//void doel_bepaling |
phgbartels | 0:2386012c6594 | 206 | |
phgbartels | 11:687aa4656a6e | 207 | void meten_hoogte() |
phgbartels | 11:687aa4656a6e | 208 | { |
phgbartels | 11:687aa4656a6e | 209 | current_herhalingen = previous_herhalingen + 1; |
phgbartels | 11:687aa4656a6e | 210 | previous_herhalingen = current_herhalingen; |
lauradeheus | 7:7e3e183bf063 | 211 | doel_bepaling(); |
lauradeheus | 8:f733c6a27c15 | 212 | }//void meten_hoogte |
lauradeheus | 7:7e3e183bf063 | 213 | |
phgbartels | 11:687aa4656a6e | 214 | void meten_richting() |
phgbartels | 11:687aa4656a6e | 215 | { |
phgbartels | 11:687aa4656a6e | 216 | current_herhalingen = previous_herhalingen + 1; |
phgbartels | 11:687aa4656a6e | 217 | previous_herhalingen = current_herhalingen; |
lauradeheus | 7:7e3e183bf063 | 218 | doel_bepaling(); |
lauradeheus | 8:f733c6a27c15 | 219 | }//void meten_richting |
phgbartels | 0:2386012c6594 | 220 | |
phgbartels | 14:b1de410384c2 | 221 | void GotoPosition (float position_setpoint_rad, float speed_radpersecond) |
phgbartels | 11:687aa4656a6e | 222 | { |
phgbartels | 14:b1de410384c2 | 223 | static float setpoint = 0; |
phgbartels | 14:b1de410384c2 | 224 | if (setpoint < position_setpoint_rad) { |
phgbartels | 15:2d2278caf861 | 225 | setpoint += (0.005 * speed_radpersecond); |
phgbartels | 14:b1de410384c2 | 226 | if (setpoint > position_setpoint_rad) { |
phgbartels | 14:b1de410384c2 | 227 | setpoint = position_setpoint_rad; |
phgbartels | 14:b1de410384c2 | 228 | } |
phgbartels | 14:b1de410384c2 | 229 | } |
phgbartels | 11:687aa4656a6e | 230 | |
phgbartels | 14:b1de410384c2 | 231 | else if (setpoint > position_setpoint_rad) { |
phgbartels | 15:2d2278caf861 | 232 | setpoint -= (0.005 * speed_radpersecond); |
phgbartels | 14:b1de410384c2 | 233 | if (setpoint < position_setpoint_rad) { |
phgbartels | 14:b1de410384c2 | 234 | setpoint = position_setpoint_rad; |
phgbartels | 14:b1de410384c2 | 235 | } |
phgbartels | 14:b1de410384c2 | 236 | } else if (setpoint == position_setpoint_rad) { |
phgbartels | 11:687aa4656a6e | 237 | current_herhalingen = previous_herhalingen + 1; |
phgbartels | 2:de7b6c1d67c4 | 238 | previous_herhalingen = current_herhalingen; |
phgbartels | 14:b1de410384c2 | 239 | } |
phgbartels | 14:b1de410384c2 | 240 | |
phgbartels | 14:b1de410384c2 | 241 | current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is |
phgbartels | 14:b1de410384c2 | 242 | pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa |
phgbartels | 14:b1de410384c2 | 243 | PWM1_percentage = pid(setpoint, pos_motor1_rad); |
phgbartels | 14:b1de410384c2 | 244 | pc.printf("setpoint : %f\r\n",setpoint); |
phgbartels | 14:b1de410384c2 | 245 | pc.printf("pos_motor_1 :%f\r\n", pos_motor1_rad); |
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.)); |
lauradeheus | 8:f733c6a27c15 | 261 | }//void GotoPosition |
phgbartels | 2:de7b6c1d67c4 | 262 | |
phgbartels | 0:2386012c6594 | 263 | float pid(float setpoint, float measurement) |
phgbartels | 0:2386012c6594 | 264 | { |
phgbartels | 0:2386012c6594 | 265 | float error; |
phgbartels | 0:2386012c6594 | 266 | static float prev_error = 0; |
phgbartels | 0:2386012c6594 | 267 | float out_p = 0; |
phgbartels | 0:2386012c6594 | 268 | static float out_i = 0; |
phgbartels | 0:2386012c6594 | 269 | float out_d = 0; |
phgbartels | 0:2386012c6594 | 270 | error = (setpoint-measurement); |
phgbartels | 11:687aa4656a6e | 271 | out_p = error*K_P; |
phgbartels | 0:2386012c6594 | 272 | out_i += error*K_I; |
phgbartels | 0:2386012c6594 | 273 | out_d = (error-prev_error)*K_D; |
phgbartels | 0:2386012c6594 | 274 | clamp(&out_i,-I_LIMIT,I_LIMIT); |
phgbartels | 0:2386012c6594 | 275 | prev_error = error; |
phgbartels | 0:2386012c6594 | 276 | return out_p + out_i + out_d; |
lauradeheus | 8:f733c6a27c15 | 277 | }//float pid |
phgbartels | 0:2386012c6594 | 278 | |
phgbartels | 13:95a4bb9daf63 | 279 | void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2) |
phgbartels | 13:95a4bb9daf63 | 280 | { |
phgbartels | 14:b1de410384c2 | 281 | static float setpoint = 0; |
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 | 14:b1de410384c2 | 426 | state = SLAAN; |
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; |
phgbartels | 14:b1de410384c2 | 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: { |
phgbartels | 15:2d2278caf861 | 453 | GotoPosition(1.9 ,1000); |
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 | 14:b1de410384c2 | 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 | 14:b1de410384c2 | 465 | //translatie(0,10,0.1); |
phgbartels | 15:2d2278caf861 | 466 | GotoPosition(0,0.25); |
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 | 14:b1de410384c2 | 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(¬ch,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 | } |