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@7:7e3e183bf063, 2014-10-31 (annotated)

Committer:
lauradeheus
Date:
Fri Oct 31 17:14:30 2014 +0000
Revision:
7:7e3e183bf063
Parent:
6:a7379a681adf
Child:
8:f733c6a27c15
Main-script aanpassen dit weekend

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