Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
main.cpp@7:7e3e183bf063, 2014-10-31 (annotated)
- Committer:
- lauradeheus
- Date:
- Fri Oct 31 17:14:30 2014 +0000
- Revision:
- 7:7e3e183bf063
- Parent:
- 6:a7379a681adf
- Child:
- 8:f733c6a27c15
Main-script aanpassen dit weekend
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
phgbartels | 0:2386012c6594 | 1 | /********************************************/ |
phgbartels | 0:2386012c6594 | 2 | /* */ |
phgbartels | 0:2386012c6594 | 3 | /* BRONCODE GROEP 7, MODULE 9, 2014 */ |
phgbartels | 0:2386012c6594 | 4 | /* *-THE SLAP-* */ |
phgbartels | 0:2386012c6594 | 5 | /* */ |
phgbartels | 0:2386012c6594 | 6 | /* -Anne ten Dam */ |
phgbartels | 0:2386012c6594 | 7 | /* -Laura de Heus */ |
phgbartels | 0:2386012c6594 | 8 | /* -Moniek Strijdveen */ |
phgbartels | 0:2386012c6594 | 9 | /* -Bart Arendshorst */ |
phgbartels | 0:2386012c6594 | 10 | /* -Peter Bartels */ |
phgbartels | 0:2386012c6594 | 11 | /********************************************/ |
phgbartels | 0:2386012c6594 | 12 | |
phgbartels | 0:2386012c6594 | 13 | /* |
phgbartels | 0:2386012c6594 | 14 | #include voor alle libraries |
phgbartels | 0:2386012c6594 | 15 | */ |
phgbartels | 0:2386012c6594 | 16 | #include "TextLCD.h" |
phgbartels | 0:2386012c6594 | 17 | #include "mbed.h" |
phgbartels | 0:2386012c6594 | 18 | #include "encoder.h" |
lauradeheus | 6:a7379a681adf | 19 | #include "HIDScope.h" |
phgbartels | 0:2386012c6594 | 20 | #include "PwmOut.h" |
lauradeheus | 6:a7379a681adf | 21 | #include "arm_math.h" |
phgbartels | 0:2386012c6594 | 22 | |
phgbartels | 0:2386012c6594 | 23 | /* |
phgbartels | 0:2386012c6594 | 24 | #define vaste waarden |
phgbartels | 0:2386012c6594 | 25 | */ |
phgbartels | 0:2386012c6594 | 26 | /*definieren pinnen Motor 1*/ |
phgbartels | 0:2386012c6594 | 27 | #define M1_PWM PTA5 |
phgbartels | 0:2386012c6594 | 28 | #define M1_DIR PTA4 |
phgbartels | 0:2386012c6594 | 29 | #define M2_PWM PTC8 |
phgbartels | 0:2386012c6594 | 30 | #define M2_DIR PTC9 |
phgbartels | 0:2386012c6594 | 31 | /*Definieren minimale waarden PWM per motor*/ |
phgbartels | 0:2386012c6594 | 32 | #define PWM1_min_50 0.529 /*met koppelstuk!*/ |
phgbartels | 0:2386012c6594 | 33 | #define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/ |
phgbartels | 0:2386012c6594 | 34 | /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/ |
phgbartels | 0:2386012c6594 | 35 | #define TSAMP 0.005 |
phgbartels | 5:4842219cb77c | 36 | #define K_P (5) |
phgbartels | 5:4842219cb77c | 37 | #define K_I (0.1 * TSAMP) |
phgbartels | 5:4842219cb77c | 38 | #define K_D (0) |
phgbartels | 0:2386012c6594 | 39 | //#define K_D (0.0005 /TSAMP) |
phgbartels | 0:2386012c6594 | 40 | #define I_LIMIT 100. |
phgbartels | 0:2386012c6594 | 41 | #define PI 3.1415926535897 |
phgbartels | 0:2386012c6594 | 42 | #define lengte_arm 0.5 |
phgbartels | 0:2386012c6594 | 43 | |
phgbartels | 0:2386012c6594 | 44 | /* |
phgbartels | 0:2386012c6594 | 45 | Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden. |
phgbartels | 0:2386012c6594 | 46 | */ |
phgbartels | 0:2386012c6594 | 47 | TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7 |
phgbartels | 0:2386012c6594 | 48 | Encoder motor1(PTD3,PTD1); |
phgbartels | 0:2386012c6594 | 49 | Encoder motor2(PTD5, PTD0); |
phgbartels | 0:2386012c6594 | 50 | PwmOut pwm_motor1(M1_PWM); |
phgbartels | 0:2386012c6594 | 51 | PwmOut pwm_motor2(M2_PWM); |
phgbartels | 0:2386012c6594 | 52 | DigitalOut motordir1(M1_DIR); |
phgbartels | 0:2386012c6594 | 53 | DigitalOut motordir2(M2_DIR); |
phgbartels | 0:2386012c6594 | 54 | DigitalOut LEDGREEN(LED_GREEN); |
phgbartels | 0:2386012c6594 | 55 | DigitalOut LEDRED(LED_RED); |
phgbartels | 4:377ddd65e4a6 | 56 | Serial pc(USBTX,USBRX); |
lauradeheus | 6:a7379a681adf | 57 | HIDScope scope(3); |
lauradeheus | 6:a7379a681adf | 58 | AnalogIn emg(PTB1); |
phgbartels | 0:2386012c6594 | 59 | /* |
phgbartels | 0:2386012c6594 | 60 | definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde |
phgbartels | 0:2386012c6594 | 61 | */ |
phgbartels | 0:2386012c6594 | 62 | Ticker statemachine; |
phgbartels | 0:2386012c6594 | 63 | Ticker screen; |
lauradeheus | 6:a7379a681adf | 64 | arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz |
lauradeheus | 6:a7379a681adf | 65 | arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz |
lauradeheus | 6:a7379a681adf | 66 | arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz |
lauradeheus | 6:a7379a681adf | 67 | arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz |
phgbartels | 0:2386012c6594 | 68 | int previous_herhalingen = 0; |
phgbartels | 1:b08ac32d1ddc | 69 | int current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 70 | int PWM2_percentage = 100; |
phgbartels | 0:2386012c6594 | 71 | int aantal_rotaties_1 = 10; |
phgbartels | 0:2386012c6594 | 72 | int aantalcr_1 = 1600; |
phgbartels | 0:2386012c6594 | 73 | int aantalcr_2 = 960; |
phgbartels | 0:2386012c6594 | 74 | int beginpos_motor1; |
phgbartels | 0:2386012c6594 | 75 | int beginpos_motor1_new; |
phgbartels | 0:2386012c6594 | 76 | int beginpos_motor2; |
phgbartels | 0:2386012c6594 | 77 | int beginpos_motor2_new; |
phgbartels | 0:2386012c6594 | 78 | int previous_pos_motor1 = 0; |
phgbartels | 0:2386012c6594 | 79 | int current_pos_motor1; |
phgbartels | 5:4842219cb77c | 80 | int EMG = 1; |
phgbartels | 0:2386012c6594 | 81 | int delta_pos_motor1_puls; |
lauradeheus | 6:a7379a681adf | 82 | int aantal_pieken; |
lauradeheus | 6:a7379a681adf | 83 | int doel; |
lauradeheus | 7:7e3e183bf063 | 84 | int doel_richting; |
lauradeheus | 7:7e3e183bf063 | 85 | int doel_hoogte; |
lauradeheus | 6:a7379a681adf | 86 | bool aanspan; |
phgbartels | 0:2386012c6594 | 87 | void clamp(float * in, float min, float max); |
phgbartels | 0:2386012c6594 | 88 | volatile bool looptimerflag; |
phgbartels | 2:de7b6c1d67c4 | 89 | int16_t gewenste_snelheid = 2; |
phgbartels | 2:de7b6c1d67c4 | 90 | int16_t gewenste_snelheid_rad = 4; |
phgbartels | 2:de7b6c1d67c4 | 91 | int16_t gewenste_snelheid_rad_return = 1; |
phgbartels | 0:2386012c6594 | 92 | float pid(float setpoint, float measurement); |
phgbartels | 0:2386012c6594 | 93 | float pos_motor1_rad; |
phgbartels | 0:2386012c6594 | 94 | float PWM1_percentage = 0; |
phgbartels | 0:2386012c6594 | 95 | float delta_pos_motor1_rad; |
phgbartels | 0:2386012c6594 | 96 | float snelheid_motor1_rad; |
phgbartels | 0:2386012c6594 | 97 | float snelheid_arm_ms; |
phgbartels | 0:2386012c6594 | 98 | float PWM1; |
phgbartels | 0:2386012c6594 | 99 | float PWM2; |
phgbartels | 0:2386012c6594 | 100 | float Speed_motor1; |
phgbartels | 0:2386012c6594 | 101 | float Speed_motor1rad; |
phgbartels | 1:b08ac32d1ddc | 102 | float setpoint = 0; |
lauradeheus | 6:a7379a681adf | 103 | float prev_setpoint = 0; |
lauradeheus | 6:a7379a681adf | 104 | float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203}; |
lauradeheus | 6:a7379a681adf | 105 | float lowpass_1_states[4]; |
lauradeheus | 6:a7379a681adf | 106 | float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684}; |
lauradeheus | 6:a7379a681adf | 107 | float lowpass_2_states[4]; |
lauradeheus | 6:a7379a681adf | 108 | float highpass_const[] = {0.638945525159022 , -1.277891050318045 , 0.638945525159022 , 1.142980502539901 , -0.412801598096189}; |
lauradeheus | 6:a7379a681adf | 109 | float highpass_states[4]; |
lauradeheus | 6:a7379a681adf | 110 | float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362}; |
lauradeheus | 6:a7379a681adf | 111 | float notch_states[4]; |
lauradeheus | 6:a7379a681adf | 112 | float emg_filtered; |
lauradeheus | 6:a7379a681adf | 113 | float emg_max = 0; |
lauradeheus | 6:a7379a681adf | 114 | float emg_treshhold_laag = 0; |
lauradeheus | 6:a7379a681adf | 115 | float emg_treshhold_hoog = 0; |
phgbartels | 0:2386012c6594 | 116 | |
phgbartels | 1:b08ac32d1ddc | 117 | //HIDScope scope(6); |
phgbartels | 0:2386012c6594 | 118 | |
lauradeheus | 7:7e3e183bf063 | 119 | enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES |
phgbartels | 0:2386012c6594 | 120 | state_t state = RUST; |
phgbartels | 0:2386012c6594 | 121 | |
phgbartels | 0:2386012c6594 | 122 | //functies die vanuit de statemachinefunction aangeroepen kunnen worden |
phgbartels | 0:2386012c6594 | 123 | void rust() { |
phgbartels | 0:2386012c6594 | 124 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 125 | } |
phgbartels | 0:2386012c6594 | 126 | |
lauradeheus | 6:a7379a681adf | 127 | void pieken_tellen(){ |
lauradeheus | 6:a7379a681adf | 128 | if (emg_filtered>=emg_treshhold_hoog) |
lauradeheus | 6:a7379a681adf | 129 | { |
lauradeheus | 6:a7379a681adf | 130 | aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is. |
lauradeheus | 6:a7379a681adf | 131 | } |
lauradeheus | 6:a7379a681adf | 132 | if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt |
lauradeheus | 6:a7379a681adf | 133 | { |
lauradeheus | 6:a7379a681adf | 134 | aanspan=false; |
lauradeheus | 6:a7379a681adf | 135 | aantal_pieken++; |
lauradeheus | 6:a7379a681adf | 136 | doel = aantal_pieken-((aantal_pieken/3)*3)+1; |
lauradeheus | 6:a7379a681adf | 137 | } |
lauradeheus | 6:a7379a681adf | 138 | } |
lauradeheus | 6:a7379a681adf | 139 | |
lauradeheus | 6:a7379a681adf | 140 | void emg_filtering() { |
lauradeheus | 6:a7379a681adf | 141 | uint16_t emg_value; |
lauradeheus | 6:a7379a681adf | 142 | float emg_value_f32; |
lauradeheus | 6:a7379a681adf | 143 | 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 | 144 | emg_value_f32 = emg.read(); |
lauradeheus | 6:a7379a681adf | 145 | |
lauradeheus | 6:a7379a681adf | 146 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 ); |
lauradeheus | 6:a7379a681adf | 147 | arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 ); |
lauradeheus | 6:a7379a681adf | 148 | arm_biquad_cascade_df1_f32(¬ch, &emg_filtered, &emg_filtered, 1); |
lauradeheus | 6:a7379a681adf | 149 | emg_filtered = fabs(emg_filtered); |
lauradeheus | 6:a7379a681adf | 150 | arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 ); |
lauradeheus | 6:a7379a681adf | 151 | scope.set(0,emg_value); //uint value |
lauradeheus | 6:a7379a681adf | 152 | scope.set(1,emg_filtered); //processed float |
lauradeheus | 6:a7379a681adf | 153 | scope.set(2,doel); |
lauradeheus | 6:a7379a681adf | 154 | scope.send(); |
lauradeheus | 6:a7379a681adf | 155 | if(state!=EMG_KALIBRATIE) |
lauradeheus | 6:a7379a681adf | 156 | { |
lauradeheus | 6:a7379a681adf | 157 | pieken_tellen(); |
lauradeheus | 6:a7379a681adf | 158 | } |
lauradeheus | 6:a7379a681adf | 159 | } |
lauradeheus | 6:a7379a681adf | 160 | |
lauradeheus | 6:a7379a681adf | 161 | void emg_max_meting(){ |
lauradeheus | 6:a7379a681adf | 162 | emg_filtering(); |
lauradeheus | 6:a7379a681adf | 163 | if (emg_filtered>=emg_max) |
lauradeheus | 6:a7379a681adf | 164 | { |
lauradeheus | 6:a7379a681adf | 165 | emg_max=emg_filtered; |
lauradeheus | 6:a7379a681adf | 166 | } |
lauradeheus | 6:a7379a681adf | 167 | emg_treshhold_laag = 0.3*emg_max; |
lauradeheus | 6:a7379a681adf | 168 | emg_treshhold_hoog = 0.7*emg_max; |
lauradeheus | 6:a7379a681adf | 169 | } |
lauradeheus | 6:a7379a681adf | 170 | |
phgbartels | 0:2386012c6594 | 171 | void emg_kalibratie() { |
lauradeheus | 7:7e3e183bf063 | 172 | //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. |
lauradeheus | 6:a7379a681adf | 173 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
lauradeheus | 7:7e3e183bf063 | 174 | //} |
lauradeheus | 6:a7379a681adf | 175 | if(current_herhalingen<=1000) |
lauradeheus | 6:a7379a681adf | 176 | { |
lauradeheus | 6:a7379a681adf | 177 | emg_max_meting(); |
lauradeheus | 6:a7379a681adf | 178 | } |
phgbartels | 0:2386012c6594 | 179 | } |
phgbartels | 0:2386012c6594 | 180 | |
lauradeheus | 7:7e3e183bf063 | 181 | void arm_kalibratie() { |
lauradeheus | 7:7e3e183bf063 | 182 | //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 | 183 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
lauradeheus | 7:7e3e183bf063 | 184 | motor1.setPosition(0); |
lauradeheus | 7:7e3e183bf063 | 185 | motor2.setPosition(0); |
lauradeheus | 7:7e3e183bf063 | 186 | } |
lauradeheus | 7:7e3e183bf063 | 187 | |
lauradeheus | 7:7e3e183bf063 | 188 | void doel_bepaling() { |
lauradeheus | 7:7e3e183bf063 | 189 | if(200<=current_herhalingen<=1200){ |
lauradeheus | 7:7e3e183bf063 | 190 | emg_filtering(); |
lauradeheus | 7:7e3e183bf063 | 191 | } |
lauradeheus | 7:7e3e183bf063 | 192 | else if(1600<=current_herhalingen<=2200){ |
lauradeheus | 7:7e3e183bf063 | 193 | emg_filtering(); |
lauradeheus | 7:7e3e183bf063 | 194 | } |
lauradeheus | 7:7e3e183bf063 | 195 | else{ |
lauradeheus | 7:7e3e183bf063 | 196 | } |
phgbartels | 0:2386012c6594 | 197 | } |
phgbartels | 0:2386012c6594 | 198 | |
phgbartels | 0:2386012c6594 | 199 | void meten_hoogte() { |
phgbartels | 0:2386012c6594 | 200 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
lauradeheus | 7:7e3e183bf063 | 201 | doel_bepaling(); |
lauradeheus | 7:7e3e183bf063 | 202 | if(1600<=current_herhalingen<=2200 && aantal_pieken==1){ |
lauradeheus | 7:7e3e183bf063 | 203 | doel=doel_hoogte; |
lauradeheus | 7:7e3e183bf063 | 204 | } |
lauradeheus | 7:7e3e183bf063 | 205 | } |
lauradeheus | 7:7e3e183bf063 | 206 | |
lauradeheus | 7:7e3e183bf063 | 207 | void meten_richting() { |
lauradeheus | 7:7e3e183bf063 | 208 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
lauradeheus | 7:7e3e183bf063 | 209 | doel_bepaling(); |
lauradeheus | 7:7e3e183bf063 | 210 | if(1600<=current_herhalingen<=2200 && aantal_pieken==1){ |
lauradeheus | 7:7e3e183bf063 | 211 | doel=doel_richting; |
lauradeheus | 7:7e3e183bf063 | 212 | } |
phgbartels | 0:2386012c6594 | 213 | } |
phgbartels | 0:2386012c6594 | 214 | |
phgbartels | 0:2386012c6594 | 215 | void instellen_richting() { |
phgbartels | 0:2386012c6594 | 216 | current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; |
phgbartels | 0:2386012c6594 | 217 | } |
phgbartels | 0:2386012c6594 | 218 | |
phgbartels | 3:156c3e536ed4 | 219 | void GotoPosition (float position_setpoint_rad, float speed_radpersecond) { |
phgbartels | 5:4842219cb77c | 220 | |
phgbartels | 0:2386012c6594 | 221 | current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is |
phgbartels | 5:4842219cb77c | 222 | //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 | 223 | pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar |
phgbartels | 5:4842219cb77c | 224 | //delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad) |
phgbartels | 2:de7b6c1d67c4 | 225 | //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen |
phgbartels | 2:de7b6c1d67c4 | 226 | //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 | 227 | //scope.set(0, snelheid_motor1_rad); |
phgbartels | 0:2386012c6594 | 228 | |
phgbartels | 0:2386012c6594 | 229 | previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde. |
phgbartels | 0:2386012c6594 | 230 | |
phgbartels | 0:2386012c6594 | 231 | //nu gaan we positie regelen i.p.v. snelheid. |
phgbartels | 5:4842219cb77c | 232 | |
phgbartels | 5:4842219cb77c | 233 | if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop. |
phgbartels | 0:2386012c6594 | 234 | { |
phgbartels | 3:156c3e536ed4 | 235 | speed_radpersecond = 0; |
phgbartels | 4:377ddd65e4a6 | 236 | setpoint = pos_motor1_rad; |
phgbartels | 2:de7b6c1d67c4 | 237 | current_herhalingen = previous_herhalingen + 1; |
phgbartels | 2:de7b6c1d67c4 | 238 | previous_herhalingen = current_herhalingen; |
phgbartels | 4:377ddd65e4a6 | 239 | pc.printf("stop\n\r"); |
phgbartels | 5:4842219cb77c | 240 | PWM1_percentage = 0; |
phgbartels | 0:2386012c6594 | 241 | } |
phgbartels | 4:377ddd65e4a6 | 242 | else if(pos_motor1_rad - position_setpoint_rad < 0) |
phgbartels | 4:377ddd65e4a6 | 243 | { |
phgbartels | 5:4842219cb77c | 244 | setpoint = prev_setpoint +( TSAMP * speed_radpersecond); |
phgbartels | 5:4842219cb77c | 245 | PWM1_percentage = pid(setpoint, pos_motor1_rad); |
phgbartels | 4:377ddd65e4a6 | 246 | } |
phgbartels | 3:156c3e536ed4 | 247 | else |
phgbartels | 4:377ddd65e4a6 | 248 | { |
phgbartels | 5:4842219cb77c | 249 | setpoint = prev_setpoint -( TSAMP * speed_radpersecond); |
phgbartels | 5:4842219cb77c | 250 | PWM1_percentage = pid(setpoint, pos_motor1_rad); |
phgbartels | 4:377ddd65e4a6 | 251 | } |
phgbartels | 0:2386012c6594 | 252 | /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/ |
phgbartels | 5:4842219cb77c | 253 | pc.printf("%f\n\r",PWM1_percentage); |
phgbartels | 4:377ddd65e4a6 | 254 | |
phgbartels | 5:4842219cb77c | 255 | |
phgbartels | 1:b08ac32d1ddc | 256 | //scope.set(1, setpoint-pos_motor1_rad); |
phgbartels | 0:2386012c6594 | 257 | |
phgbartels | 0:2386012c6594 | 258 | if (PWM1_percentage < -100) |
phgbartels | 0:2386012c6594 | 259 | { |
phgbartels | 0:2386012c6594 | 260 | PWM1_percentage = -100; |
phgbartels | 0:2386012c6594 | 261 | } |
phgbartels | 0:2386012c6594 | 262 | else if (PWM1_percentage >100) |
phgbartels | 0:2386012c6594 | 263 | { |
phgbartels | 0:2386012c6594 | 264 | PWM1_percentage =100; |
phgbartels | 0:2386012c6594 | 265 | } |
phgbartels | 0:2386012c6594 | 266 | |
phgbartels | 4:377ddd65e4a6 | 267 | if(PWM1_percentage < 0) |
phgbartels | 0:2386012c6594 | 268 | { |
phgbartels | 0:2386012c6594 | 269 | motordir1 = 1; |
phgbartels | 0:2386012c6594 | 270 | } |
phgbartels | 0:2386012c6594 | 271 | else |
phgbartels | 0:2386012c6594 | 272 | { |
phgbartels | 0:2386012c6594 | 273 | motordir1 = 0; |
phgbartels | 0:2386012c6594 | 274 | } |
phgbartels | 0:2386012c6594 | 275 | |
phgbartels | 4:377ddd65e4a6 | 276 | pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); |
phgbartels | 1:b08ac32d1ddc | 277 | //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); |
phgbartels | 0:2386012c6594 | 278 | prev_setpoint = setpoint; |
phgbartels | 2:de7b6c1d67c4 | 279 | //scope.send(); |
phgbartels | 2:de7b6c1d67c4 | 280 | } |
phgbartels | 2:de7b6c1d67c4 | 281 | |
phgbartels | 0:2386012c6594 | 282 | float pid(float setpoint, float measurement) |
phgbartels | 0:2386012c6594 | 283 | { |
phgbartels | 0:2386012c6594 | 284 | float error; |
phgbartels | 0:2386012c6594 | 285 | static float prev_error = 0; |
phgbartels | 0:2386012c6594 | 286 | float out_p = 0; |
phgbartels | 0:2386012c6594 | 287 | static float out_i = 0; |
phgbartels | 0:2386012c6594 | 288 | float out_d = 0; |
phgbartels | 0:2386012c6594 | 289 | error = (setpoint-measurement); |
phgbartels | 0:2386012c6594 | 290 | out_p = error*K_P; |
phgbartels | 0:2386012c6594 | 291 | out_i += error*K_I; |
phgbartels | 0:2386012c6594 | 292 | out_d = (error-prev_error)*K_D; |
phgbartels | 0:2386012c6594 | 293 | clamp(&out_i,-I_LIMIT,I_LIMIT); |
phgbartels | 0:2386012c6594 | 294 | prev_error = error; |
phgbartels | 1:b08ac32d1ddc | 295 | //scope.set(2, out_p); |
phgbartels | 1:b08ac32d1ddc | 296 | //scope.set(3, out_i); |
phgbartels | 1:b08ac32d1ddc | 297 | //scope.set(4, out_d); |
phgbartels | 0:2386012c6594 | 298 | return out_p + out_i + out_d; |
phgbartels | 0:2386012c6594 | 299 | } |
phgbartels | 0:2386012c6594 | 300 | |
phgbartels | 0:2386012c6594 | 301 | 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 | 302 | // maar de locatie van de variabele. |
phgbartels | 0:2386012c6594 | 303 | { |
phgbartels | 0:2386012c6594 | 304 | *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 | 305 | // *in = het getal dat staat op locatie van in --> waarde van new_pwm |
phgbartels | 0:2386012c6594 | 306 | } |
phgbartels | 0:2386012c6594 | 307 | |
phgbartels | 0:2386012c6594 | 308 | void statemachinefunction() |
phgbartels | 0:2386012c6594 | 309 | { |
phgbartels | 0:2386012c6594 | 310 | switch(state) { |
phgbartels | 0:2386012c6594 | 311 | case RUST: { |
phgbartels | 0:2386012c6594 | 312 | rust(); |
phgbartels | 0:2386012c6594 | 313 | /*voorwaarde wanneer hij door kan naar de volgende case*/ |
phgbartels | 5:4842219cb77c | 314 | if (current_herhalingen == 100 && EMG == 1) |
phgbartels | 0:2386012c6594 | 315 | { |
phgbartels | 0:2386012c6594 | 316 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 317 | previous_herhalingen = 0; |
phgbartels | 1:b08ac32d1ddc | 318 | state = ARM_KALIBRATIE; |
phgbartels | 5:4842219cb77c | 319 | EMG = 0; |
phgbartels | 0:2386012c6594 | 320 | } |
phgbartels | 1:b08ac32d1ddc | 321 | break; |
phgbartels | 0:2386012c6594 | 322 | /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/ |
phgbartels | 0:2386012c6594 | 323 | //if (metingstatus<5); |
phgbartels | 0:2386012c6594 | 324 | // state = ARMKALIBRATIE; |
phgbartels | 0:2386012c6594 | 325 | //if (metingstatus==5); |
phgbartels | 0:2386012c6594 | 326 | // state = METEN_RICHTING; |
phgbartels | 0:2386012c6594 | 327 | //break; |
phgbartels | 0:2386012c6594 | 328 | //} |
phgbartels | 0:2386012c6594 | 329 | } |
phgbartels | 0:2386012c6594 | 330 | |
phgbartels | 0:2386012c6594 | 331 | case ARM_KALIBRATIE: |
phgbartels | 0:2386012c6594 | 332 | { |
phgbartels | 0:2386012c6594 | 333 | arm_kalibratie(); |
phgbartels | 5:4842219cb77c | 334 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 335 | { |
phgbartels | 0:2386012c6594 | 336 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 337 | previous_herhalingen = 0; |
phgbartels | 1:b08ac32d1ddc | 338 | state = EMG_KALIBRATIE; |
phgbartels | 2:de7b6c1d67c4 | 339 | motor1.setPosition(0); |
phgbartels | 2:de7b6c1d67c4 | 340 | motor2.setPosition(0); |
phgbartels | 2:de7b6c1d67c4 | 341 | pwm_motor1.period_us(100); |
phgbartels | 2:de7b6c1d67c4 | 342 | pwm_motor2.period_us(100); |
phgbartels | 0:2386012c6594 | 343 | } |
phgbartels | 1:b08ac32d1ddc | 344 | break; |
phgbartels | 0:2386012c6594 | 345 | } |
phgbartels | 0:2386012c6594 | 346 | |
phgbartels | 0:2386012c6594 | 347 | case EMG_KALIBRATIE: |
phgbartels | 0:2386012c6594 | 348 | { |
phgbartels | 0:2386012c6594 | 349 | emg_kalibratie(); |
lauradeheus | 6:a7379a681adf | 350 | if (current_herhalingen >=1000) |
phgbartels | 0:2386012c6594 | 351 | { |
lauradeheus | 7:7e3e183bf063 | 352 | state = METEN_HOOGTE; |
lauradeheus | 7:7e3e183bf063 | 353 | current_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 354 | previous_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 355 | aantal_pieken = 0; |
lauradeheus | 7:7e3e183bf063 | 356 | doel = 0; |
lauradeheus | 7:7e3e183bf063 | 357 | } |
lauradeheus | 7:7e3e183bf063 | 358 | break; |
lauradeheus | 7:7e3e183bf063 | 359 | } |
lauradeheus | 7:7e3e183bf063 | 360 | |
lauradeheus | 7:7e3e183bf063 | 361 | case METEN_HOOGTE: |
lauradeheus | 7:7e3e183bf063 | 362 | { |
lauradeheus | 7:7e3e183bf063 | 363 | meten_hoogte(); |
lauradeheus | 7:7e3e183bf063 | 364 | if (current_herhalingen == 2800 && aantal_pieken == 1) |
lauradeheus | 7:7e3e183bf063 | 365 | { |
phgbartels | 0:2386012c6594 | 366 | state = METEN_RICHTING; |
phgbartels | 0:2386012c6594 | 367 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 368 | previous_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 369 | aantal_pieken = 0; |
lauradeheus | 7:7e3e183bf063 | 370 | doel = 0; |
lauradeheus | 7:7e3e183bf063 | 371 | } |
lauradeheus | 7:7e3e183bf063 | 372 | else |
lauradeheus | 7:7e3e183bf063 | 373 | { |
lauradeheus | 7:7e3e183bf063 | 374 | state = METEN_HOOGTE; |
lauradeheus | 7:7e3e183bf063 | 375 | current_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 376 | previous_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 377 | aantal_pieken = 0; |
lauradeheus | 7:7e3e183bf063 | 378 | doel = 0; |
phgbartels | 0:2386012c6594 | 379 | } |
phgbartels | 1:b08ac32d1ddc | 380 | break; |
phgbartels | 0:2386012c6594 | 381 | } |
phgbartels | 0:2386012c6594 | 382 | |
phgbartels | 0:2386012c6594 | 383 | case METEN_RICHTING: |
phgbartels | 0:2386012c6594 | 384 | { |
phgbartels | 0:2386012c6594 | 385 | meten_richting(); |
lauradeheus | 7:7e3e183bf063 | 386 | if (current_herhalingen == 2800 && aantal_pieken == 1) |
phgbartels | 0:2386012c6594 | 387 | { |
phgbartels | 0:2386012c6594 | 388 | state = INSTELLEN_RICHTING; |
phgbartels | 0:2386012c6594 | 389 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 390 | previous_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 391 | aantal_pieken = 0; |
lauradeheus | 7:7e3e183bf063 | 392 | doel = 0; |
lauradeheus | 7:7e3e183bf063 | 393 | } |
lauradeheus | 7:7e3e183bf063 | 394 | else |
lauradeheus | 7:7e3e183bf063 | 395 | { |
lauradeheus | 7:7e3e183bf063 | 396 | state = METEN_RICHTING; |
lauradeheus | 7:7e3e183bf063 | 397 | current_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 398 | previous_herhalingen = 0; |
lauradeheus | 7:7e3e183bf063 | 399 | aantal_pieken = 0; |
lauradeheus | 7:7e3e183bf063 | 400 | doel = 0; |
phgbartels | 0:2386012c6594 | 401 | } |
phgbartels | 1:b08ac32d1ddc | 402 | break; |
phgbartels | 0:2386012c6594 | 403 | } |
phgbartels | 0:2386012c6594 | 404 | |
phgbartels | 0:2386012c6594 | 405 | case INSTELLEN_RICHTING: |
phgbartels | 0:2386012c6594 | 406 | { |
phgbartels | 0:2386012c6594 | 407 | instellen_richting(); |
phgbartels | 5:4842219cb77c | 408 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 409 | { |
phgbartels | 0:2386012c6594 | 410 | state = SLAAN; |
phgbartels | 0:2386012c6594 | 411 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 412 | previous_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 413 | } |
phgbartels | 2:de7b6c1d67c4 | 414 | break; |
phgbartels | 1:b08ac32d1ddc | 415 | |
phgbartels | 0:2386012c6594 | 416 | } |
phgbartels | 0:2386012c6594 | 417 | |
phgbartels | 0:2386012c6594 | 418 | case SLAAN: |
phgbartels | 0:2386012c6594 | 419 | { |
phgbartels | 5:4842219cb77c | 420 | GotoPosition(1.5 ,8); |
phgbartels | 5:4842219cb77c | 421 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 422 | { |
phgbartels | 0:2386012c6594 | 423 | state = RETURN2RUST; |
phgbartels | 0:2386012c6594 | 424 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 425 | previous_herhalingen = 0; |
phgbartels | 2:de7b6c1d67c4 | 426 | prev_setpoint =0; |
phgbartels | 2:de7b6c1d67c4 | 427 | setpoint =0; |
phgbartels | 2:de7b6c1d67c4 | 428 | |
phgbartels | 0:2386012c6594 | 429 | } |
phgbartels | 2:de7b6c1d67c4 | 430 | break; |
phgbartels | 0:2386012c6594 | 431 | } |
phgbartels | 0:2386012c6594 | 432 | |
phgbartels | 0:2386012c6594 | 433 | case RETURN2RUST: |
phgbartels | 0:2386012c6594 | 434 | { |
phgbartels | 5:4842219cb77c | 435 | GotoPosition(0,2); |
phgbartels | 5:4842219cb77c | 436 | if (current_herhalingen == 100) |
phgbartels | 0:2386012c6594 | 437 | { |
phgbartels | 0:2386012c6594 | 438 | state = RUST; |
phgbartels | 0:2386012c6594 | 439 | current_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 440 | previous_herhalingen = 0; |
phgbartels | 0:2386012c6594 | 441 | } |
phgbartels | 5:4842219cb77c | 442 | |
phgbartels | 1:b08ac32d1ddc | 443 | break; |
phgbartels | 0:2386012c6594 | 444 | } |
phgbartels | 0:2386012c6594 | 445 | |
phgbartels | 0:2386012c6594 | 446 | default: { |
phgbartels | 0:2386012c6594 | 447 | state = RUST; |
phgbartels | 0:2386012c6594 | 448 | } |
phgbartels | 0:2386012c6594 | 449 | |
phgbartels | 0:2386012c6594 | 450 | }//switch(state) |
phgbartels | 0:2386012c6594 | 451 | }//void statemachinefunction |
phgbartels | 0:2386012c6594 | 452 | |
phgbartels | 0:2386012c6594 | 453 | |
phgbartels | 0:2386012c6594 | 454 | void screenupdate(){ |
phgbartels | 0:2386012c6594 | 455 | if(state==RUST){ |
phgbartels | 0:2386012c6594 | 456 | lcd.cls(); |
phgbartels | 0:2386012c6594 | 457 | lcd.locate(0,0); |
phgbartels | 5:4842219cb77c | 458 | lcd.printf("V.I.C.T.O.R.Y."); //regel 1 LCD scherm |
phgbartels | 0:2386012c6594 | 459 | lcd.locate(0,1); |
phgbartels | 0:2386012c6594 | 460 | lcd.printf(" GROEP 7 "); |
phgbartels | 0:2386012c6594 | 461 | } |
lauradeheus | 6:a7379a681adf | 462 | else if(state==EMG_KALIBRATIE){ |
lauradeheus | 6:a7379a681adf | 463 | lcd.cls(); |
lauradeheus | 6:a7379a681adf | 464 | lcd.locate(0,0); |
lauradeheus | 6:a7379a681adf | 465 | lcd.printf("Max. aanspannen"); |
lauradeheus | 6:a7379a681adf | 466 | if(current_herhalingen<=200){ |
lauradeheus | 6:a7379a681adf | 467 | lcd.locate(0,1); |
lauradeheus | 6:a7379a681adf | 468 | lcd.printf("nog 5 sec."); |
lauradeheus | 6:a7379a681adf | 469 | } |
lauradeheus | 6:a7379a681adf | 470 | else if(current_herhalingen<=400){ |
lauradeheus | 6:a7379a681adf | 471 | lcd.locate(0,1); |
lauradeheus | 6:a7379a681adf | 472 | lcd.printf("nog 4 sec."); |
lauradeheus | 6:a7379a681adf | 473 | } |
lauradeheus | 6:a7379a681adf | 474 | else if(current_herhalingen<=600){ |
lauradeheus | 6:a7379a681adf | 475 | lcd.locate(0,1); |
lauradeheus | 6:a7379a681adf | 476 | lcd.printf("nog 3 sec."); |
lauradeheus | 6:a7379a681adf | 477 | } |
lauradeheus | 6:a7379a681adf | 478 | else if(current_herhalingen<=800){ |
lauradeheus | 6:a7379a681adf | 479 | lcd.locate(0,1); |
lauradeheus | 6:a7379a681adf | 480 | lcd.printf("nog 2 sec."); |
lauradeheus | 6:a7379a681adf | 481 | } |
lauradeheus | 6:a7379a681adf | 482 | else if(current_herhalingen<=1000){ |
lauradeheus | 6:a7379a681adf | 483 | lcd.locate(0,1); |
lauradeheus | 6:a7379a681adf | 484 | lcd.printf("nog 1 sec."); |
lauradeheus | 6:a7379a681adf | 485 | } |
lauradeheus | 6:a7379a681adf | 486 | } |
lauradeheus | 7:7e3e183bf063 | 487 | else if(state==METEN_HOOGTE){ |
lauradeheus | 7:7e3e183bf063 | 488 | lcd.cls(); |
lauradeheus | 7:7e3e183bf063 | 489 | if(current_herhalingen<=200){ |
lauradeheus | 7:7e3e183bf063 | 490 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 491 | lcd.printf("Hoogte bepalen:"); |
lauradeheus | 7:7e3e183bf063 | 492 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 493 | lcd.printf("span aan per vak"); |
lauradeheus | 7:7e3e183bf063 | 494 | } |
lauradeheus | 7:7e3e183bf063 | 495 | else if(200<=current_herhalingen<=1200){ |
lauradeheus | 7:7e3e183bf063 | 496 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 497 | lcd.printf("Vak %d",doel); |
lauradeheus | 7:7e3e183bf063 | 498 | if(current_herhalingen<=400){ |
lauradeheus | 7:7e3e183bf063 | 499 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 500 | lcd.printf("nog 5 sec."); |
lauradeheus | 7:7e3e183bf063 | 501 | } |
lauradeheus | 7:7e3e183bf063 | 502 | else if(current_herhalingen<=600){ |
lauradeheus | 7:7e3e183bf063 | 503 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 504 | lcd.printf("nog 4 sec."); |
lauradeheus | 7:7e3e183bf063 | 505 | } |
lauradeheus | 7:7e3e183bf063 | 506 | else if(current_herhalingen<=800){ |
lauradeheus | 7:7e3e183bf063 | 507 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 508 | lcd.printf("nog 3 sec."); |
lauradeheus | 7:7e3e183bf063 | 509 | } |
lauradeheus | 7:7e3e183bf063 | 510 | else if(current_herhalingen<=1000){ |
lauradeheus | 7:7e3e183bf063 | 511 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 512 | lcd.printf("nog 2 sec."); |
lauradeheus | 7:7e3e183bf063 | 513 | } |
lauradeheus | 7:7e3e183bf063 | 514 | else if(current_herhalingen<=1200){ |
lauradeheus | 7:7e3e183bf063 | 515 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 516 | lcd.printf("nog 1 sec."); |
lauradeheus | 7:7e3e183bf063 | 517 | } |
lauradeheus | 7:7e3e183bf063 | 518 | } |
lauradeheus | 7:7e3e183bf063 | 519 | else if(current_herhalingen<=1600){ |
lauradeheus | 7:7e3e183bf063 | 520 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 521 | lcd.printf("Vak %d akkoord?",doel); |
lauradeheus | 7:7e3e183bf063 | 522 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 523 | lcd.printf("Span aan"); |
lauradeheus | 7:7e3e183bf063 | 524 | } |
lauradeheus | 7:7e3e183bf063 | 525 | else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){ |
lauradeheus | 7:7e3e183bf063 | 526 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 527 | lcd.printf("Vak %d akkoord",doel_hoogte); |
lauradeheus | 7:7e3e183bf063 | 528 | } |
lauradeheus | 7:7e3e183bf063 | 529 | else{ |
lauradeheus | 7:7e3e183bf063 | 530 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 531 | lcd.printf("Opnieuw hoogte"); |
lauradeheus | 7:7e3e183bf063 | 532 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 533 | lcd.printf("bepalen"); |
lauradeheus | 7:7e3e183bf063 | 534 | } |
lauradeheus | 7:7e3e183bf063 | 535 | } |
lauradeheus | 7:7e3e183bf063 | 536 | else if(state==METEN_RICHTING){ |
lauradeheus | 7:7e3e183bf063 | 537 | lcd.cls(); |
lauradeheus | 7:7e3e183bf063 | 538 | if(current_herhalingen<=200){ |
lauradeheus | 7:7e3e183bf063 | 539 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 540 | lcd.printf("Richting bepalen:"); |
lauradeheus | 7:7e3e183bf063 | 541 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 542 | lcd.printf("span aan per vak"); |
lauradeheus | 7:7e3e183bf063 | 543 | } |
lauradeheus | 7:7e3e183bf063 | 544 | else if(200<=current_herhalingen<=1200){ |
lauradeheus | 7:7e3e183bf063 | 545 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 546 | lcd.printf("Vak %d",doel); |
lauradeheus | 7:7e3e183bf063 | 547 | if(current_herhalingen<=400){ |
lauradeheus | 7:7e3e183bf063 | 548 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 549 | lcd.printf("nog 5 sec."); |
lauradeheus | 7:7e3e183bf063 | 550 | } |
lauradeheus | 7:7e3e183bf063 | 551 | else if(current_herhalingen<=600){ |
lauradeheus | 7:7e3e183bf063 | 552 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 553 | lcd.printf("nog 4 sec."); |
lauradeheus | 7:7e3e183bf063 | 554 | } |
lauradeheus | 7:7e3e183bf063 | 555 | else if(current_herhalingen<=800){ |
lauradeheus | 7:7e3e183bf063 | 556 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 557 | lcd.printf("nog 3 sec."); |
lauradeheus | 7:7e3e183bf063 | 558 | } |
lauradeheus | 7:7e3e183bf063 | 559 | else if(current_herhalingen<=1000){ |
lauradeheus | 7:7e3e183bf063 | 560 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 561 | lcd.printf("nog 2 sec."); |
lauradeheus | 7:7e3e183bf063 | 562 | } |
lauradeheus | 7:7e3e183bf063 | 563 | else if(current_herhalingen<=1200){ |
lauradeheus | 7:7e3e183bf063 | 564 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 565 | lcd.printf("nog 1 sec."); |
lauradeheus | 7:7e3e183bf063 | 566 | } |
lauradeheus | 7:7e3e183bf063 | 567 | } |
lauradeheus | 7:7e3e183bf063 | 568 | else if(current_herhalingen<=1600){ |
lauradeheus | 7:7e3e183bf063 | 569 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 570 | lcd.printf("Vak %d akkoord?",doel); |
lauradeheus | 7:7e3e183bf063 | 571 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 572 | lcd.printf("Span aan"); |
lauradeheus | 7:7e3e183bf063 | 573 | } |
lauradeheus | 7:7e3e183bf063 | 574 | else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){ |
lauradeheus | 7:7e3e183bf063 | 575 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 576 | lcd.printf("Vak %d akkoord",doel_richting); |
lauradeheus | 7:7e3e183bf063 | 577 | } |
lauradeheus | 7:7e3e183bf063 | 578 | else{ |
lauradeheus | 7:7e3e183bf063 | 579 | lcd.locate(0,0); |
lauradeheus | 7:7e3e183bf063 | 580 | lcd.printf("Opnieuw richting"); |
lauradeheus | 7:7e3e183bf063 | 581 | lcd.locate(0,1); |
lauradeheus | 7:7e3e183bf063 | 582 | lcd.printf("bepalen"); |
lauradeheus | 7:7e3e183bf063 | 583 | } |
lauradeheus | 7:7e3e183bf063 | 584 | } |
phgbartels | 0:2386012c6594 | 585 | else{ |
phgbartels | 0:2386012c6594 | 586 | lcd.cls(); |
phgbartels | 0:2386012c6594 | 587 | 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 | 588 | } |
phgbartels | 0:2386012c6594 | 589 | } |
phgbartels | 0:2386012c6594 | 590 | |
lauradeheus | 6:a7379a681adf | 591 | int main(){ |
phgbartels | 4:377ddd65e4a6 | 592 | pc.baud(115200); |
lauradeheus | 6:a7379a681adf | 593 | arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states); |
lauradeheus | 6:a7379a681adf | 594 | arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states); |
lauradeheus | 6:a7379a681adf | 595 | arm_biquad_cascade_df1_init_f32(¬ch,1 , notch_const, notch_states); |
lauradeheus | 6:a7379a681adf | 596 | arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states); |
phgbartels | 4:377ddd65e4a6 | 597 | statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds) |
phgbartels | 0:2386012c6594 | 598 | screen.attach(&screenupdate, 0.2); |
phgbartels | 4:377ddd65e4a6 | 599 | while(1); |
phgbartels | 0:2386012c6594 | 600 | } |