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@8:f733c6a27c15, 2014-11-01 (annotated)

Committer:
lauradeheus
Date:
Sat Nov 01 13:20:22 2014 +0000
Revision:
8:f733c6a27c15
Parent:
7:7e3e183bf063
Child:
9:7e9b63fe8988
Opgeschoont script

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 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;
phgbartels 0:2386012c6594 60 int previous_pos_motor1 = 0;
phgbartels 0:2386012c6594 61 int current_pos_motor1;
phgbartels 5:4842219cb77c 62 int EMG = 1;
lauradeheus 6:a7379a681adf 63 int aantal_pieken;
lauradeheus 6:a7379a681adf 64 int doel;
lauradeheus 7:7e3e183bf063 65 int doel_richting;
lauradeheus 7:7e3e183bf063 66 int doel_hoogte;
lauradeheus 6:a7379a681adf 67 bool aanspan;
phgbartels 0:2386012c6594 68 void clamp(float * in, float min, float max);
phgbartels 0:2386012c6594 69 float pid(float setpoint, float measurement);
phgbartels 0:2386012c6594 70 float pos_motor1_rad;
phgbartels 0:2386012c6594 71 float PWM1_percentage = 0;
phgbartels 0:2386012c6594 72 float PWM1;
phgbartels 0:2386012c6594 73 float PWM2;
phgbartels 1:b08ac32d1ddc 74 float setpoint = 0;
lauradeheus 6:a7379a681adf 75 float prev_setpoint = 0;
lauradeheus 6:a7379a681adf 76 float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
lauradeheus 6:a7379a681adf 77 float lowpass_1_states[4];
lauradeheus 6:a7379a681adf 78 float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684};
lauradeheus 6:a7379a681adf 79 float lowpass_2_states[4];
lauradeheus 6:a7379a681adf 80 float highpass_const[] = {0.638945525159022 , -1.277891050318045 , 0.638945525159022 , 1.142980502539901 , -0.412801598096189};
lauradeheus 6:a7379a681adf 81 float highpass_states[4];
lauradeheus 6:a7379a681adf 82 float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362};
lauradeheus 6:a7379a681adf 83 float notch_states[4];
lauradeheus 6:a7379a681adf 84 float emg_filtered;
lauradeheus 6:a7379a681adf 85 float emg_max = 0;
lauradeheus 6:a7379a681adf 86 float emg_treshhold_laag = 0;
lauradeheus 6:a7379a681adf 87 float emg_treshhold_hoog = 0;
phgbartels 0:2386012c6594 88
lauradeheus 7:7e3e183bf063 89 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 90 state_t state = RUST;
phgbartels 0:2386012c6594 91
phgbartels 0:2386012c6594 92 void rust() {
phgbartels 0:2386012c6594 93 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
lauradeheus 8:f733c6a27c15 94 }//void rust
phgbartels 0:2386012c6594 95
lauradeheus 6:a7379a681adf 96 void pieken_tellen(){
lauradeheus 6:a7379a681adf 97 if (emg_filtered>=emg_treshhold_hoog)
lauradeheus 6:a7379a681adf 98 {
lauradeheus 6:a7379a681adf 99 aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
lauradeheus 8:f733c6a27c15 100 }//if
lauradeheus 6:a7379a681adf 101 if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt
lauradeheus 6:a7379a681adf 102 {
lauradeheus 6:a7379a681adf 103 aanspan=false;
lauradeheus 6:a7379a681adf 104 aantal_pieken++;
lauradeheus 6:a7379a681adf 105 doel = aantal_pieken-((aantal_pieken/3)*3)+1;
lauradeheus 8:f733c6a27c15 106 }//if
lauradeheus 8:f733c6a27c15 107 }//void pieken_tellen
lauradeheus 6:a7379a681adf 108
lauradeheus 6:a7379a681adf 109 void emg_filtering() {
lauradeheus 6:a7379a681adf 110 uint16_t emg_value;
lauradeheus 6:a7379a681adf 111 float emg_value_f32;
lauradeheus 6:a7379a681adf 112 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 113 emg_value_f32 = emg.read();
lauradeheus 6:a7379a681adf 114
lauradeheus 6:a7379a681adf 115 arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
lauradeheus 6:a7379a681adf 116 arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
lauradeheus 6:a7379a681adf 117 arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
lauradeheus 6:a7379a681adf 118 emg_filtered = fabs(emg_filtered);
lauradeheus 6:a7379a681adf 119 arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
lauradeheus 6:a7379a681adf 120 scope.set(0,emg_value); //uint value
lauradeheus 6:a7379a681adf 121 scope.set(1,emg_filtered); //processed float
lauradeheus 6:a7379a681adf 122 scope.set(2,doel);
lauradeheus 6:a7379a681adf 123 scope.send();
lauradeheus 6:a7379a681adf 124 if(state!=EMG_KALIBRATIE)
lauradeheus 6:a7379a681adf 125 {
lauradeheus 6:a7379a681adf 126 pieken_tellen();
lauradeheus 8:f733c6a27c15 127 }//if
lauradeheus 8:f733c6a27c15 128 }//void emg_filtering()
lauradeheus 6:a7379a681adf 129
lauradeheus 6:a7379a681adf 130 void emg_max_meting(){
lauradeheus 6:a7379a681adf 131 emg_filtering();
lauradeheus 6:a7379a681adf 132 if (emg_filtered>=emg_max)
lauradeheus 6:a7379a681adf 133 {
lauradeheus 6:a7379a681adf 134 emg_max=emg_filtered;
lauradeheus 8:f733c6a27c15 135 }//if
lauradeheus 6:a7379a681adf 136 emg_treshhold_laag = 0.3*emg_max;
lauradeheus 6:a7379a681adf 137 emg_treshhold_hoog = 0.7*emg_max;
lauradeheus 8:f733c6a27c15 138 }//void emg_max_meting
lauradeheus 6:a7379a681adf 139
phgbartels 0:2386012c6594 140 void emg_kalibratie() {
lauradeheus 7:7e3e183bf063 141 //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 142 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 143 //}
lauradeheus 6:a7379a681adf 144 if(current_herhalingen<=1000)
lauradeheus 6:a7379a681adf 145 {
lauradeheus 6:a7379a681adf 146 emg_max_meting();
lauradeheus 8:f733c6a27c15 147 }//if
lauradeheus 8:f733c6a27c15 148 }//void emg_kalibratie
phgbartels 0:2386012c6594 149
lauradeheus 7:7e3e183bf063 150 void arm_kalibratie() {
lauradeheus 7:7e3e183bf063 151 //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 152 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 153 motor1.setPosition(0);
lauradeheus 7:7e3e183bf063 154 motor2.setPosition(0);
lauradeheus 8:f733c6a27c15 155 }//void arm_kalibratie
lauradeheus 7:7e3e183bf063 156
lauradeheus 7:7e3e183bf063 157 void doel_bepaling() {
lauradeheus 7:7e3e183bf063 158 if(200<=current_herhalingen<=1200){
lauradeheus 7:7e3e183bf063 159 emg_filtering();
lauradeheus 8:f733c6a27c15 160 }//if
lauradeheus 7:7e3e183bf063 161 else if(1600<=current_herhalingen<=2200){
lauradeheus 7:7e3e183bf063 162 emg_filtering();
lauradeheus 8:f733c6a27c15 163 }//else if
lauradeheus 7:7e3e183bf063 164 else{
lauradeheus 8:f733c6a27c15 165 }//else
lauradeheus 8:f733c6a27c15 166 }//void doel_bepaling
phgbartels 0:2386012c6594 167
phgbartels 0:2386012c6594 168 void meten_hoogte() {
phgbartels 0:2386012c6594 169 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 170 doel_bepaling();
lauradeheus 7:7e3e183bf063 171 if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
lauradeheus 7:7e3e183bf063 172 doel=doel_hoogte;
lauradeheus 8:f733c6a27c15 173 }//if
lauradeheus 8:f733c6a27c15 174 }//void meten_hoogte
lauradeheus 7:7e3e183bf063 175
lauradeheus 7:7e3e183bf063 176 void meten_richting() {
lauradeheus 7:7e3e183bf063 177 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
lauradeheus 7:7e3e183bf063 178 doel_bepaling();
lauradeheus 7:7e3e183bf063 179 if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
lauradeheus 7:7e3e183bf063 180 doel=doel_richting;
lauradeheus 8:f733c6a27c15 181 }//if
lauradeheus 8:f733c6a27c15 182 }//void meten_richting
phgbartels 0:2386012c6594 183
phgbartels 0:2386012c6594 184 void instellen_richting() {
phgbartels 0:2386012c6594 185 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
lauradeheus 8:f733c6a27c15 186 }//void instellen_richting
phgbartels 0:2386012c6594 187
phgbartels 3:156c3e536ed4 188 void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
phgbartels 5:4842219cb77c 189
phgbartels 0:2386012c6594 190 current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
phgbartels 0:2386012c6594 191 pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
phgbartels 0:2386012c6594 192
phgbartels 0:2386012c6594 193 previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde.
phgbartels 0:2386012c6594 194
phgbartels 0:2386012c6594 195 //nu gaan we positie regelen i.p.v. snelheid.
phgbartels 5:4842219cb77c 196
phgbartels 5:4842219cb77c 197 if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop.
phgbartels 0:2386012c6594 198 {
phgbartels 3:156c3e536ed4 199 speed_radpersecond = 0;
phgbartels 4:377ddd65e4a6 200 setpoint = pos_motor1_rad;
phgbartels 2:de7b6c1d67c4 201 current_herhalingen = previous_herhalingen + 1;
phgbartels 2:de7b6c1d67c4 202 previous_herhalingen = current_herhalingen;
phgbartels 4:377ddd65e4a6 203 pc.printf("stop\n\r");
phgbartels 5:4842219cb77c 204 PWM1_percentage = 0;
lauradeheus 8:f733c6a27c15 205 }//if
phgbartels 4:377ddd65e4a6 206 else if(pos_motor1_rad - position_setpoint_rad < 0)
phgbartels 4:377ddd65e4a6 207 {
phgbartels 5:4842219cb77c 208 setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
phgbartels 5:4842219cb77c 209 PWM1_percentage = pid(setpoint, pos_motor1_rad);
lauradeheus 8:f733c6a27c15 210 }//else if
phgbartels 3:156c3e536ed4 211 else
phgbartels 4:377ddd65e4a6 212 {
phgbartels 5:4842219cb77c 213 setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
phgbartels 5:4842219cb77c 214 PWM1_percentage = pid(setpoint, pos_motor1_rad);
lauradeheus 8:f733c6a27c15 215 }//else
phgbartels 5:4842219cb77c 216 pc.printf("%f\n\r",PWM1_percentage);
phgbartels 4:377ddd65e4a6 217
phgbartels 0:2386012c6594 218 if (PWM1_percentage < -100)
phgbartels 0:2386012c6594 219 {
phgbartels 0:2386012c6594 220 PWM1_percentage = -100;
lauradeheus 8:f733c6a27c15 221 }//if
phgbartels 0:2386012c6594 222 else if (PWM1_percentage >100)
phgbartels 0:2386012c6594 223 {
phgbartels 0:2386012c6594 224 PWM1_percentage =100;
lauradeheus 8:f733c6a27c15 225 }//else if
phgbartels 0:2386012c6594 226
phgbartels 4:377ddd65e4a6 227 if(PWM1_percentage < 0)
phgbartels 0:2386012c6594 228 {
phgbartels 0:2386012c6594 229 motordir1 = 1;
lauradeheus 8:f733c6a27c15 230 }//if
phgbartels 0:2386012c6594 231 else
phgbartels 0:2386012c6594 232 {
phgbartels 0:2386012c6594 233 motordir1 = 0;
lauradeheus 8:f733c6a27c15 234 }//else
phgbartels 0:2386012c6594 235
lauradeheus 8:f733c6a27c15 236 pwm_motor1.write(abs(PWM1_percentage/100.))
phgbartels 0:2386012c6594 237 prev_setpoint = setpoint;
lauradeheus 8:f733c6a27c15 238 }//void GotoPosition
phgbartels 2:de7b6c1d67c4 239
phgbartels 0:2386012c6594 240 float pid(float setpoint, float measurement)
phgbartels 0:2386012c6594 241 {
phgbartels 0:2386012c6594 242 float error;
phgbartels 0:2386012c6594 243 static float prev_error = 0;
phgbartels 0:2386012c6594 244 float out_p = 0;
phgbartels 0:2386012c6594 245 static float out_i = 0;
phgbartels 0:2386012c6594 246 float out_d = 0;
phgbartels 0:2386012c6594 247 error = (setpoint-measurement);
phgbartels 0:2386012c6594 248 out_p = error*K_P;
phgbartels 0:2386012c6594 249 out_i += error*K_I;
phgbartels 0:2386012c6594 250 out_d = (error-prev_error)*K_D;
phgbartels 0:2386012c6594 251 clamp(&out_i,-I_LIMIT,I_LIMIT);
phgbartels 0:2386012c6594 252 prev_error = error;
phgbartels 0:2386012c6594 253 return out_p + out_i + out_d;
lauradeheus 8:f733c6a27c15 254 }//float pid
phgbartels 0:2386012c6594 255
lauradeheus 8:f733c6a27c15 256 void clamp(float* in, float min, float max)
phgbartels 0:2386012c6594 257 {
lauradeheus 8:f733c6a27c15 258 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min;
lauradeheus 8:f733c6a27c15 259 }//void clamp
phgbartels 0:2386012c6594 260
phgbartels 0:2386012c6594 261 void statemachinefunction()
phgbartels 0:2386012c6594 262 {
phgbartels 0:2386012c6594 263 switch(state) {
phgbartels 0:2386012c6594 264 case RUST: {
phgbartels 0:2386012c6594 265 rust();
phgbartels 0:2386012c6594 266 /*voorwaarde wanneer hij door kan naar de volgende case*/
phgbartels 5:4842219cb77c 267 if (current_herhalingen == 100 && EMG == 1)
phgbartels 0:2386012c6594 268 {
phgbartels 0:2386012c6594 269 current_herhalingen = 0;
phgbartels 0:2386012c6594 270 previous_herhalingen = 0;
phgbartels 1:b08ac32d1ddc 271 state = ARM_KALIBRATIE;
phgbartels 5:4842219cb77c 272 EMG = 0;
lauradeheus 8:f733c6a27c15 273 }//if (current_herhalingen == 100 && EMG == 1)
phgbartels 1:b08ac32d1ddc 274 break;
lauradeheus 8:f733c6a27c15 275 }//case RUST:
phgbartels 0:2386012c6594 276
phgbartels 0:2386012c6594 277 case ARM_KALIBRATIE:
phgbartels 0:2386012c6594 278 {
phgbartels 0:2386012c6594 279 arm_kalibratie();
phgbartels 5:4842219cb77c 280 if (current_herhalingen == 100)
phgbartels 0:2386012c6594 281 {
phgbartels 0:2386012c6594 282 current_herhalingen = 0;
phgbartels 0:2386012c6594 283 previous_herhalingen = 0;
phgbartels 1:b08ac32d1ddc 284 state = EMG_KALIBRATIE;
phgbartels 2:de7b6c1d67c4 285 motor1.setPosition(0);
phgbartels 2:de7b6c1d67c4 286 motor2.setPosition(0);
phgbartels 2:de7b6c1d67c4 287 pwm_motor1.period_us(100);
phgbartels 2:de7b6c1d67c4 288 pwm_motor2.period_us(100);
lauradeheus 8:f733c6a27c15 289 }//if (current_herhalingen == 100)
phgbartels 1:b08ac32d1ddc 290 break;
lauradeheus 8:f733c6a27c15 291 }//case ARM_KALIBRATIE:
phgbartels 0:2386012c6594 292
phgbartels 0:2386012c6594 293 case EMG_KALIBRATIE:
phgbartels 0:2386012c6594 294 {
phgbartels 0:2386012c6594 295 emg_kalibratie();
lauradeheus 6:a7379a681adf 296 if (current_herhalingen >=1000)
phgbartels 0:2386012c6594 297 {
lauradeheus 7:7e3e183bf063 298 state = METEN_HOOGTE;
lauradeheus 7:7e3e183bf063 299 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 300 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 301 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 302 doel = 0;
lauradeheus 8:f733c6a27c15 303 }//if (current_herhalingen >=1000)
lauradeheus 7:7e3e183bf063 304 break;
lauradeheus 8:f733c6a27c15 305 }//case EMG_KALIBRATIE
lauradeheus 7:7e3e183bf063 306
lauradeheus 7:7e3e183bf063 307 case METEN_HOOGTE:
lauradeheus 7:7e3e183bf063 308 {
lauradeheus 7:7e3e183bf063 309 meten_hoogte();
lauradeheus 7:7e3e183bf063 310 if (current_herhalingen == 2800 && aantal_pieken == 1)
lauradeheus 7:7e3e183bf063 311 {
phgbartels 0:2386012c6594 312 state = METEN_RICHTING;
phgbartels 0:2386012c6594 313 current_herhalingen = 0;
phgbartels 0:2386012c6594 314 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 315 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 316 doel = 0;
lauradeheus 8:f733c6a27c15 317 }//if (current_herhalingen == 2800 && aantal_pieken == 1)
lauradeheus 7:7e3e183bf063 318 else
lauradeheus 7:7e3e183bf063 319 {
lauradeheus 7:7e3e183bf063 320 state = METEN_HOOGTE;
lauradeheus 7:7e3e183bf063 321 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 322 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 323 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 324 doel = 0;
lauradeheus 8:f733c6a27c15 325 }///else
phgbartels 1:b08ac32d1ddc 326 break;
lauradeheus 8:f733c6a27c15 327 }//case METEN_HOOGTE
phgbartels 0:2386012c6594 328
phgbartels 0:2386012c6594 329 case METEN_RICHTING:
phgbartels 0:2386012c6594 330 {
phgbartels 0:2386012c6594 331 meten_richting();
lauradeheus 7:7e3e183bf063 332 if (current_herhalingen == 2800 && aantal_pieken == 1)
phgbartels 0:2386012c6594 333 {
phgbartels 0:2386012c6594 334 state = INSTELLEN_RICHTING;
phgbartels 0:2386012c6594 335 current_herhalingen = 0;
phgbartels 0:2386012c6594 336 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 337 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 338 doel = 0;
lauradeheus 8:f733c6a27c15 339 }//if (current_herhalingen == 2800 && aantal_pieken == 1)
lauradeheus 7:7e3e183bf063 340 else
lauradeheus 7:7e3e183bf063 341 {
lauradeheus 7:7e3e183bf063 342 state = METEN_RICHTING;
lauradeheus 7:7e3e183bf063 343 current_herhalingen = 0;
lauradeheus 7:7e3e183bf063 344 previous_herhalingen = 0;
lauradeheus 7:7e3e183bf063 345 aantal_pieken = 0;
lauradeheus 7:7e3e183bf063 346 doel = 0;
lauradeheus 8:f733c6a27c15 347 }//else
phgbartels 1:b08ac32d1ddc 348 break;
lauradeheus 8:f733c6a27c15 349 }//case METEN_RICHTING
phgbartels 0:2386012c6594 350
phgbartels 0:2386012c6594 351 case INSTELLEN_RICHTING:
phgbartels 0:2386012c6594 352 {
phgbartels 0:2386012c6594 353 instellen_richting();
phgbartels 5:4842219cb77c 354 if (current_herhalingen == 100)
phgbartels 0:2386012c6594 355 {
phgbartels 0:2386012c6594 356 state = SLAAN;
phgbartels 0:2386012c6594 357 current_herhalingen = 0;
phgbartels 0:2386012c6594 358 previous_herhalingen = 0;
lauradeheus 8:f733c6a27c15 359 }//if (current_herhalingen == 100)
lauradeheus 8:f733c6a27c15 360 break;
lauradeheus 8:f733c6a27c15 361 }//case INSTELLEN_RICHTING
phgbartels 0:2386012c6594 362
phgbartels 0:2386012c6594 363 case SLAAN:
phgbartels 0:2386012c6594 364 {
lauradeheus 8:f733c6a27c15 365 GotoPosition(1.5 ,8, 0.1);
phgbartels 5:4842219cb77c 366 if (current_herhalingen == 100)
phgbartels 0:2386012c6594 367 {
phgbartels 0:2386012c6594 368 state = RETURN2RUST;
phgbartels 0:2386012c6594 369 current_herhalingen = 0;
phgbartels 0:2386012c6594 370 previous_herhalingen = 0;
phgbartels 2:de7b6c1d67c4 371 prev_setpoint =0;
phgbartels 2:de7b6c1d67c4 372 setpoint =0;
lauradeheus 8:f733c6a27c15 373 }//if (current_herhalingen == 100)
phgbartels 2:de7b6c1d67c4 374 break;
lauradeheus 8:f733c6a27c15 375 }//case SLAAN
phgbartels 0:2386012c6594 376
phgbartels 0:2386012c6594 377 case RETURN2RUST:
phgbartels 0:2386012c6594 378 {
lauradeheus 8:f733c6a27c15 379 GotoPosition(0,4, 0.05);
lauradeheus 8:f733c6a27c15 380 //if (current_herhalingen == 100)
lauradeheus 8:f733c6a27c15 381 //{
lauradeheus 8:f733c6a27c15 382 // state = RUST;
lauradeheus 8:f733c6a27c15 383 // current_herhalingen = 0;
lauradeheus 8:f733c6a27c15 384 // previous_herhalingen = 0;
lauradeheus 8:f733c6a27c15 385 //}//if (current_herhalingen == 100)
phgbartels 1:b08ac32d1ddc 386 break;
lauradeheus 8:f733c6a27c15 387 }// case RETURN2RUST
phgbartels 0:2386012c6594 388
phgbartels 0:2386012c6594 389 default: {
phgbartels 0:2386012c6594 390 state = RUST;
lauradeheus 8:f733c6a27c15 391 }//default
phgbartels 0:2386012c6594 392
phgbartels 0:2386012c6594 393 }//switch(state)
phgbartels 0:2386012c6594 394 }//void statemachinefunction
phgbartels 0:2386012c6594 395
phgbartels 0:2386012c6594 396
phgbartels 0:2386012c6594 397 void screenupdate(){
phgbartels 0:2386012c6594 398 if(state==RUST){
phgbartels 0:2386012c6594 399 lcd.cls();
phgbartels 0:2386012c6594 400 lcd.locate(0,0);
phgbartels 5:4842219cb77c 401 lcd.printf("V.I.C.T.O.R.Y."); //regel 1 LCD scherm
phgbartels 0:2386012c6594 402 lcd.locate(0,1);
phgbartels 0:2386012c6594 403 lcd.printf(" GROEP 7 ");
lauradeheus 8:f733c6a27c15 404 }//if(state==RUST)
lauradeheus 8:f733c6a27c15 405
lauradeheus 6:a7379a681adf 406 else if(state==EMG_KALIBRATIE){
lauradeheus 6:a7379a681adf 407 lcd.cls();
lauradeheus 6:a7379a681adf 408 lcd.locate(0,0);
lauradeheus 6:a7379a681adf 409 lcd.printf("Max. aanspannen");
lauradeheus 6:a7379a681adf 410 if(current_herhalingen<=200){
lauradeheus 6:a7379a681adf 411 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 412 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 413 }//if(current_herhalingen<=200)
lauradeheus 6:a7379a681adf 414 else if(current_herhalingen<=400){
lauradeheus 6:a7379a681adf 415 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 416 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 417 }//else if(current_herhalingen<=400)
lauradeheus 6:a7379a681adf 418 else if(current_herhalingen<=600){
lauradeheus 6:a7379a681adf 419 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 420 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 421 }//else if(current_herhalingen<=600)
lauradeheus 6:a7379a681adf 422 else if(current_herhalingen<=800){
lauradeheus 6:a7379a681adf 423 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 424 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 425 }//else if(current_herhalingen<=800)
lauradeheus 6:a7379a681adf 426 else if(current_herhalingen<=1000){
lauradeheus 6:a7379a681adf 427 lcd.locate(0,1);
lauradeheus 6:a7379a681adf 428 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 429 }//else if(current_herhalingen<=1000)
lauradeheus 8:f733c6a27c15 430 }//else if(state==EMG_KALIBRATIE)
lauradeheus 8:f733c6a27c15 431
lauradeheus 7:7e3e183bf063 432 else if(state==METEN_HOOGTE){
lauradeheus 7:7e3e183bf063 433 lcd.cls();
lauradeheus 7:7e3e183bf063 434 if(current_herhalingen<=200){
lauradeheus 7:7e3e183bf063 435 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 436 lcd.printf("Hoogte bepalen:");
lauradeheus 7:7e3e183bf063 437 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 438 lcd.printf("span aan per vak");
lauradeheus 8:f733c6a27c15 439 }//if(current_herhalingen<=200){
lauradeheus 7:7e3e183bf063 440 else if(200<=current_herhalingen<=1200){
lauradeheus 7:7e3e183bf063 441 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 442 lcd.printf("Vak %d",doel);
lauradeheus 7:7e3e183bf063 443 if(current_herhalingen<=400){
lauradeheus 7:7e3e183bf063 444 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 445 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 446 }//if(current_herhalingen<=400)
lauradeheus 7:7e3e183bf063 447 else if(current_herhalingen<=600){
lauradeheus 7:7e3e183bf063 448 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 449 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 450 }//else if(current_herhalingen<=600)
lauradeheus 7:7e3e183bf063 451 else if(current_herhalingen<=800){
lauradeheus 7:7e3e183bf063 452 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 453 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 454 }//else if(current_herhalingen<=800)
lauradeheus 7:7e3e183bf063 455 else if(current_herhalingen<=1000){
lauradeheus 7:7e3e183bf063 456 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 457 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 458 }//else if(current_herhalingen<=1000)
lauradeheus 7:7e3e183bf063 459 else if(current_herhalingen<=1200){
lauradeheus 7:7e3e183bf063 460 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 461 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 462 }//else if(current_herhalingen<=1200)
lauradeheus 8:f733c6a27c15 463 }//else if(200<=current_herhalingen<=1200)
lauradeheus 7:7e3e183bf063 464 else if(current_herhalingen<=1600){
lauradeheus 7:7e3e183bf063 465 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 466 lcd.printf("Vak %d akkoord?",doel);
lauradeheus 7:7e3e183bf063 467 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 468 lcd.printf("Span aan");
lauradeheus 8:f733c6a27c15 469 }//else if(current_herhalingen<=1600){
lauradeheus 7:7e3e183bf063 470 else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
lauradeheus 7:7e3e183bf063 471 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 472 lcd.printf("Vak %d akkoord",doel_hoogte);
lauradeheus 8:f733c6a27c15 473 }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
lauradeheus 7:7e3e183bf063 474 else{
lauradeheus 7:7e3e183bf063 475 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 476 lcd.printf("Opnieuw hoogte");
lauradeheus 7:7e3e183bf063 477 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 478 lcd.printf("bepalen");
lauradeheus 8:f733c6a27c15 479 }//else{
lauradeheus 8:f733c6a27c15 480 }//else if(state==METEN_HOOGTE){
lauradeheus 8:f733c6a27c15 481
lauradeheus 7:7e3e183bf063 482 else if(state==METEN_RICHTING){
lauradeheus 7:7e3e183bf063 483 lcd.cls();
lauradeheus 7:7e3e183bf063 484 if(current_herhalingen<=200){
lauradeheus 7:7e3e183bf063 485 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 486 lcd.printf("Richting bepalen:");
lauradeheus 7:7e3e183bf063 487 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 488 lcd.printf("span aan per vak");
lauradeheus 8:f733c6a27c15 489 }//if(current_herhalingen<=200)
lauradeheus 7:7e3e183bf063 490 else if(200<=current_herhalingen<=1200){
lauradeheus 7:7e3e183bf063 491 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 492 lcd.printf("Vak %d",doel);
lauradeheus 7:7e3e183bf063 493 if(current_herhalingen<=400){
lauradeheus 7:7e3e183bf063 494 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 495 lcd.printf("nog 5 sec.");
lauradeheus 8:f733c6a27c15 496 }//if(current_herhalingen<=400)
lauradeheus 7:7e3e183bf063 497 else if(current_herhalingen<=600){
lauradeheus 7:7e3e183bf063 498 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 499 lcd.printf("nog 4 sec.");
lauradeheus 8:f733c6a27c15 500 }//else if(current_herhalingen<=600)
lauradeheus 7:7e3e183bf063 501 else if(current_herhalingen<=800){
lauradeheus 7:7e3e183bf063 502 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 503 lcd.printf("nog 3 sec.");
lauradeheus 8:f733c6a27c15 504 }//else if(current_herhalingen<=800)
lauradeheus 7:7e3e183bf063 505 else if(current_herhalingen<=1000){
lauradeheus 7:7e3e183bf063 506 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 507 lcd.printf("nog 2 sec.");
lauradeheus 8:f733c6a27c15 508 }//else if(current_herhalingen<=1000)
lauradeheus 7:7e3e183bf063 509 else if(current_herhalingen<=1200){
lauradeheus 7:7e3e183bf063 510 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 511 lcd.printf("nog 1 sec.");
lauradeheus 8:f733c6a27c15 512 }//else if(current_herhalingen<=1200)
lauradeheus 8:f733c6a27c15 513 }//else if(200<=current_herhalingen<=1200)
lauradeheus 7:7e3e183bf063 514 else if(current_herhalingen<=1600){
lauradeheus 7:7e3e183bf063 515 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 516 lcd.printf("Vak %d akkoord?",doel);
lauradeheus 7:7e3e183bf063 517 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 518 lcd.printf("Span aan");
lauradeheus 8:f733c6a27c15 519 }//else if(current_herhalingen<=1600)
lauradeheus 7:7e3e183bf063 520 else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
lauradeheus 7:7e3e183bf063 521 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 522 lcd.printf("Vak %d akkoord",doel_richting);
lauradeheus 8:f733c6a27c15 523 }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1)
lauradeheus 7:7e3e183bf063 524 else{
lauradeheus 7:7e3e183bf063 525 lcd.locate(0,0);
lauradeheus 7:7e3e183bf063 526 lcd.printf("Opnieuw richting");
lauradeheus 7:7e3e183bf063 527 lcd.locate(0,1);
lauradeheus 7:7e3e183bf063 528 lcd.printf("bepalen");
lauradeheus 8:f733c6a27c15 529 }//else
lauradeheus 8:f733c6a27c15 530 }//else if(state==METEN_RICHTING){
lauradeheus 8:f733c6a27c15 531
phgbartels 0:2386012c6594 532 else{
phgbartels 0:2386012c6594 533 lcd.cls();
phgbartels 0:2386012c6594 534 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 535 }//else{
phgbartels 0:2386012c6594 536 }
phgbartels 0:2386012c6594 537
lauradeheus 6:a7379a681adf 538 int main(){
phgbartels 4:377ddd65e4a6 539 pc.baud(115200);
lauradeheus 6:a7379a681adf 540 arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
lauradeheus 6:a7379a681adf 541 arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
lauradeheus 6:a7379a681adf 542 arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
lauradeheus 6:a7379a681adf 543 arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
phgbartels 4:377ddd65e4a6 544 statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
phgbartels 0:2386012c6594 545 screen.attach(&screenupdate, 0.2);
phgbartels 4:377ddd65e4a6 546 while(1);
phgbartels 0:2386012c6594 547 }