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@10:cd89569cd847, 2014-11-03 (annotated)

Committer:
phgbartels
Date:
Mon Nov 03 12:34:33 2014 +0000
Revision:
10:cd89569cd847
Parent:
9:7e9b63fe8988
Child:
11:687aa4656a6e
Main script werkend inclusief EMG en PID. Alleen slaan en beweging batje nog.

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