Laura Heus / Mbed 2 deprecated Main-script_groep7_V2

Alleen display nog goed instellen

Dependencies:   Encoder HIDScope TextLCD mbed-dsp mbed

Fork of Main-script_groep7 by Laura Heus

main.cpp@9:7e9b63fe8988, 2014-11-01 (annotated)

Committer:
lauradeheus
Date:
Sat Nov 01 16:29:46 2014 +0000
Revision:
9:7e9b63fe8988
Parent:
8:f733c6a27c15 
Ik heb het script zo aangepast dat alles het (ZOU) moeten doen. Niets is getest. Het script voor de LCD moet nog toegevoegd worden.

Who changed what in which revision?

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