Peter Bartels / Mbed 2 deprecated Main-script_groep7_V4

Laatste versie van ons script

Dependencies:   Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed

Fork of Main-script_groep7_V3 by Peter Bartels

main.cpp@11:687aa4656a6e, 2014-11-03 (annotated)

Committer:
phgbartels
Date:
Mon Nov 03 14:46:31 2014 +0000
Revision:
11:687aa4656a6e
Parent:
10:cd89569cd847
Child:
12:3fec73bc3318
Werkend script incl instellen richting;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
phgbartels 0:2386012c6594 1 /********************************************/
phgbartels 0:2386012c6594 2 /* */
phgbartels 0:2386012c6594 3 /* BRONCODE GROEP 7, MODULE 9, 2014 */
phgbartels 0:2386012c6594 4 /* *-THE SLAP-* */
phgbartels 0:2386012c6594 5 /* */
phgbartels 0:2386012c6594 6 /* -Anne ten Dam */
phgbartels 0:2386012c6594 7 /* -Laura de Heus */
phgbartels 0:2386012c6594 8 /* -Moniek Strijdveen */
phgbartels 0:2386012c6594 9 /* -Bart Arendshorst */
phgbartels 0:2386012c6594 10 /* -Peter Bartels */
phgbartels 0:2386012c6594 11 /********************************************/
phgbartels 0:2386012c6594 12
phgbartels 0:2386012c6594 13 #include "TextLCD.h"
phgbartels 0:2386012c6594 14 #include "mbed.h"
phgbartels 0:2386012c6594 15 #include "encoder.h"
lauradeheus 6:a7379a681adf 16 #include "HIDScope.h"
phgbartels 0:2386012c6594 17 #include "PwmOut.h"
lauradeheus 6:a7379a681adf 18 #include "arm_math.h"
phgbartels 11:687aa4656a6e 19 #include "MODSERIAL.h"
phgbartels 0:2386012c6594 20
lauradeheus 8:f733c6a27c15 21 /*definieren pinnen Motoren*/
phgbartels 0:2386012c6594 22 #define M1_PWM PTA5
phgbartels 0:2386012c6594 23 #define M1_DIR PTA4
phgbartels 0:2386012c6594 24 #define M2_PWM PTC8
phgbartels 0:2386012c6594 25 #define M2_DIR PTC9
phgbartels 0:2386012c6594 26 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
phgbartels 0:2386012c6594 27 #define TSAMP 0.005
lauradeheus 8:f733c6a27c15 28 #define K_P (80000)
lauradeheus 8:f733c6a27c15 29 #define K_I (0.01)
lauradeheus 8:f733c6a27c15 30 #define K_D (0.01)
phgbartels 0:2386012c6594 31 #define I_LIMIT 100.
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 11:687aa4656a6e 39 Encoder motor2(PTD5, PTD0);
phgbartels 0:2386012c6594 40 PwmOut pwm_motor1(M1_PWM);
phgbartels 11:687aa4656a6e 41 PwmOut pwm_motor2(M2_PWM);
phgbartels 0:2386012c6594 42 DigitalOut motordir1(M1_DIR);
phgbartels 11:687aa4656a6e 43 DigitalOut motordir2(M2_DIR);
phgbartels 0:2386012c6594 44 DigitalOut LEDGREEN(LED_GREEN);
phgbartels 11:687aa4656a6e 45 DigitalOut LEDRED(LED_RED);
phgbartels 11:687aa4656a6e 46 MODSERIAL 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;
phgbartels 11:687aa4656a6e 60 int current_herhalingen_1 = 0;
phgbartels 11:687aa4656a6e 61 int previous_herhalingen_1 = 0;
phgbartels 0:2386012c6594 62 int previous_pos_motor1 = 0;
phgbartels 0:2386012c6594 63 int current_pos_motor1;
phgbartels 11:687aa4656a6e 64 int current_pos_motor2;
phgbartels 5:4842219cb77c 65 int EMG = 1;
phgbartels 10:cd89569cd847 66 int aantal_pieken = 0;
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 11:687aa4656a6e 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;
phgbartels 11:687aa4656a6e 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 11:687aa4656a6e 102 void rust()
phgbartels 11:687aa4656a6e 103 {
phgbartels 11:687aa4656a6e 104 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 105 previous_herhalingen = current_herhalingen;
lauradeheus 8:f733c6a27c15 106 }//void rust
phgbartels 11:687aa4656a6e 107
phgbartels 11:687aa4656a6e 108 void pieken_tellen()
phgbartels 11:687aa4656a6e 109 {
phgbartels 11:687aa4656a6e 110 if (emg_filtered>=emg_treshhold_hoog) {
lauradeheus 6:a7379a681adf 111 aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
lauradeheus 8:f733c6a27c15 112 }//if
phgbartels 11:687aa4656a6e 113 if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt
lauradeheus 6:a7379a681adf 114 aanspan=false;
lauradeheus 6:a7379a681adf 115 aantal_pieken++;
phgbartels 10:cd89569cd847 116 doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1;
phgbartels 11:687aa4656a6e 117
lauradeheus 8:f733c6a27c15 118 }//if
lauradeheus 8:f733c6a27c15 119 }//void pieken_tellen
lauradeheus 6:a7379a681adf 120
phgbartels 11:687aa4656a6e 121 void emg_filtering()
phgbartels 11:687aa4656a6e 122 {
lauradeheus 6:a7379a681adf 123 uint16_t emg_value;
lauradeheus 6:a7379a681adf 124 float emg_value_f32;
lauradeheus 6:a7379a681adf 125 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 126 emg_value_f32 = emg.read();
phgbartels 11:687aa4656a6e 127
lauradeheus 6:a7379a681adf 128 arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
lauradeheus 6:a7379a681adf 129 arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
lauradeheus 6:a7379a681adf 130 arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
lauradeheus 6:a7379a681adf 131 emg_filtered = fabs(emg_filtered);
lauradeheus 6:a7379a681adf 132 arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
phgbartels 10:cd89569cd847 133 //scope.set(0,emg_value); //uint value
phgbartels 10:cd89569cd847 134 //scope.set(1,emg_filtered); //processed float
phgbartels 11:687aa4656a6e 135 if(state!=EMG_KALIBRATIE) {
lauradeheus 6:a7379a681adf 136 pieken_tellen();
lauradeheus 8:f733c6a27c15 137 }//if
phgbartels 10:cd89569cd847 138 //pc.printf("%d\n\r",doel);
phgbartels 11:687aa4656a6e 139 scope.set(0, doel);
phgbartels 10:cd89569cd847 140 scope.set(1, aantal_pieken);
lauradeheus 9:7e9b63fe8988 141 scope.send();
lauradeheus 8:f733c6a27c15 142 }//void emg_filtering()
lauradeheus 6:a7379a681adf 143
phgbartels 11:687aa4656a6e 144 void emg_max_meting()
phgbartels 11:687aa4656a6e 145 {
lauradeheus 6:a7379a681adf 146 emg_filtering();
phgbartels 11:687aa4656a6e 147 if (emg_filtered>=emg_max) {
lauradeheus 6:a7379a681adf 148 emg_max=emg_filtered;
lauradeheus 8:f733c6a27c15 149 }//if
phgbartels 11:687aa4656a6e 150 emg_treshhold_laag = 0.4*emg_max;
phgbartels 11:687aa4656a6e 151 emg_treshhold_hoog = 0.7*emg_max;
lauradeheus 8:f733c6a27c15 152 }//void emg_max_meting
lauradeheus 6:a7379a681adf 153
phgbartels 11:687aa4656a6e 154 void akkoord_geven()
phgbartels 11:687aa4656a6e 155 {
lauradeheus 9:7e9b63fe8988 156 emg_filtering();
lauradeheus 9:7e9b63fe8988 157 }
lauradeheus 9:7e9b63fe8988 158
phgbartels 11:687aa4656a6e 159 void emg_kalibratie()
phgbartels 11:687aa4656a6e 160 {
lauradeheus 7:7e3e183bf063 161 //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 162 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 163 previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 164 //}
phgbartels 11:687aa4656a6e 165 if(current_herhalingen<=1000) {
lauradeheus 6:a7379a681adf 166 emg_max_meting();
lauradeheus 8:f733c6a27c15 167 }//if
lauradeheus 8:f733c6a27c15 168 }//void emg_kalibratie
phgbartels 0:2386012c6594 169
phgbartels 11:687aa4656a6e 170 void arm_kalibratie()
phgbartels 11:687aa4656a6e 171 {
lauradeheus 7:7e3e183bf063 172 //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 173 motor1.setPosition(0);
phgbartels 11:687aa4656a6e 174 motor2.setPosition(0);
lauradeheus 9:7e9b63fe8988 175 pwm_motor1.period_us(100);
lauradeheus 9:7e9b63fe8988 176 pwm_motor2.period_us(100);
phgbartels 11:687aa4656a6e 177 akkoord_geven();
lauradeheus 8:f733c6a27c15 178 }//void arm_kalibratie
lauradeheus 7:7e3e183bf063 179
phgbartels 11:687aa4656a6e 180 void doel_bepaling()
phgbartels 11:687aa4656a6e 181 {
phgbartels 11:687aa4656a6e 182 if(200<=current_herhalingen && current_herhalingen <1200) {
lauradeheus 7:7e3e183bf063 183 emg_filtering();
phgbartels 10:cd89569cd847 184 doel = aantal_pieken-(((aantal_pieken-1)/3)*3);
lauradeheus 8:f733c6a27c15 185 }//if
phgbartels 11:687aa4656a6e 186 else if(current_herhalingen == 1200 && state==METEN_HOOGTE) {
lauradeheus 9:7e9b63fe8988 187 doel_hoogte = doel;
lauradeheus 9:7e9b63fe8988 188 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 189 doel = 0;
phgbartels 11:687aa4656a6e 190 } else if(current_herhalingen == 1200 && state==METEN_RICHTING) {
lauradeheus 9:7e9b63fe8988 191 doel_richting = doel;
lauradeheus 9:7e9b63fe8988 192 aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
phgbartels 10:cd89569cd847 193 doel = 0;
phgbartels 11:687aa4656a6e 194 } else if(1200<current_herhalingen && current_herhalingen<=2200) {
phgbartels 11:687aa4656a6e 195 akkoord_geven();
lauradeheus 8:f733c6a27c15 196 }//else if
phgbartels 11:687aa4656a6e 197 else {
phgbartels 11:687aa4656a6e 198 }//else
lauradeheus 8:f733c6a27c15 199 }//void doel_bepaling
phgbartels 0:2386012c6594 200
phgbartels 11:687aa4656a6e 201 void meten_hoogte()
phgbartels 11:687aa4656a6e 202 {
phgbartels 11:687aa4656a6e 203 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 204 previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 205 doel_bepaling();
lauradeheus 8:f733c6a27c15 206 }//void meten_hoogte
lauradeheus 7:7e3e183bf063 207
phgbartels 11:687aa4656a6e 208 void meten_richting()
phgbartels 11:687aa4656a6e 209 {
phgbartels 11:687aa4656a6e 210 current_herhalingen = previous_herhalingen + 1;
phgbartels 11:687aa4656a6e 211 previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 212 doel_bepaling();
lauradeheus 8:f733c6a27c15 213 }//void meten_richting
phgbartels 0:2386012c6594 214
phgbartels 11:687aa4656a6e 215 void instellen_richting()
phgbartels 11:687aa4656a6e 216 {
lauradeheus 9:7e9b63fe8988 217 current_pos_motor2 = motor2.getPosition();
phgbartels 11:687aa4656a6e 218 pc.printf("%d\n\r", current_pos_motor2);
phgbartels 11:687aa4656a6e 219 /*
phgbartels 11:687aa4656a6e 220 if (doel_richting ==1){
lauradeheus 9:7e9b63fe8988 221 if (state==RETURN2RUST){
lauradeheus 9:7e9b63fe8988 222 motordir2 = 1;
lauradeheus 9:7e9b63fe8988 223 while (current_pos_motor2 > 0){
lauradeheus 9:7e9b63fe8988 224 pwm_motor2.write(PWM_richting1);
lauradeheus 9:7e9b63fe8988 225 }//while (current_pos_motor2 < rad_richting1)
phgbartels 10:cd89569cd847 226 current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
lauradeheus 9:7e9b63fe8988 227 }//if (doel_richting == 1)
lauradeheus 9:7e9b63fe8988 228 else{
lauradeheus 9:7e9b63fe8988 229 motordir2 = 0;
lauradeheus 9:7e9b63fe8988 230 while (current_pos_motor2 < puls_richting1){
lauradeheus 9:7e9b63fe8988 231 pwm_motor2.write(PWM_richting1);
lauradeheus 9:7e9b63fe8988 232 }//while (current_pos_motor2 < rad_richting1)
phgbartels 10:cd89569cd847 233 current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
phgbartels 10:cd89569cd847 234 }//else
phgbartels 11:687aa4656a6e 235 }//if (doel_richting ==1)
lauradeheus 9:7e9b63fe8988 236
lauradeheus 9:7e9b63fe8988 237
phgbartels 11:687aa4656a6e 238 else if (doel_richting == 2){
lauradeheus 9:7e9b63fe8988 239 if (state==RETURN2RUST){
lauradeheus 9:7e9b63fe8988 240 motordir2 = 1;
lauradeheus 9:7e9b63fe8988 241 while (current_pos_motor2 > 0){
lauradeheus 9:7e9b63fe8988 242 pwm_motor2.write(PWM_richting2);
lauradeheus 9:7e9b63fe8988 243 }//while (current_pos_motor2 < rad_richting1)
phgbartels 10:cd89569cd847 244 current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
lauradeheus 9:7e9b63fe8988 245 }//if (doel_richting == 1)
lauradeheus 9:7e9b63fe8988 246 else{
lauradeheus 9:7e9b63fe8988 247 motordir2 = 0;
lauradeheus 9:7e9b63fe8988 248 while (current_pos_motor2 < puls_richting2){
lauradeheus 9:7e9b63fe8988 249 pwm_motor2.write(PWM_richting2);
lauradeheus 9:7e9b63fe8988 250 }//while (current_pos_motor2 < rad_richting1)
phgbartels 10:cd89569cd847 251 current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
lauradeheus 9:7e9b63fe8988 252 }//if (doel_richting == 1)
phgbartels 11:687aa4656a6e 253 }//if (doel_richting ==1)
lauradeheus 9:7e9b63fe8988 254
phgbartels 11:687aa4656a6e 255 else if(doel_richting == 3){
lauradeheus 9:7e9b63fe8988 256 if (state==RETURN2RUST){
lauradeheus 9:7e9b63fe8988 257 motordir2 =1;
lauradeheus 9:7e9b63fe8988 258 while (current_pos_motor2 > 0){
lauradeheus 9:7e9b63fe8988 259 pwm_motor2.write(PWM_richting3);
lauradeheus 9:7e9b63fe8988 260 }//while (current_pos_motor2 < rad_richting1)
phgbartels 10:cd89569cd847 261 current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
lauradeheus 9:7e9b63fe8988 262 }//if (doel_richting == 1)
lauradeheus 9:7e9b63fe8988 263 else{
lauradeheus 9:7e9b63fe8988 264 motordir2 = 0;
lauradeheus 9:7e9b63fe8988 265 while (current_pos_motor2 < puls_richting3){
lauradeheus 9:7e9b63fe8988 266 pwm_motor2.write(PWM_richting3);
lauradeheus 9:7e9b63fe8988 267 }//while (current_pos_motor2 < rad_richting1)
phgbartels 10:cd89569cd847 268 current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
lauradeheus 9:7e9b63fe8988 269 }//if (doel_richting == 1)
phgbartels 11:687aa4656a6e 270 }//if (doel_richting ==1)
phgbartels 11:687aa4656a6e 271 */
lauradeheus 8:f733c6a27c15 272 }//void instellen_richting
phgbartels 0:2386012c6594 273
phgbartels 11:687aa4656a6e 274 void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge)
phgbartels 11:687aa4656a6e 275 {
phgbartels 11:687aa4656a6e 276
phgbartels 0:2386012c6594 277 current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
phgbartels 0:2386012c6594 278 pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
phgbartels 0:2386012c6594 279 previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde.
phgbartels 11:687aa4656a6e 280
lauradeheus 9:7e9b63fe8988 281 //nu gaan we snelheid volgen d.m.v. positie regeling
phgbartels 11:687aa4656a6e 282 if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) { //if position error < ...rad, then stop.
phgbartels 11:687aa4656a6e 283 speed_radpersecond = 0;
phgbartels 4:377ddd65e4a6 284 setpoint = pos_motor1_rad;
phgbartels 11:687aa4656a6e 285 current_herhalingen = previous_herhalingen + 1;
phgbartels 2:de7b6c1d67c4 286 previous_herhalingen = current_herhalingen;
phgbartels 11:687aa4656a6e 287 //pc.printf("stop\n\r");
phgbartels 5:4842219cb77c 288 PWM1_percentage = 0;
lauradeheus 8:f733c6a27c15 289 }//if
phgbartels 11:687aa4656a6e 290 else if(pos_motor1_rad - position_setpoint_rad < 0) {
phgbartels 5:4842219cb77c 291 setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
phgbartels 11:687aa4656a6e 292 PWM1_percentage = pid(setpoint, pos_motor1_rad);
lauradeheus 8:f733c6a27c15 293 }//else if
phgbartels 11:687aa4656a6e 294 else {
phgbartels 5:4842219cb77c 295 setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
phgbartels 11:687aa4656a6e 296 PWM1_percentage = pid(setpoint, pos_motor1_rad);
lauradeheus 8:f733c6a27c15 297 }//else
phgbartels 11:687aa4656a6e 298 //pc.printf("%f\n\r",PWM1_percentage);
phgbartels 11:687aa4656a6e 299
phgbartels 11:687aa4656a6e 300 if (PWM1_percentage < -100) {
phgbartels 0:2386012c6594 301 PWM1_percentage = -100;
lauradeheus 8:f733c6a27c15 302 }//if
phgbartels 11:687aa4656a6e 303 else if (PWM1_percentage >100) {
phgbartels 0:2386012c6594 304 PWM1_percentage =100;
lauradeheus 8:f733c6a27c15 305 }//else if
phgbartels 11:687aa4656a6e 306
phgbartels 11:687aa4656a6e 307 if(PWM1_percentage < 0) {
phgbartels 0:2386012c6594 308 motordir1 = 1;
lauradeheus 8:f733c6a27c15 309 }//if
phgbartels 11:687aa4656a6e 310 else {
phgbartels 0:2386012c6594 311 motordir1 = 0;
lauradeheus 8:f733c6a27c15 312 }//else
phgbartels 11:687aa4656a6e 313
lauradeheus 9:7e9b63fe8988 314 pwm_motor1.write(abs(PWM1_percentage/100.));
phgbartels 0:2386012c6594 315 prev_setpoint = setpoint;
lauradeheus 8:f733c6a27c15 316 }//void GotoPosition
phgbartels 2:de7b6c1d67c4 317
phgbartels 0:2386012c6594 318 float pid(float setpoint, float measurement)
phgbartels 0:2386012c6594 319 {
phgbartels 0:2386012c6594 320 float error;
phgbartels 0:2386012c6594 321 static float prev_error = 0;
phgbartels 0:2386012c6594 322 float out_p = 0;
phgbartels 0:2386012c6594 323 static float out_i = 0;
phgbartels 0:2386012c6594 324 float out_d = 0;
phgbartels 0:2386012c6594 325 error = (setpoint-measurement);
phgbartels 11:687aa4656a6e 326 out_p = error*K_P;
phgbartels 0:2386012c6594 327 out_i += error*K_I;
phgbartels 0:2386012c6594 328 out_d = (error-prev_error)*K_D;
phgbartels 0:2386012c6594 329 clamp(&out_i,-I_LIMIT,I_LIMIT);
phgbartels 0:2386012c6594 330 prev_error = error;
phgbartels 0:2386012c6594 331 return out_p + out_i + out_d;
lauradeheus 8:f733c6a27c15 332 }//float pid
phgbartels 0:2386012c6594 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 11:687aa4656a6e 341 pc.printf(".");
phgbartels 0:2386012c6594 342 switch(state) {
phgbartels 0:2386012c6594 343 case RUST: {
phgbartels 0:2386012c6594 344 rust();
phgbartels 0:2386012c6594 345 /*voorwaarde wanneer hij door kan naar de volgende case*/
phgbartels 11:687aa4656a6e 346 if (current_herhalingen == 100 && EMG == 1) {
phgbartels 0:2386012c6594 347 current_herhalingen = 0;
phgbartels 0:2386012c6594 348 previous_herhalingen = 0;
lauradeheus 9:7e9b63fe8988 349 state = EMG_KALIBRATIE;
lauradeheus 9:7e9b63fe8988 350 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 351 }//if (current_herhalingen == 100 && EMG == 1)
phgbartels 11:687aa4656a6e 352 else if(current_herhalingen == 100) {
phgbartels 11:687aa4656a6e 353 current_herhalingen = 0;
phgbartels 11:687aa4656a6e 354 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 355 state = METEN_HOOGTE;
lauradeheus 9:7e9b63fe8988 356 }//else
phgbartels 1:b08ac32d1ddc 357 break;
phgbartels 11:687aa4656a6e 358 }//case RUST:
phgbartels 0:2386012c6594 359
phgbartels 11:687aa4656a6e 360 case EMG_KALIBRATIE: {
phgbartels 0:2386012c6594 361 emg_kalibratie();
phgbartels 11:687aa4656a6e 362 if (current_herhalingen >=1000) { /*waarom >= en niet ==?*/
lauradeheus 7:7e3e183bf063 363 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 364 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 365 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 366 doel = 0;
lauradeheus 9:7e9b63fe8988 367 state = ARM_KALIBRATIE;
phgbartels 11:687aa4656a6e 368 }//if (current_herhalingen >=1000)
lauradeheus 7:7e3e183bf063 369 break;
lauradeheus 8:f733c6a27c15 370 }//case EMG_KALIBRATIE
phgbartels 11:687aa4656a6e 371
phgbartels 11:687aa4656a6e 372 case ARM_KALIBRATIE: {
lauradeheus 9:7e9b63fe8988 373 arm_kalibratie();
phgbartels 11:687aa4656a6e 374 if (aantal_pieken == 1) {
phgbartels 0:2386012c6594 375 current_herhalingen = 0;
phgbartels 0:2386012c6594 376 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 377 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 378 doel = 0;
phgbartels 11:687aa4656a6e 379 state = METEN_HOOGTE;
phgbartels 11:687aa4656a6e 380 }//if (current_herhalingen == 100)
phgbartels 11:687aa4656a6e 381 break;
phgbartels 11:687aa4656a6e 382 }//case ARM_KALIBRATIE:
phgbartels 11:687aa4656a6e 383
phgbartels 11:687aa4656a6e 384 case METEN_HOOGTE: {
phgbartels 11:687aa4656a6e 385 meten_hoogte();
phgbartels 11:687aa4656a6e 386 if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) {
lauradeheus 7:7e3e183bf063 387 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 388 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 389 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 390 doel = 0;
phgbartels 11:687aa4656a6e 391 //doel_hoogte = 0;
phgbartels 11:687aa4656a6e 392 state = METEN_RICHTING;
phgbartels 11:687aa4656a6e 393 }//if (current_herhalingen == 2800 && aantal_pieken == 1)
phgbartels 11:687aa4656a6e 394 else if (current_herhalingen == 2200) {
phgbartels 11:687aa4656a6e 395 current_herhalingen = 0;
phgbartels 11:687aa4656a6e 396 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 397 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 398 doel = 0;
lauradeheus 9:7e9b63fe8988 399 state = METEN_HOOGTE;
lauradeheus 8:f733c6a27c15 400 }///else
phgbartels 1:b08ac32d1ddc 401 break;
lauradeheus 8:f733c6a27c15 402 }//case METEN_HOOGTE
phgbartels 0:2386012c6594 403
phgbartels 11:687aa4656a6e 404 case METEN_RICHTING: {
phgbartels 0:2386012c6594 405 meten_richting();
phgbartels 11:687aa4656a6e 406 if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) {
phgbartels 0:2386012c6594 407 current_herhalingen = 0;
phgbartels 0:2386012c6594 408 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 409 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 410 doel = 0;
lauradeheus 9:7e9b63fe8988 411 state = INSTELLEN_RICHTING;
phgbartels 11:687aa4656a6e 412 }//if (current_herhalingen == 2800 && aantal_pieken == 1)
phgbartels 11:687aa4656a6e 413 else if (current_herhalingen == 2200) {
lauradeheus 7:7e3e183bf063 414 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 415 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 416 aantal_pieken = 0;
phgbartels 11:687aa4656a6e 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 11:687aa4656a6e 423 case INSTELLEN_RICHTING: {
phgbartels 0:2386012c6594 424 instellen_richting();
phgbartels 11:687aa4656a6e 425 /*if (current_herhalingen == 100)
phgbartels 0:2386012c6594 426 {
lauradeheus 9:7e9b63fe8988 427 current_herhalingen_1 = 0;
lauradeheus 9:7e9b63fe8988 428 previous_herhalingen_1 = 0;
lauradeheus 9:7e9b63fe8988 429 doel_richting = 0;
phgbartels 0:2386012c6594 430 state = SLAAN;
lauradeheus 8:f733c6a27c15 431 }//if (current_herhalingen == 100)
phgbartels 11:687aa4656a6e 432 */
phgbartels 11:687aa4656a6e 433 break;
lauradeheus 8:f733c6a27c15 434 }//case INSTELLEN_RICHTING
phgbartels 0:2386012c6594 435
phgbartels 11:687aa4656a6e 436 case SLAAN: {
lauradeheus 8:f733c6a27c15 437 GotoPosition(1.5 ,8, 0.1);
phgbartels 11:687aa4656a6e 438 if (current_herhalingen == 400) {
phgbartels 0:2386012c6594 439 current_herhalingen = 0;
phgbartels 0:2386012c6594 440 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 441 prev_setpoint =0;
phgbartels 2:de7b6c1d67c4 442 setpoint =0;
lauradeheus 9:7e9b63fe8988 443 state = RETURN2RUST;
phgbartels 11:687aa4656a6e 444 }//if (current_herhalingen == 100)
phgbartels 11:687aa4656a6e 445 break;
lauradeheus 8:f733c6a27c15 446 }//case SLAAN
phgbartels 0:2386012c6594 447
phgbartels 11:687aa4656a6e 448 case RETURN2RUST: {
lauradeheus 9:7e9b63fe8988 449 instellen_richting();
lauradeheus 8:f733c6a27c15 450 GotoPosition(0,4, 0.05);
lauradeheus 9:7e9b63fe8988 451 doel_richting = 0;
phgbartels 11:687aa4656a6e 452 doel_hoogte = 0;
phgbartels 11:687aa4656a6e 453 if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) {
lauradeheus 9:7e9b63fe8988 454 state = RUST;
lauradeheus 9:7e9b63fe8988 455 current_herhalingen = 0;
lauradeheus 9:7e9b63fe8988 456 previous_herhalingen = 0;
phgbartels 11:687aa4656a6e 457 current_herhalingen = 0;
lauradeheus 9:7e9b63fe8988 458 current_herhalingen = 0;
phgbartels 11:687aa4656a6e 459 }//if (current_herhalingen == 100)
phgbartels 1:b08ac32d1ddc 460 break;
lauradeheus 8:f733c6a27c15 461 }// case RETURN2RUST
phgbartels 11:687aa4656a6e 462
phgbartels 0:2386012c6594 463 default: {
phgbartels 0:2386012c6594 464 state = RUST;
lauradeheus 8:f733c6a27c15 465 }//default
phgbartels 0:2386012c6594 466
phgbartels 0:2386012c6594 467 }//switch(state)
phgbartels 0:2386012c6594 468 }//void statemachinefunction
phgbartels 0:2386012c6594 469
phgbartels 0:2386012c6594 470
phgbartels 11:687aa4656a6e 471 void screenupdate()
phgbartels 11:687aa4656a6e 472 {
phgbartels 11:687aa4656a6e 473 pc.printf("l");
phgbartels 11:687aa4656a6e 474 if(state==RUST) {
phgbartels 11:687aa4656a6e 475 lcd.cls();
phgbartels 0:2386012c6594 476 lcd.locate(0,0);
phgbartels 5:4842219cb77c 477 lcd.printf("V.I.C.T.O.R.Y."); //regel 1 LCD scherm
phgbartels 0:2386012c6594 478 lcd.locate(0,1);
phgbartels 0:2386012c6594 479 lcd.printf(" GROEP 7 ");
lauradeheus 8:f733c6a27c15 480 }//if(state==RUST)
phgbartels 11:687aa4656a6e 481
phgbartels 11:687aa4656a6e 482 else if(state==EMG_KALIBRATIE) {
lauradeheus 6:a7379a681adf 483 lcd.cls();
lauradeheus 6:a7379a681adf 484 lcd.locate(0,0);
lauradeheus 6:a7379a681adf 485 lcd.printf("Max. aanspannen");
phgbartels 11:687aa4656a6e 486 if(current_herhalingen<=200) {
phgbartels 11:687aa4656a6e 487 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 488 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 489 }//if(current_herhalingen<=200)
phgbartels 11:687aa4656a6e 490 else if(current_herhalingen<=400) {
phgbartels 11:687aa4656a6e 491 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 492 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 493 }//else if(current_herhalingen<=400)
phgbartels 11:687aa4656a6e 494 else if(current_herhalingen<=600) {
phgbartels 11:687aa4656a6e 495 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 496 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 497 }//else if(current_herhalingen<=600)
phgbartels 11:687aa4656a6e 498 else if(current_herhalingen<=800) {
phgbartels 11:687aa4656a6e 499 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 500 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 501 }//else if(current_herhalingen<=800)
phgbartels 11:687aa4656a6e 502 else if(current_herhalingen<=1000) {
phgbartels 11:687aa4656a6e 503 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 504 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 505 }//else if(current_herhalingen<=1000)
lauradeheus 8:f733c6a27c15 506 }//else if(state==EMG_KALIBRATIE)
phgbartels 11:687aa4656a6e 507
phgbartels 11:687aa4656a6e 508 else if(state==ARM_KALIBRATIE) {
lauradeheus 9:7e9b63fe8988 509 lcd.cls();
lauradeheus 9:7e9b63fe8988 510 lcd.locate(0,0);
lauradeheus 9:7e9b63fe8988 511 lcd.printf("Set arm to zero");
phgbartels 11:687aa4656a6e 512 lcd.locate(0,1);
lauradeheus 9:7e9b63fe8988 513 lcd.printf("Klaar? Span aan");
lauradeheus 9:7e9b63fe8988 514 }//else if(state==ARM_KALIBRATIE)
phgbartels 11:687aa4656a6e 515
phgbartels 11:687aa4656a6e 516 else if(state==METEN_HOOGTE) {
lauradeheus 7:7e3e183bf063 517 lcd.cls();
phgbartels 11:687aa4656a6e 518 if(current_herhalingen<=200) {
lauradeheus 7:7e3e183bf063 519 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 520 lcd.printf("Hoogte bepalen:");
lauradeheus 7:7e3e183bf063 521 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 522 lcd.printf("span aan per vak");
lauradeheus 8:f733c6a27c15 523 }//if(current_herhalingen<=200){
phgbartels 11:687aa4656a6e 524 else if(200<=current_herhalingen && current_herhalingen<1200) {
lauradeheus 7:7e3e183bf063 525 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 526 lcd.printf("Vak %d",doel);
phgbartels 11:687aa4656a6e 527 if(current_herhalingen<=400) {
phgbartels 11:687aa4656a6e 528 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 529 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 530 }//if(current_herhalingen<=400)
phgbartels 11:687aa4656a6e 531 else if(current_herhalingen<=600) {
phgbartels 11:687aa4656a6e 532 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 533 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 534 }//else if(current_herhalingen<=600)
phgbartels 11:687aa4656a6e 535 else if(current_herhalingen<=800) {
phgbartels 11:687aa4656a6e 536 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 537 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 538 }//else if(current_herhalingen<=800)
phgbartels 11:687aa4656a6e 539 else if(current_herhalingen<=1000) {
phgbartels 11:687aa4656a6e 540 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 541 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 542 }//else if(current_herhalingen<=1000)
phgbartels 11:687aa4656a6e 543 else if(current_herhalingen<1200) {
phgbartels 11:687aa4656a6e 544 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 545 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 546 }//else if(current_herhalingen<=1200)
lauradeheus 8:f733c6a27c15 547 }//else if(200<=current_herhalingen<=1200)
phgbartels 11:687aa4656a6e 548 else if(current_herhalingen<=2200) {
lauradeheus 7:7e3e183bf063 549 lcd.locate(0,0);
lauradeheus 9:7e9b63fe8988 550 lcd.printf("Vak %d akkoord?",doel_hoogte);
lauradeheus 7:7e3e183bf063 551 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 552 lcd.printf("Span aan");
lauradeheus 8:f733c6a27c15 553 }//else if(current_herhalingen<=1600){
phgbartels 11:687aa4656a6e 554 else {
lauradeheus 7:7e3e183bf063 555 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 556 lcd.printf("Opnieuw hoogte");
lauradeheus 7:7e3e183bf063 557 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 558 lcd.printf("bepalen");
lauradeheus 8:f733c6a27c15 559 }//else{
lauradeheus 8:f733c6a27c15 560 }//else if(state==METEN_HOOGTE){
phgbartels 11:687aa4656a6e 561
phgbartels 11:687aa4656a6e 562 else if(state==METEN_RICHTING) {
lauradeheus 7:7e3e183bf063 563 lcd.cls();
phgbartels 11:687aa4656a6e 564 if(current_herhalingen<=200) {
lauradeheus 7:7e3e183bf063 565 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 566 lcd.printf("Richting bepalen:");
lauradeheus 7:7e3e183bf063 567 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 568 lcd.printf("span aan per vak");
lauradeheus 8:f733c6a27c15 569 }//if(current_herhalingen<=200)
phgbartels 11:687aa4656a6e 570 else if(200<=current_herhalingen && current_herhalingen<1200) {
lauradeheus 7:7e3e183bf063 571 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 572 lcd.printf("Vak %d",doel);
phgbartels 11:687aa4656a6e 573 if(current_herhalingen<=400) {
phgbartels 11:687aa4656a6e 574 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 575 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 576 }//if(current_herhalingen<=400)
phgbartels 11:687aa4656a6e 577 else if(current_herhalingen<=600) {
phgbartels 11:687aa4656a6e 578 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 579 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 580 }//else if(current_herhalingen<=600)
phgbartels 11:687aa4656a6e 581 else if(current_herhalingen<=800) {
phgbartels 11:687aa4656a6e 582 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 583 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 584 }//else if(current_herhalingen<=800)
phgbartels 11:687aa4656a6e 585 else if(current_herhalingen<=1000) {
phgbartels 11:687aa4656a6e 586 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 587 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 588 }//else if(current_herhalingen<=1000)
phgbartels 11:687aa4656a6e 589 else if(current_herhalingen<1200) {
phgbartels 11:687aa4656a6e 590 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 591 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 592 }//else if(current_herhalingen<=1200)
lauradeheus 8:f733c6a27c15 593 }//else if(200<=current_herhalingen<=1200)
phgbartels 11:687aa4656a6e 594 else if(current_herhalingen<=2200) {
lauradeheus 7:7e3e183bf063 595 lcd.locate(0,0);
lauradeheus 9:7e9b63fe8988 596 lcd.printf("Vak %d akkoord?",doel_richting);
lauradeheus 7:7e3e183bf063 597 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 598 lcd.printf("Span aan");
lauradeheus 8:f733c6a27c15 599 }//else if(current_herhalingen<=1600)
phgbartels 11:687aa4656a6e 600 else {
lauradeheus 7:7e3e183bf063 601 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 602 lcd.printf("Opnieuw richting");
lauradeheus 7:7e3e183bf063 603 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 604 lcd.printf("bepalen");
lauradeheus 8:f733c6a27c15 605 }//else
lauradeheus 8:f733c6a27c15 606 }//else if(state==METEN_RICHTING){
phgbartels 11:687aa4656a6e 607
phgbartels 11:687aa4656a6e 608 else if(state==INSTELLEN_RICHTING) {
phgbartels 11:687aa4656a6e 609 pc.printf("i");
phgbartels 10:cd89569cd847 610 lcd.cls();
phgbartels 10:cd89569cd847 611 lcd.locate(0,0);
phgbartels 10:cd89569cd847 612 lcd.printf("Instellen hoek");
phgbartels 10:cd89569cd847 613 lcd.locate(0,1);
phgbartels 11:687aa4656a6e 614 lcd.printf("Even ");
phgbartels 11:687aa4656a6e 615 pc.printf("e");
phgbartels 10:cd89569cd847 616 }//else if(state==INSTELLEN_RICHTING){
phgbartels 11:687aa4656a6e 617
phgbartels 11:687aa4656a6e 618 else if(state==SLAAN) {
phgbartels 10:cd89569cd847 619 lcd.cls();
phgbartels 10:cd89569cd847 620 lcd.locate(0,0);
phgbartels 10:cd89569cd847 621 lcd.printf("Slaan, pas op");
phgbartels 10:cd89569cd847 622 lcd.locate(0,1);
phgbartels 10:cd89569cd847 623 lcd.printf("Let's pray");
phgbartels 10:cd89569cd847 624 }//else if(state==INSTELLEN_RICHTING){
phgbartels 11:687aa4656a6e 625
phgbartels 11:687aa4656a6e 626 else if(state==RETURN2RUST) {
phgbartels 10:cd89569cd847 627 lcd.cls();
phgbartels 10:cd89569cd847 628 lcd.locate(0,0);
phgbartels 10:cd89569cd847 629 lcd.printf("Terug naar");
phgbartels 10:cd89569cd847 630 lcd.locate(0,1);
phgbartels 10:cd89569cd847 631 lcd.printf("0-positie");
phgbartels 10:cd89569cd847 632 }//else if(state==INSTELLEN_RICHTING){
phgbartels 11:687aa4656a6e 633
phgbartels 11:687aa4656a6e 634 else {
phgbartels 0:2386012c6594 635 lcd.cls();
phgbartels 0:2386012c6594 636 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 637 }//else{
phgbartels 0:2386012c6594 638 }
phgbartels 0:2386012c6594 639
phgbartels 11:687aa4656a6e 640 int main()
phgbartels 11:687aa4656a6e 641 {
phgbartels 4:377ddd65e4a6 642 pc.baud(115200);
lauradeheus 6:a7379a681adf 643 arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
lauradeheus 6:a7379a681adf 644 arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
lauradeheus 6:a7379a681adf 645 arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
lauradeheus 6:a7379a681adf 646 arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
phgbartels 4:377ddd65e4a6 647 statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
phgbartels 0:2386012c6594 648 screen.attach(&screenupdate, 0.2);
phgbartels 4:377ddd65e4a6 649 while(1);
phgbartels 0:2386012c6594 650 }