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