thijs ruikes
/
PROJECT-ROBOTCONTROL-KNOPJES
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp@5:017f78f0340a, 2014-11-14 (annotated)
- Committer:
- wiesdat
- Date:
- Fri Nov 14 09:15:21 2014 +0000
- Revision:
- 5:017f78f0340a
- Parent:
- 4:7aa6bb892157
- Child:
- 6:337b3e0211d9
werkt, laatste wijzigingen ongedaan gemaakt
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| wiesdat | 0:1e91c3ae4e28 | 1 | //ROBOTCONTROL BMT M9 - GROEP 2 |
| wiesdat | 0:1e91c3ae4e28 | 2 | //Alex Overbeek |
| wiesdat | 0:1e91c3ae4e28 | 3 | //Tom Baumeister |
| wiesdat | 0:1e91c3ae4e28 | 4 | //Bas van Buuren |
| wiesdat | 0:1e91c3ae4e28 | 5 | //Bas Mattern |
| wiesdat | 0:1e91c3ae4e28 | 6 | //Thijs ruikes |
| wiesdat | 0:1e91c3ae4e28 | 7 | |
| wiesdat | 0:1e91c3ae4e28 | 8 | /*Het script bestaat uit drie delen: INCLUDE AND DEFINE, FUNCTIONS, MAINSCRIPT. Per gedeelte wordt uitgelegd wat gedaan wordt. |
| wiesdat | 0:1e91c3ae4e28 | 9 | We laten de beweging in fases lopen. Fase 1 werkt in dit script. Voor fase 2 kan de positie al worden ingesteld, de slagbeweging |
| wiesdat | 0:1e91c3ae4e28 | 10 | moet nog gemaakt worden. Dit gaan we doen met een PID regelaar die naar een snelheid toe regelt. |
| wiesdat | 0:1e91c3ae4e28 | 11 | */ |
| wiesdat | 0:1e91c3ae4e28 | 12 | |
| wiesdat | 0:1e91c3ae4e28 | 13 | //INCLUDE AND DEFINE ALL |
| wiesdat | 0:1e91c3ae4e28 | 14 | #include "mbed.h" |
| wiesdat | 0:1e91c3ae4e28 | 15 | #include "MODSERIAL.h" |
| wiesdat | 0:1e91c3ae4e28 | 16 | #include <iostream> |
| wiesdat | 0:1e91c3ae4e28 | 17 | #include "encoder.h" |
| wiesdat | 0:1e91c3ae4e28 | 18 | #include "HIDScope.h" |
| wiesdat | 0:1e91c3ae4e28 | 19 | |
| wiesdat | 0:1e91c3ae4e28 | 20 | #define THRESHOLD 0.8 |
| wiesdat | 0:1e91c3ae4e28 | 21 | #define NOSAMPL 500 |
| wiesdat | 0:1e91c3ae4e28 | 22 | |
| wiesdat | 0:1e91c3ae4e28 | 23 | #define TSAMP1 0.01 |
| wiesdat | 0:1e91c3ae4e28 | 24 | #define K_P1 (0.0015) |
| wiesdat | 0:1e91c3ae4e28 | 25 | #define K_I1 (0.0000001*TSAMP1) |
| wiesdat | 0:1e91c3ae4e28 | 26 | #define K_D1 (0.0003/TSAMP1) |
| wiesdat | 0:1e91c3ae4e28 | 27 | #define I_LIMIT1 1. |
| wiesdat | 0:1e91c3ae4e28 | 28 | |
| wiesdat | 0:1e91c3ae4e28 | 29 | //KPID voor slagfunctie |
| wiesdat | 0:1e91c3ae4e28 | 30 | #define KSLA_P (0.06) |
| wiesdat | 0:1e91c3ae4e28 | 31 | #define KSLA_I (0.00002 *TSAMP1) |
| wiesdat | 0:1e91c3ae4e28 | 32 | #define KSLA_D (0.0005 /TSAMP1) |
| wiesdat | 0:1e91c3ae4e28 | 33 | |
| wiesdat | 0:1e91c3ae4e28 | 34 | #define MAXPOS 750 |
| wiesdat | 0:1e91c3ae4e28 | 35 | |
| wiesdat | 0:1e91c3ae4e28 | 36 | // Constantes voor de filters definiëren: |
| wiesdat | 0:1e91c3ae4e28 | 37 | // Constantes voor de Low Pass filter |
| wiesdat | 0:1e91c3ae4e28 | 38 | #define A1LP 0.018180963222803 |
| wiesdat | 0:1e91c3ae4e28 | 39 | #define A0LP 0.016544013176248 |
| wiesdat | 0:1e91c3ae4e28 | 40 | #define B1LP -1.718913340044714 |
| wiesdat | 0:1e91c3ae4e28 | 41 | #define B0LP 0.753638316443765 |
| wiesdat | 0:1e91c3ae4e28 | 42 | // Constantes voor de High Pass Filter |
| wiesdat | 0:1e91c3ae4e28 | 43 | #define A1HP -1.999801878951505 |
| wiesdat | 0:1e91c3ae4e28 | 44 | #define A0HP 0.999801878951505 |
| wiesdat | 0:1e91c3ae4e28 | 45 | #define B1HP -1.971717601075000 |
| wiesdat | 0:1e91c3ae4e28 | 46 | #define B0HP 0.972111984032897 |
| wiesdat | 0:1e91c3ae4e28 | 47 | // Constantes voor het Notch Filter |
| wiesdat | 0:1e91c3ae4e28 | 48 | #define A0N 0.99436777112 |
| wiesdat | 0:1e91c3ae4e28 | 49 | #define A1N -1.89139989664 |
| wiesdat | 0:1e91c3ae4e28 | 50 | #define A2N 0.99436777112 |
| wiesdat | 0:1e91c3ae4e28 | 51 | #define B1N 1.89070035439 |
| wiesdat | 0:1e91c3ae4e28 | 52 | #define B2N -0.988036 |
| wiesdat | 0:1e91c3ae4e28 | 53 | |
| wiesdat | 0:1e91c3ae4e28 | 54 | // Sample Time |
| wiesdat | 0:1e91c3ae4e28 | 55 | #define TSAMP 0.001 |
| wiesdat | 0:1e91c3ae4e28 | 56 | |
| wiesdat | 0:1e91c3ae4e28 | 57 | // Outputs van de motor |
| wiesdat | 0:1e91c3ae4e28 | 58 | Encoder encoderA(PTD5,PTA13); |
| wiesdat | 0:1e91c3ae4e28 | 59 | Encoder encoder1(PTD0,PTD2); |
| wiesdat | 0:1e91c3ae4e28 | 60 | |
| wiesdat | 0:1e91c3ae4e28 | 61 | // Outputs naar de motor |
| wiesdat | 0:1e91c3ae4e28 | 62 | PwmOut pwm(PTC8); |
| wiesdat | 0:1e91c3ae4e28 | 63 | DigitalOut dir(PTC9); |
| wiesdat | 0:1e91c3ae4e28 | 64 | PwmOut pwm1(PTA5); |
| wiesdat | 0:1e91c3ae4e28 | 65 | DigitalOut dir1(PTA4); |
| wiesdat | 0:1e91c3ae4e28 | 66 | |
| wiesdat | 0:1e91c3ae4e28 | 67 | //lampjes |
| wiesdat | 0:1e91c3ae4e28 | 68 | DigitalOut rood(LED1); |
| wiesdat | 0:1e91c3ae4e28 | 69 | DigitalOut blauw(LED3); |
| wiesdat | 0:1e91c3ae4e28 | 70 | DigitalOut groen(LED2); |
| wiesdat | 0:1e91c3ae4e28 | 71 | |
| wiesdat | 0:1e91c3ae4e28 | 72 | //emg input |
| wiesdat | 0:1e91c3ae4e28 | 73 | AnalogIn emg1(PTB1); |
| wiesdat | 0:1e91c3ae4e28 | 74 | AnalogIn emg2(PTB2); |
| wiesdat | 0:1e91c3ae4e28 | 75 | |
| wiesdat | 0:1e91c3ae4e28 | 76 | // PC communicatie |
| wiesdat | 0:1e91c3ae4e28 | 77 | MODSERIAL pc(USBTX,USBRX); |
| wiesdat | 0:1e91c3ae4e28 | 78 | |
| wiesdat | 0:1e91c3ae4e28 | 79 | // Ticker voor de meetgegevens |
| wiesdat | 0:1e91c3ae4e28 | 80 | Ticker timer; |
| wiesdat | 0:1e91c3ae4e28 | 81 | volatile bool looptimerflag; |
| wiesdat | 0:1e91c3ae4e28 | 82 | |
| wiesdat | 0:1e91c3ae4e28 | 83 | // Waardes voor de filters reserveren en als float vaststellen |
| wiesdat | 0:1e91c3ae4e28 | 84 | float emg_value2, ylp2, yhp2, yn2,ysum1 = 0, yave1=0; |
| wiesdat | 0:1e91c3ae4e28 | 85 | float emg_value1, ylp1, yhp1, yn1,ysum2 = 0, yave2=0 ; |
| wiesdat | 0:1e91c3ae4e28 | 86 | |
| wiesdat | 0:1e91c3ae4e28 | 87 | //variabvelen voor positie motor1 |
| wiesdat | 0:1e91c3ae4e28 | 88 | float v1=0,out_i1 = 0; //out_i1 globaal gedef om reset |
| wiesdat | 0:1e91c3ae4e28 | 89 | |
| wiesdat | 0:1e91c3ae4e28 | 90 | // 0 of 1 waardes gedefinieerd uit het EMG, met 0 is te lage activiteit, 1 is hoog genoeg, voor de zekerheid toch als int. |
| wiesdat | 0:1e91c3ae4e28 | 91 | uint8_t y1, y2; |
| wiesdat | 0:1e91c3ae4e28 | 92 | int check = 0; |
| wiesdat | 0:1e91c3ae4e28 | 93 | |
| wiesdat | 0:1e91c3ae4e28 | 94 | // integers reserveren voor het deel van het regelsysteem als we de boolean emg waardes hebben: hoek van het badje (0,1, of 2) en de gewenste snelheid (0,1, of 2) |
| wiesdat | 0:1e91c3ae4e28 | 95 | int badjestand, badjes=1,badjedone=0,speeding,armstand=0,armspeed=0; |
| wiesdat | 0:1e91c3ae4e28 | 96 | bool speeddone=0; |
| wiesdat | 0:1e91c3ae4e28 | 97 | |
| wiesdat | 0:1e91c3ae4e28 | 98 | // Teller voor hoeveel metingen er zijn gedaan |
| wiesdat | 0:1e91c3ae4e28 | 99 | uint16_t teller=0; |
| wiesdat | 0:1e91c3ae4e28 | 100 | |
| wiesdat | 0:1e91c3ae4e28 | 101 | //Definieer alle functies |
| wiesdat | 0:1e91c3ae4e28 | 102 | void clamp(float * in, float min, float max); |
| wiesdat | 0:1e91c3ae4e28 | 103 | void clampint(int * in, int min, int max); |
| wiesdat | 0:1e91c3ae4e28 | 104 | void setlooptimerflag(void); |
| wiesdat | 0:1e91c3ae4e28 | 105 | float readEMG1(); |
| wiesdat | 0:1e91c3ae4e28 | 106 | float readEMG2(); |
| wiesdat | 0:1e91c3ae4e28 | 107 | float notchfilter1(float ylp1); |
| wiesdat | 0:1e91c3ae4e28 | 108 | float notchfilter2(float ylp2); |
| wiesdat | 0:1e91c3ae4e28 | 109 | float hpfilter1(float yn1); |
| wiesdat | 0:1e91c3ae4e28 | 110 | float hpfilter2(float yn2); |
| wiesdat | 0:1e91c3ae4e28 | 111 | float lpfilter1(float yhp1); |
| wiesdat | 0:1e91c3ae4e28 | 112 | float lpfilter2(float yhp2); |
| wiesdat | 0:1e91c3ae4e28 | 113 | float filter1(float emg_value1); |
| wiesdat | 0:1e91c3ae4e28 | 114 | float filter2(float emg_value2); |
| wiesdat | 0:1e91c3ae4e28 | 115 | float threshold1 (float yave1); |
| wiesdat | 0:1e91c3ae4e28 | 116 | float threshold2 (float yave2); |
| wiesdat | 0:1e91c3ae4e28 | 117 | float getv(float delta_t); |
| wiesdat | 0:1e91c3ae4e28 | 118 | float resetarm(); |
| wiesdat | 0:1e91c3ae4e28 | 119 | uint8_t badje (uint8_t y1, uint8_t y2); |
| wiesdat | 0:1e91c3ae4e28 | 120 | void batposition(int y); |
| wiesdat | 0:1e91c3ae4e28 | 121 | void sla(int k); |
| wiesdat | 0:1e91c3ae4e28 | 122 | float pidposition(float setpoint, float measurement); |
| wiesdat | 0:1e91c3ae4e28 | 123 | float pidarm(float rev_value, float mea_value); |
| wiesdat | 0:1e91c3ae4e28 | 124 | uint8_t armposition (uint8_t y1, uint8_t y2); |
| wiesdat | 0:1e91c3ae4e28 | 125 | |
| wiesdat | 0:1e91c3ae4e28 | 126 | /*FUNCTIES |
| wiesdat | 0:1e91c3ae4e28 | 127 | In deze sectie worden de functie geprogrammeerd, hieronder een uitleg per functie. De functies zijn verdeeld in drie groepen. In het script |
| wiesdat | 0:1e91c3ae4e28 | 128 | staat achter enkele functies een 1 of een 2. Er worden 2 signalen bewerkt, dus sommige functies staan dubbel in het script. Signaal 1 wordt |
| wiesdat | 0:1e91c3ae4e28 | 129 | door de functies met 1, en signaal 2 wordt door de functie met 2, bewerkt. |
| wiesdat | 0:1e91c3ae4e28 | 130 | |
| wiesdat | 0:1e91c3ae4e28 | 131 | GLOBAL FUNCTIONS |
| wiesdat | 0:1e91c3ae4e28 | 132 | Clamp - "clampt" een waarde binnen grenzen, als de geclampte variabele over de grens heen gaat krijgt de variabele de grenswaarde. |
| wiesdat | 0:1e91c3ae4e28 | 133 | setLooptimerflag - Hiermee wordt de ticker aangeroepen. |
| wiesdat | 0:1e91c3ae4e28 | 134 | |
| wiesdat | 0:1e91c3ae4e28 | 135 | EMG FUNCTIONS |
| wiesdat | 0:1e91c3ae4e28 | 136 | readEMG - Leest het voltage af van de geselecteerde pin, in ons geval een EMG waarde. |
| wiesdat | 0:1e91c3ae4e28 | 137 | (notch/lp/hp)filter -Er worden drie filters gebruikt: lowpass, highpass en notchfilter. De funtie krijgt een input en geeft dan een gefilterde output. |
| wiesdat | 0:1e91c3ae4e28 | 138 | filter - Voert de notch,lp,hp filters achter elkaar uit op een input en geeft het gemiddelde van 500 meetpunten als output. |
| wiesdat | 0:1e91c3ae4e28 | 139 | threshold - Vergelijkt de input met een treshold, als de waarde boven de treshold komt (zodra een spier wordt aangespannen) wordt de output 1, anders 0. |
| wiesdat | 0:1e91c3ae4e28 | 140 | |
| wiesdat | 0:1e91c3ae4e28 | 141 | MOTORCONTROLFUNCTIONS |
| wiesdat | 0:1e91c3ae4e28 | 142 | resetarm - Brengt arm terug naar de nul positie, zet de encoder op 0. |
| wiesdat | 0:1e91c3ae4e28 | 143 | getv(delta_t) - Geeft de huidige draaisnelheid van motor1, delta_t is het tijdsinterval waarover gemeten wordt |
| wiesdat | 0:1e91c3ae4e28 | 144 | badje - Vergelijkt met dubbele input, y1 en y2. Er zijn vier situaties, beide = 0, y1=1, y2=1, beide =1. |
| wiesdat | 0:1e91c3ae4e28 | 145 | badposition - Leest de output van badje, laat het badje links, recht, niet draaien. |
| wiesdat | 0:1e91c3ae4e28 | 146 | pid... - Pid regelaar, de k waardes wordenin de define sectie gedefineerd. |
| wiesdat | 0:1e91c3ae4e28 | 147 | armposition - Op basis van de emgsignalen wordt hier een positie ingesteld. |
| wiesdat | 0:1e91c3ae4e28 | 148 | armtopos - Regelt de arm naar de opgegeven positie toe. |
| wiesdat | 0:1e91c3ae4e28 | 149 | */ |
| wiesdat | 0:1e91c3ae4e28 | 150 | //HIDScope scope(5); |
| wiesdat | 0:1e91c3ae4e28 | 151 | //GLOBAL FUNCTIOMS |
| wiesdat | 0:1e91c3ae4e28 | 152 | /*void viewer(){ |
| wiesdat | 0:1e91c3ae4e28 | 153 | scope.set(0,yave1); |
| wiesdat | 0:1e91c3ae4e28 | 154 | scope.set(1,y1); |
| wiesdat | 0:1e91c3ae4e28 | 155 | scope.set(2,yave2); |
| wiesdat | 0:1e91c3ae4e28 | 156 | scope.set(3,y2); |
| wiesdat | 0:1e91c3ae4e28 | 157 | scope.send(); |
| wiesdat | 0:1e91c3ae4e28 | 158 | }*/ |
| wiesdat | 0:1e91c3ae4e28 | 159 | |
| wiesdat | 0:1e91c3ae4e28 | 160 | void clamp(float * in, float min, float max) |
| wiesdat | 0:1e91c3ae4e28 | 161 | { |
| wiesdat | 0:1e91c3ae4e28 | 162 | *in > min ? *in < max? : *in = max: *in = min; |
| wiesdat | 0:1e91c3ae4e28 | 163 | } |
| wiesdat | 0:1e91c3ae4e28 | 164 | |
| wiesdat | 0:1e91c3ae4e28 | 165 | void clampint(int * in, int min, int max) |
| wiesdat | 0:1e91c3ae4e28 | 166 | { |
| wiesdat | 0:1e91c3ae4e28 | 167 | *in > min ? *in < max? : *in = max: *in = min; |
| wiesdat | 0:1e91c3ae4e28 | 168 | } |
| wiesdat | 0:1e91c3ae4e28 | 169 | |
| wiesdat | 0:1e91c3ae4e28 | 170 | void setlooptimerflag(void) |
| wiesdat | 0:1e91c3ae4e28 | 171 | { |
| wiesdat | 0:1e91c3ae4e28 | 172 | looptimerflag = true; |
| wiesdat | 0:1e91c3ae4e28 | 173 | } |
| wiesdat | 0:1e91c3ae4e28 | 174 | |
| wiesdat | 0:1e91c3ae4e28 | 175 | //EMG FUNCTIONS |
| wiesdat | 0:1e91c3ae4e28 | 176 | float readEMG1() |
| wiesdat | 0:1e91c3ae4e28 | 177 | { |
| wiesdat | 0:1e91c3ae4e28 | 178 | emg_value1=emg1.read(); |
| wiesdat | 0:1e91c3ae4e28 | 179 | return emg_value1; |
| wiesdat | 0:1e91c3ae4e28 | 180 | } |
| wiesdat | 0:1e91c3ae4e28 | 181 | |
| wiesdat | 0:1e91c3ae4e28 | 182 | float readEMG2() |
| wiesdat | 0:1e91c3ae4e28 | 183 | { |
| wiesdat | 0:1e91c3ae4e28 | 184 | emg_value2=emg2.read(); |
| wiesdat | 0:1e91c3ae4e28 | 185 | return emg_value2; |
| wiesdat | 0:1e91c3ae4e28 | 186 | } |
| wiesdat | 0:1e91c3ae4e28 | 187 | |
| wiesdat | 0:1e91c3ae4e28 | 188 | float notchfilter1(float ylp1) |
| wiesdat | 0:1e91c3ae4e28 | 189 | { |
| wiesdat | 0:1e91c3ae4e28 | 190 | static float yn1,x1=0,x2=0,y1=0,y2=0,x; |
| wiesdat | 0:1e91c3ae4e28 | 191 | x = ylp1; |
| wiesdat | 0:1e91c3ae4e28 | 192 | yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2; |
| wiesdat | 0:1e91c3ae4e28 | 193 | x2 = x1; |
| wiesdat | 0:1e91c3ae4e28 | 194 | x1 = x; |
| wiesdat | 0:1e91c3ae4e28 | 195 | y2 = y1; |
| wiesdat | 0:1e91c3ae4e28 | 196 | y1 = yn1; |
| wiesdat | 0:1e91c3ae4e28 | 197 | return yn1; |
| wiesdat | 0:1e91c3ae4e28 | 198 | } |
| wiesdat | 0:1e91c3ae4e28 | 199 | |
| wiesdat | 0:1e91c3ae4e28 | 200 | float notchfilter2(float ylp2) |
| wiesdat | 0:1e91c3ae4e28 | 201 | { |
| wiesdat | 0:1e91c3ae4e28 | 202 | static float x1=0,x2=0,y1=0,y2=0,x; |
| wiesdat | 0:1e91c3ae4e28 | 203 | x = ylp2; |
| wiesdat | 0:1e91c3ae4e28 | 204 | yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2; |
| wiesdat | 0:1e91c3ae4e28 | 205 | x2 = x1; |
| wiesdat | 0:1e91c3ae4e28 | 206 | x1 = x; |
| wiesdat | 0:1e91c3ae4e28 | 207 | y2 = y1; |
| wiesdat | 0:1e91c3ae4e28 | 208 | y1 = yn2; |
| wiesdat | 0:1e91c3ae4e28 | 209 | return yn2; |
| wiesdat | 0:1e91c3ae4e28 | 210 | } |
| wiesdat | 0:1e91c3ae4e28 | 211 | |
| wiesdat | 0:1e91c3ae4e28 | 212 | float hpfilter1(float yn1) |
| wiesdat | 0:1e91c3ae4e28 | 213 | { |
| wiesdat | 0:1e91c3ae4e28 | 214 | static float x1=0,y1=0,x2=0, y2=0,x; |
| wiesdat | 0:1e91c3ae4e28 | 215 | x = yn1; |
| wiesdat | 0:1e91c3ae4e28 | 216 | yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2; |
| wiesdat | 0:1e91c3ae4e28 | 217 | x2 = x1; |
| wiesdat | 0:1e91c3ae4e28 | 218 | x1 = x; |
| wiesdat | 0:1e91c3ae4e28 | 219 | y2 = y1; |
| wiesdat | 0:1e91c3ae4e28 | 220 | y1 = yhp1; |
| wiesdat | 0:1e91c3ae4e28 | 221 | return yhp1; |
| wiesdat | 0:1e91c3ae4e28 | 222 | } |
| wiesdat | 0:1e91c3ae4e28 | 223 | |
| wiesdat | 0:1e91c3ae4e28 | 224 | float hpfilter2(float yn2) |
| wiesdat | 0:1e91c3ae4e28 | 225 | { |
| wiesdat | 0:1e91c3ae4e28 | 226 | static float x1=0,y1=0,x2=0, y2=0,x; |
| wiesdat | 0:1e91c3ae4e28 | 227 | x = yn2; |
| wiesdat | 0:1e91c3ae4e28 | 228 | yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2; |
| wiesdat | 0:1e91c3ae4e28 | 229 | x2 = x1; |
| wiesdat | 0:1e91c3ae4e28 | 230 | x1 = x; |
| wiesdat | 0:1e91c3ae4e28 | 231 | y2 = y1; |
| wiesdat | 0:1e91c3ae4e28 | 232 | y1 = yhp2; |
| wiesdat | 0:1e91c3ae4e28 | 233 | return yhp2; |
| wiesdat | 0:1e91c3ae4e28 | 234 | } |
| wiesdat | 0:1e91c3ae4e28 | 235 | |
| wiesdat | 0:1e91c3ae4e28 | 236 | float lpfilter1(float yhp1) |
| wiesdat | 0:1e91c3ae4e28 | 237 | { |
| wiesdat | 0:1e91c3ae4e28 | 238 | static float x1=0,y1=0,x2=0, y2=0,x; |
| wiesdat | 0:1e91c3ae4e28 | 239 | x = yhp1; |
| wiesdat | 0:1e91c3ae4e28 | 240 | ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2; |
| wiesdat | 0:1e91c3ae4e28 | 241 | x2 = x1; |
| wiesdat | 0:1e91c3ae4e28 | 242 | x1 = x; |
| wiesdat | 0:1e91c3ae4e28 | 243 | y2 = y1; |
| wiesdat | 0:1e91c3ae4e28 | 244 | y1 = ylp1; |
| wiesdat | 0:1e91c3ae4e28 | 245 | return ylp1; |
| wiesdat | 0:1e91c3ae4e28 | 246 | } |
| wiesdat | 0:1e91c3ae4e28 | 247 | |
| wiesdat | 0:1e91c3ae4e28 | 248 | float lpfilter2(float yhp2) |
| wiesdat | 0:1e91c3ae4e28 | 249 | { |
| wiesdat | 0:1e91c3ae4e28 | 250 | static float x1=0,y1=0,x2=0, y2=0,x; |
| wiesdat | 0:1e91c3ae4e28 | 251 | x = yhp2; |
| wiesdat | 0:1e91c3ae4e28 | 252 | ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2; |
| wiesdat | 0:1e91c3ae4e28 | 253 | x2 = x1; |
| wiesdat | 0:1e91c3ae4e28 | 254 | x1 = x; |
| wiesdat | 0:1e91c3ae4e28 | 255 | y2 = y1; |
| wiesdat | 0:1e91c3ae4e28 | 256 | y1 = ylp2; |
| wiesdat | 0:1e91c3ae4e28 | 257 | return ylp2; |
| wiesdat | 0:1e91c3ae4e28 | 258 | } |
| wiesdat | 0:1e91c3ae4e28 | 259 | |
| wiesdat | 0:1e91c3ae4e28 | 260 | float filter1(float emg_value1) |
| wiesdat | 0:1e91c3ae4e28 | 261 | { |
| wiesdat | 0:1e91c3ae4e28 | 262 | static uint16_t n; |
| wiesdat | 0:1e91c3ae4e28 | 263 | yn1 = notchfilter1(emg_value1); |
| wiesdat | 1:42fad02fb157 | 264 | //yhp1 = hpfilter1(yn1); |
| wiesdat | 1:42fad02fb157 | 265 | ylp1 = lpfilter1(yn1); |
| wiesdat | 1:42fad02fb157 | 266 | ylp1 = fabs(ylp1); |
| wiesdat | 0:1e91c3ae4e28 | 267 | ysum1 = ysum1+ylp1; |
| wiesdat | 0:1e91c3ae4e28 | 268 | n++; |
| wiesdat | 0:1e91c3ae4e28 | 269 | if(n==500) { |
| wiesdat | 0:1e91c3ae4e28 | 270 | yave1 = ysum1/500; |
| wiesdat | 0:1e91c3ae4e28 | 271 | ysum1 = 0; |
| wiesdat | 0:1e91c3ae4e28 | 272 | n = 0; |
| wiesdat | 0:1e91c3ae4e28 | 273 | } |
| wiesdat | 0:1e91c3ae4e28 | 274 | return yave1; |
| wiesdat | 0:1e91c3ae4e28 | 275 | } |
| wiesdat | 0:1e91c3ae4e28 | 276 | |
| wiesdat | 0:1e91c3ae4e28 | 277 | float filter2(float emg_value2) |
| wiesdat | 0:1e91c3ae4e28 | 278 | { |
| wiesdat | 0:1e91c3ae4e28 | 279 | static uint16_t n; |
| wiesdat | 0:1e91c3ae4e28 | 280 | yn2 = notchfilter2(emg_value2); |
| wiesdat | 1:42fad02fb157 | 281 | //yhp2 = hpfilter2(yn2); |
| wiesdat | 1:42fad02fb157 | 282 | ylp2 = lpfilter2(yn2); |
| wiesdat | 1:42fad02fb157 | 283 | ylp2 = fabs(ylp2); |
| wiesdat | 0:1e91c3ae4e28 | 284 | ysum2 = ysum2 + ylp2; |
| wiesdat | 0:1e91c3ae4e28 | 285 | n++; |
| wiesdat | 0:1e91c3ae4e28 | 286 | |
| wiesdat | 0:1e91c3ae4e28 | 287 | if(n==NOSAMPL) { |
| wiesdat | 0:1e91c3ae4e28 | 288 | yave2 = ysum2/NOSAMPL; |
| wiesdat | 0:1e91c3ae4e28 | 289 | ysum2 = 0; |
| wiesdat | 0:1e91c3ae4e28 | 290 | n = 0; |
| wiesdat | 0:1e91c3ae4e28 | 291 | } |
| wiesdat | 0:1e91c3ae4e28 | 292 | return yave2; |
| wiesdat | 0:1e91c3ae4e28 | 293 | } |
| wiesdat | 0:1e91c3ae4e28 | 294 | |
| wiesdat | 0:1e91c3ae4e28 | 295 | float threshold1 (float yave1) |
| wiesdat | 0:1e91c3ae4e28 | 296 | { |
| wiesdat | 0:1e91c3ae4e28 | 297 | if(yave1>THRESHOLD) { |
| wiesdat | 0:1e91c3ae4e28 | 298 | y1 = 1; |
| wiesdat | 0:1e91c3ae4e28 | 299 | } else { |
| wiesdat | 0:1e91c3ae4e28 | 300 | y1 = 0; |
| wiesdat | 0:1e91c3ae4e28 | 301 | } |
| wiesdat | 0:1e91c3ae4e28 | 302 | return y1; |
| wiesdat | 0:1e91c3ae4e28 | 303 | } |
| wiesdat | 0:1e91c3ae4e28 | 304 | |
| wiesdat | 0:1e91c3ae4e28 | 305 | float threshold2 (float yave2) |
| wiesdat | 0:1e91c3ae4e28 | 306 | { |
| wiesdat | 0:1e91c3ae4e28 | 307 | if(yave2>THRESHOLD) { |
| wiesdat | 0:1e91c3ae4e28 | 308 | y2 = 1; |
| wiesdat | 0:1e91c3ae4e28 | 309 | } else { |
| wiesdat | 0:1e91c3ae4e28 | 310 | y2 = 0; |
| wiesdat | 0:1e91c3ae4e28 | 311 | } |
| wiesdat | 0:1e91c3ae4e28 | 312 | return y2; |
| wiesdat | 0:1e91c3ae4e28 | 313 | } |
| wiesdat | 0:1e91c3ae4e28 | 314 | |
| wiesdat | 0:1e91c3ae4e28 | 315 | //MOTORCONTROL FUNCTIONS |
| wiesdat | 0:1e91c3ae4e28 | 316 | float getv(float delta_t) |
| wiesdat | 0:1e91c3ae4e28 | 317 | { |
| wiesdat | 0:1e91c3ae4e28 | 318 | int enca1=0,enca2=0,counts=0; |
| wiesdat | 0:1e91c3ae4e28 | 319 | float v; |
| wiesdat | 0:1e91c3ae4e28 | 320 | int n =0 ; |
| wiesdat | 0:1e91c3ae4e28 | 321 | while(n<3) { |
| wiesdat | 0:1e91c3ae4e28 | 322 | wait(delta_t); |
| wiesdat | 0:1e91c3ae4e28 | 323 | enca2 = enca1; |
| wiesdat | 0:1e91c3ae4e28 | 324 | enca1 = encoder1.getPosition(); |
| wiesdat | 0:1e91c3ae4e28 | 325 | n++; |
| wiesdat | 0:1e91c3ae4e28 | 326 | } |
| wiesdat | 0:1e91c3ae4e28 | 327 | counts = (enca1 - enca2)/delta_t; |
| wiesdat | 0:1e91c3ae4e28 | 328 | v = (counts)*((2*3.14159265359)/1550)*0.5; |
| wiesdat | 0:1e91c3ae4e28 | 329 | return v; |
| wiesdat | 0:1e91c3ae4e28 | 330 | } |
| wiesdat | 0:1e91c3ae4e28 | 331 | |
| wiesdat | 0:1e91c3ae4e28 | 332 | float resetarm() |
| wiesdat | 0:1e91c3ae4e28 | 333 | { |
| wiesdat | 0:1e91c3ae4e28 | 334 | v1 = 1; |
| wiesdat | 0:1e91c3ae4e28 | 335 | while(v1 !=0) { |
| wiesdat | 0:1e91c3ae4e28 | 336 | dir1 = 0; |
| wiesdat | 0:1e91c3ae4e28 | 337 | pwm1.write(0.1); |
| wiesdat | 0:1e91c3ae4e28 | 338 | v1 =getv(0.1); |
| wiesdat | 0:1e91c3ae4e28 | 339 | } |
| wiesdat | 0:1e91c3ae4e28 | 340 | pwm1 = 0; |
| wiesdat | 0:1e91c3ae4e28 | 341 | dir1 =1; |
| wiesdat | 0:1e91c3ae4e28 | 342 | encoder1.setPosition(0); |
| wiesdat | 0:1e91c3ae4e28 | 343 | return pwm1; |
| wiesdat | 0:1e91c3ae4e28 | 344 | } |
| wiesdat | 0:1e91c3ae4e28 | 345 | uint8_t badje (uint8_t y1, uint8_t y2) |
| wiesdat | 0:1e91c3ae4e28 | 346 | { |
| wiesdat | 0:1e91c3ae4e28 | 347 | if (y1>0 && y2>0 && check>0) { |
| wiesdat | 0:1e91c3ae4e28 | 348 | rood = 0; |
| wiesdat | 0:1e91c3ae4e28 | 349 | groen = 0; |
| wiesdat | 0:1e91c3ae4e28 | 350 | blauw = 1; |
| wiesdat | 0:1e91c3ae4e28 | 351 | badjedone=1; |
| wiesdat | 0:1e91c3ae4e28 | 352 | check=0; |
| wiesdat | 0:1e91c3ae4e28 | 353 | cout<<"ga naar mode 2"<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 354 | wait(1); |
| wiesdat | 0:1e91c3ae4e28 | 355 | } else if (y1>0 && y2>0) { |
| wiesdat | 0:1e91c3ae4e28 | 356 | check=1; |
| wiesdat | 0:1e91c3ae4e28 | 357 | } else if (y1>0) { |
| wiesdat | 0:1e91c3ae4e28 | 358 | badjes=0; |
| wiesdat | 0:1e91c3ae4e28 | 359 | check=0; |
| wiesdat | 0:1e91c3ae4e28 | 360 | } else if (y2>0 ) { |
| wiesdat | 0:1e91c3ae4e28 | 361 | badjes=1; |
| wiesdat | 0:1e91c3ae4e28 | 362 | check=0; |
| wiesdat | 0:1e91c3ae4e28 | 363 | } else { |
| wiesdat | 0:1e91c3ae4e28 | 364 | check=0; |
| wiesdat | 0:1e91c3ae4e28 | 365 | badjes = 2; |
| wiesdat | 0:1e91c3ae4e28 | 366 | } |
| wiesdat | 0:1e91c3ae4e28 | 367 | return badjes; |
| wiesdat | 0:1e91c3ae4e28 | 368 | } |
| wiesdat | 0:1e91c3ae4e28 | 369 | |
| wiesdat | 0:1e91c3ae4e28 | 370 | void batposition(int y) |
| wiesdat | 0:1e91c3ae4e28 | 371 | { |
| wiesdat | 0:1e91c3ae4e28 | 372 | switch(y) { |
| wiesdat | 0:1e91c3ae4e28 | 373 | case 0: |
| wiesdat | 0:1e91c3ae4e28 | 374 | |
| wiesdat | 0:1e91c3ae4e28 | 375 | dir = 1; |
| wiesdat | 0:1e91c3ae4e28 | 376 | pwm.write(0.4); |
| wiesdat | 2:41b58289fed5 | 377 | wait(0.02); |
| wiesdat | 0:1e91c3ae4e28 | 378 | pwm.write(0); |
| wiesdat | 0:1e91c3ae4e28 | 379 | cout<<"links"<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 380 | break; |
| wiesdat | 0:1e91c3ae4e28 | 381 | case 1: |
| wiesdat | 0:1e91c3ae4e28 | 382 | dir = 0; |
| wiesdat | 0:1e91c3ae4e28 | 383 | pwm.write(0.4); |
| wiesdat | 2:41b58289fed5 | 384 | wait(0.02); |
| wiesdat | 0:1e91c3ae4e28 | 385 | pwm.write(0); |
| wiesdat | 0:1e91c3ae4e28 | 386 | cout<<"rechts"<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 387 | break; |
| wiesdat | 0:1e91c3ae4e28 | 388 | case 2: |
| wiesdat | 0:1e91c3ae4e28 | 389 | pwm.write(0); |
| wiesdat | 0:1e91c3ae4e28 | 390 | break; |
| wiesdat | 0:1e91c3ae4e28 | 391 | |
| wiesdat | 0:1e91c3ae4e28 | 392 | } |
| wiesdat | 0:1e91c3ae4e28 | 393 | |
| wiesdat | 0:1e91c3ae4e28 | 394 | } |
| wiesdat | 0:1e91c3ae4e28 | 395 | |
| wiesdat | 0:1e91c3ae4e28 | 396 | void sla(int k) |
| wiesdat | 0:1e91c3ae4e28 | 397 | { |
| wiesdat | 0:1e91c3ae4e28 | 398 | float maxpwm; |
| wiesdat | 0:1e91c3ae4e28 | 399 | float new_pwm; |
| wiesdat | 0:1e91c3ae4e28 | 400 | switch(k) |
| wiesdat | 0:1e91c3ae4e28 | 401 | { |
| wiesdat | 0:1e91c3ae4e28 | 402 | case 1: |
| wiesdat | 0:1e91c3ae4e28 | 403 | maxpwm=0.6; //DE MAX PWM'en AANPASSEN VOOR DE ANDERE CASES!!!! |
| wiesdat | 0:1e91c3ae4e28 | 404 | break; |
| wiesdat | 0:1e91c3ae4e28 | 405 | case 2: |
| wiesdat | 0:1e91c3ae4e28 | 406 | maxpwm=0.8; |
| wiesdat | 0:1e91c3ae4e28 | 407 | break; |
| wiesdat | 0:1e91c3ae4e28 | 408 | case 3: |
| wiesdat | 0:1e91c3ae4e28 | 409 | maxpwm=1.0; |
| wiesdat | 0:1e91c3ae4e28 | 410 | break; |
| wiesdat | 0:1e91c3ae4e28 | 411 | /*case 4: |
| wiesdat | 0:1e91c3ae4e28 | 412 | maxpwm=0.2; |
| wiesdat | 0:1e91c3ae4e28 | 413 | break;*/ |
| wiesdat | 0:1e91c3ae4e28 | 414 | default: |
| wiesdat | 0:1e91c3ae4e28 | 415 | maxpwm=0; |
| wiesdat | 0:1e91c3ae4e28 | 416 | break; |
| wiesdat | 0:1e91c3ae4e28 | 417 | } |
| wiesdat | 0:1e91c3ae4e28 | 418 | while(abs(encoder1.getPosition()<MAXPOS)) |
| wiesdat | 0:1e91c3ae4e28 | 419 | { |
| wiesdat | 0:1e91c3ae4e28 | 420 | new_pwm=pidarm(MAXPOS,encoder1.getPosition()); |
| wiesdat | 0:1e91c3ae4e28 | 421 | clamp(&new_pwm,-1,maxpwm); |
| wiesdat | 0:1e91c3ae4e28 | 422 | if (new_pwm>0) |
| wiesdat | 0:1e91c3ae4e28 | 423 | { |
| wiesdat | 0:1e91c3ae4e28 | 424 | dir1=1; |
| wiesdat | 0:1e91c3ae4e28 | 425 | } |
| wiesdat | 0:1e91c3ae4e28 | 426 | else if (new_pwm<0) |
| wiesdat | 0:1e91c3ae4e28 | 427 | { |
| wiesdat | 0:1e91c3ae4e28 | 428 | dir1=0; |
| wiesdat | 0:1e91c3ae4e28 | 429 | } |
| wiesdat | 0:1e91c3ae4e28 | 430 | pwm1.write(fabs(new_pwm)); |
| wiesdat | 0:1e91c3ae4e28 | 431 | cout<<"pwm1: "<<new_pwm<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 432 | cout<<"dir: "<<dir<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 433 | cout<<"v: "<<getv(0.01)<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 434 | cout<<"pos: "<<encoder1.getPosition()<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 435 | } |
| wiesdat | 0:1e91c3ae4e28 | 436 | dir1=0; |
| wiesdat | 0:1e91c3ae4e28 | 437 | pwm1.write(.5); |
| wiesdat | 0:1e91c3ae4e28 | 438 | wait(.1); |
| wiesdat | 0:1e91c3ae4e28 | 439 | pwm1.write(0); |
| wiesdat | 0:1e91c3ae4e28 | 440 | wait(1); |
| wiesdat | 0:1e91c3ae4e28 | 441 | //cout<< encoder1.getPosition()<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 442 | } |
| wiesdat | 0:1e91c3ae4e28 | 443 | |
| wiesdat | 0:1e91c3ae4e28 | 444 | float pidposition(float setpoint, float measurement) |
| wiesdat | 0:1e91c3ae4e28 | 445 | { |
| wiesdat | 0:1e91c3ae4e28 | 446 | float error, out, out_p=0,out_d=0; |
| wiesdat | 0:1e91c3ae4e28 | 447 | static float prev_error = 0; |
| wiesdat | 0:1e91c3ae4e28 | 448 | error = setpoint-measurement; |
| wiesdat | 0:1e91c3ae4e28 | 449 | out_p = error*K_P1; |
| wiesdat | 0:1e91c3ae4e28 | 450 | out_i1 += error*K_I1; |
| wiesdat | 0:1e91c3ae4e28 | 451 | out_d = (error-prev_error)*K_D1; |
| wiesdat | 0:1e91c3ae4e28 | 452 | clamp(&out_i1,-I_LIMIT1,I_LIMIT1); |
| wiesdat | 0:1e91c3ae4e28 | 453 | prev_error = error; |
| wiesdat | 0:1e91c3ae4e28 | 454 | out = out_i1+out_p+out_d; |
| wiesdat | 0:1e91c3ae4e28 | 455 | return out; |
| wiesdat | 0:1e91c3ae4e28 | 456 | } |
| wiesdat | 0:1e91c3ae4e28 | 457 | |
| wiesdat | 0:1e91c3ae4e28 | 458 | float pidarm(float rev_value, float mea_value) |
| wiesdat | 0:1e91c3ae4e28 | 459 | { |
| wiesdat | 0:1e91c3ae4e28 | 460 | float error; |
| wiesdat | 0:1e91c3ae4e28 | 461 | static float prev_error = 0; |
| wiesdat | 0:1e91c3ae4e28 | 462 | float p_out = 0; |
| wiesdat | 0:1e91c3ae4e28 | 463 | static float i_out = 0; |
| wiesdat | 0:1e91c3ae4e28 | 464 | float d_out = 0; |
| wiesdat | 0:1e91c3ae4e28 | 465 | error = rev_value - mea_value; |
| wiesdat | 0:1e91c3ae4e28 | 466 | p_out = error * KSLA_P; |
| wiesdat | 0:1e91c3ae4e28 | 467 | i_out += error * KSLA_I; |
| wiesdat | 0:1e91c3ae4e28 | 468 | d_out = (error - prev_error) * KSLA_D; |
| wiesdat | 0:1e91c3ae4e28 | 469 | clamp(&i_out,-I_LIMIT1,I_LIMIT1); |
| wiesdat | 0:1e91c3ae4e28 | 470 | prev_error=error; |
| wiesdat | 0:1e91c3ae4e28 | 471 | return p_out + i_out + d_out; |
| wiesdat | 0:1e91c3ae4e28 | 472 | } |
| wiesdat | 0:1e91c3ae4e28 | 473 | |
| wiesdat | 0:1e91c3ae4e28 | 474 | |
| wiesdat | 0:1e91c3ae4e28 | 475 | uint8_t armposition (uint8_t y1, uint8_t y2) |
| wiesdat | 0:1e91c3ae4e28 | 476 | { |
| wiesdat | 0:1e91c3ae4e28 | 477 | static int stand=0; |
| wiesdat | 0:1e91c3ae4e28 | 478 | if (y1>0 && y2>0 && check>0) { |
| wiesdat | 0:1e91c3ae4e28 | 479 | rood=0; |
| wiesdat | 0:1e91c3ae4e28 | 480 | groen=0; |
| wiesdat | 0:1e91c3ae4e28 | 481 | blauw=1; |
| wiesdat | 0:1e91c3ae4e28 | 482 | badjedone=2; |
| wiesdat | 0:1e91c3ae4e28 | 483 | check=0; |
| wiesdat | 0:1e91c3ae4e28 | 484 | /*rood = 0; |
| wiesdat | 0:1e91c3ae4e28 | 485 | groen = 1;*/ |
| wiesdat | 0:1e91c3ae4e28 | 486 | } else if (y1>0 && y2>0) { |
| wiesdat | 0:1e91c3ae4e28 | 487 | check=1; |
| wiesdat | 5:017f78f0340a | 488 | } else if (y1>0) { |
| wiesdat | 0:1e91c3ae4e28 | 489 | stand=stand+1; |
| wiesdat | 0:1e91c3ae4e28 | 490 | check=0; |
| wiesdat | 0:1e91c3ae4e28 | 491 | cout<<"stand "<<stand<<endl; |
| wiesdat | 5:017f78f0340a | 492 | } else if (y2>0) { |
| wiesdat | 0:1e91c3ae4e28 | 493 | stand=stand-1; |
| wiesdat | 0:1e91c3ae4e28 | 494 | check=0; |
| wiesdat | 0:1e91c3ae4e28 | 495 | cout<<"stand "<<stand<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 496 | } else { |
| wiesdat | 0:1e91c3ae4e28 | 497 | |
| wiesdat | 0:1e91c3ae4e28 | 498 | check =0; |
| wiesdat | 0:1e91c3ae4e28 | 499 | } |
| wiesdat | 0:1e91c3ae4e28 | 500 | clampint(&stand,1,3); |
| wiesdat | 0:1e91c3ae4e28 | 501 | return stand; |
| wiesdat | 0:1e91c3ae4e28 | 502 | } |
| wiesdat | 0:1e91c3ae4e28 | 503 | |
| wiesdat | 0:1e91c3ae4e28 | 504 | float gotopos(int pos) |
| wiesdat | 0:1e91c3ae4e28 | 505 | { |
| wiesdat | 0:1e91c3ae4e28 | 506 | float out1; |
| wiesdat | 0:1e91c3ae4e28 | 507 | |
| wiesdat | 0:1e91c3ae4e28 | 508 | switch(pos) { |
| wiesdat | 0:1e91c3ae4e28 | 509 | case 1: |
| wiesdat | 0:1e91c3ae4e28 | 510 | pos = 100; |
| wiesdat | 0:1e91c3ae4e28 | 511 | break; |
| wiesdat | 0:1e91c3ae4e28 | 512 | |
| wiesdat | 0:1e91c3ae4e28 | 513 | case 2: |
| wiesdat | 0:1e91c3ae4e28 | 514 | pos = 200; |
| wiesdat | 0:1e91c3ae4e28 | 515 | break; |
| wiesdat | 0:1e91c3ae4e28 | 516 | |
| wiesdat | 0:1e91c3ae4e28 | 517 | case 3: |
| wiesdat | 0:1e91c3ae4e28 | 518 | pos = 300; |
| wiesdat | 0:1e91c3ae4e28 | 519 | break; |
| wiesdat | 0:1e91c3ae4e28 | 520 | default: |
| wiesdat | 0:1e91c3ae4e28 | 521 | break; |
| wiesdat | 0:1e91c3ae4e28 | 522 | } |
| wiesdat | 0:1e91c3ae4e28 | 523 | |
| wiesdat | 0:1e91c3ae4e28 | 524 | while((abs(pos-encoder1.getPosition()) >15)|| (v1>0.1)) { |
| wiesdat | 0:1e91c3ae4e28 | 525 | |
| wiesdat | 0:1e91c3ae4e28 | 526 | while(!looptimerflag); |
| wiesdat | 0:1e91c3ae4e28 | 527 | looptimerflag = false; |
| wiesdat | 0:1e91c3ae4e28 | 528 | cout << "Deltapos: " << abs(pos-encoder1.getPosition()) << endl; |
| wiesdat | 0:1e91c3ae4e28 | 529 | out1 = pidposition(pos,encoder1.getPosition()); |
| wiesdat | 0:1e91c3ae4e28 | 530 | clamp(&out1,-1,1); |
| wiesdat | 0:1e91c3ae4e28 | 531 | cout <<"out1: " << out1 <<endl; |
| wiesdat | 0:1e91c3ae4e28 | 532 | if(out1>0) { |
| wiesdat | 0:1e91c3ae4e28 | 533 | dir1 = 1; |
| wiesdat | 0:1e91c3ae4e28 | 534 | |
| wiesdat | 0:1e91c3ae4e28 | 535 | } else if(out1<0) { |
| wiesdat | 0:1e91c3ae4e28 | 536 | dir1 = 0; |
| wiesdat | 0:1e91c3ae4e28 | 537 | } |
| wiesdat | 0:1e91c3ae4e28 | 538 | else{ |
| wiesdat | 0:1e91c3ae4e28 | 539 | } |
| wiesdat | 0:1e91c3ae4e28 | 540 | pwm1 = fabs(out1); |
| wiesdat | 0:1e91c3ae4e28 | 541 | v1 = getv(0.001); |
| wiesdat | 0:1e91c3ae4e28 | 542 | } |
| wiesdat | 0:1e91c3ae4e28 | 543 | pwm1 =0; |
| wiesdat | 0:1e91c3ae4e28 | 544 | |
| wiesdat | 0:1e91c3ae4e28 | 545 | return pwm1; |
| wiesdat | 0:1e91c3ae4e28 | 546 | } |
| wiesdat | 0:1e91c3ae4e28 | 547 | |
| wiesdat | 0:1e91c3ae4e28 | 548 | // MAIN SCRIPT |
| wiesdat | 0:1e91c3ae4e28 | 549 | int main() |
| wiesdat | 0:1e91c3ae4e28 | 550 | { |
| wiesdat | 0:1e91c3ae4e28 | 551 | /*rood = 0; |
| wiesdat | 0:1e91c3ae4e28 | 552 | blauw = 1; |
| wiesdat | 0:1e91c3ae4e28 | 553 | groen = 1;*/ |
| wiesdat | 0:1e91c3ae4e28 | 554 | resetarm(); |
| wiesdat | 0:1e91c3ae4e28 | 555 | pc.baud(115200); |
| wiesdat | 0:1e91c3ae4e28 | 556 | timer.attach(setlooptimerflag,TSAMP); |
| wiesdat | 0:1e91c3ae4e28 | 557 | wait(2); |
| wiesdat | 0:1e91c3ae4e28 | 558 | cout<<"Begin programma"<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 559 | while(1) { |
| wiesdat | 0:1e91c3ae4e28 | 560 | //Per TSAMP word EMG uitgelezen, gefilterd en gemiddeld |
| wiesdat | 0:1e91c3ae4e28 | 561 | while(!looptimerflag); |
| wiesdat | 0:1e91c3ae4e28 | 562 | looptimerflag = false; |
| wiesdat | 0:1e91c3ae4e28 | 563 | emg_value1 = readEMG1(); |
| wiesdat | 0:1e91c3ae4e28 | 564 | emg_value2 = readEMG2(); |
| wiesdat | 0:1e91c3ae4e28 | 565 | yave1 = filter1(emg_value1); |
| wiesdat | 0:1e91c3ae4e28 | 566 | yave2 = filter2(emg_value2); |
| wiesdat | 0:1e91c3ae4e28 | 567 | y1=threshold1(yave1); |
| wiesdat | 0:1e91c3ae4e28 | 568 | y2=threshold2(yave2); |
| wiesdat | 0:1e91c3ae4e28 | 569 | teller++; |
| wiesdat | 0:1e91c3ae4e28 | 570 | |
| wiesdat | 0:1e91c3ae4e28 | 571 | /*Dit gedeelte voert de bewegingen uit. In de eerste fase kan met de linker spier het batje naar links gedraaid worden, |
| wiesdat | 0:1e91c3ae4e28 | 572 | en met de rechter spier het batje naar recht. Als het badje in de juiste positie staat kunnen beide spieren tegelijk |
| wiesdat | 0:1e91c3ae4e28 | 573 | aangespannen worden om naar de volgende fase te gaan. In fase 2 wordt de armpositie ingesteld. Hoe groter de afstand van de |
| wiesdat | 0:1e91c3ae4e28 | 574 | arm met de bal, hoe harder de arm gaat slaan. Zodra de arm in de goede positie staat kan door beide spieren aan te spannen worden |
| wiesdat | 0:1e91c3ae4e28 | 575 | doorgegaan naar fase 3. In fase 3 slaat de arm de bal. De snelheid die ingesteld wordt waarmee de arm gaat slaan wordt bepaald aan de |
| wiesdat | 0:1e91c3ae4e28 | 576 | hand van de positie waarin de arm in fase 2 is gezet. Zodra de arm de bal geslagen heeft moet de arm gereset worden en terug gaan naar |
| wiesdat | 0:1e91c3ae4e28 | 577 | fase 1.*/ |
| wiesdat | 0:1e91c3ae4e28 | 578 | |
| wiesdat | 0:1e91c3ae4e28 | 579 | if (teller==NOSAMPL) { |
| wiesdat | 0:1e91c3ae4e28 | 580 | teller=0; |
| wiesdat | 0:1e91c3ae4e28 | 581 | switch(badjedone) { |
| wiesdat | 0:1e91c3ae4e28 | 582 | case 0: |
| wiesdat | 0:1e91c3ae4e28 | 583 | rood=0; |
| wiesdat | 0:1e91c3ae4e28 | 584 | groen=1; |
| wiesdat | 0:1e91c3ae4e28 | 585 | blauw=1; |
| wiesdat | 0:1e91c3ae4e28 | 586 | cout<<"fase1"<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 587 | badjestand=badje(y1,y2); |
| wiesdat | 0:1e91c3ae4e28 | 588 | batposition(badjestand); |
| wiesdat | 5:017f78f0340a | 589 | armstand=0; |
| wiesdat | 0:1e91c3ae4e28 | 590 | break; |
| wiesdat | 0:1e91c3ae4e28 | 591 | |
| wiesdat | 0:1e91c3ae4e28 | 592 | case 1: |
| wiesdat | 0:1e91c3ae4e28 | 593 | rood=1; |
| wiesdat | 0:1e91c3ae4e28 | 594 | groen=0; |
| wiesdat | 0:1e91c3ae4e28 | 595 | blauw=1; |
| wiesdat | 0:1e91c3ae4e28 | 596 | cout<<"fase2"<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 597 | armstand=armposition(y1,y2); |
| wiesdat | 0:1e91c3ae4e28 | 598 | gotopos(armstand); |
| wiesdat | 0:1e91c3ae4e28 | 599 | cout<<"armstand "<<armstand<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 600 | |
| wiesdat | 0:1e91c3ae4e28 | 601 | //armtopos(armstand); |
| wiesdat | 0:1e91c3ae4e28 | 602 | //cout<<"badjedont: "<<badjedone<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 603 | break; |
| wiesdat | 0:1e91c3ae4e28 | 604 | |
| wiesdat | 0:1e91c3ae4e28 | 605 | case 2: |
| wiesdat | 0:1e91c3ae4e28 | 606 | rood=1; |
| wiesdat | 0:1e91c3ae4e28 | 607 | groen=1; |
| wiesdat | 0:1e91c3ae4e28 | 608 | blauw=0; |
| wiesdat | 0:1e91c3ae4e28 | 609 | resetarm(); |
| wiesdat | 0:1e91c3ae4e28 | 610 | cout<<"sla"<<endl; |
| wiesdat | 0:1e91c3ae4e28 | 611 | sla(armstand); |
| wiesdat | 0:1e91c3ae4e28 | 612 | resetarm(); |
| wiesdat | 0:1e91c3ae4e28 | 613 | badjedone=0; |
| wiesdat | 0:1e91c3ae4e28 | 614 | break; |
| wiesdat | 0:1e91c3ae4e28 | 615 | default: |
| wiesdat | 0:1e91c3ae4e28 | 616 | break; |
| wiesdat | 0:1e91c3ae4e28 | 617 | } |
| wiesdat | 0:1e91c3ae4e28 | 618 | |
| wiesdat | 0:1e91c3ae4e28 | 619 | } |
| wiesdat | 0:1e91c3ae4e28 | 620 | } |
| wiesdat | 0:1e91c3ae4e28 | 621 | |
| wiesdat | 0:1e91c3ae4e28 | 622 | } |
| wiesdat | 0:1e91c3ae4e28 | 623 | |
| wiesdat | 0:1e91c3ae4e28 | 624 |