thijs ruikes / Mbed 2 deprecated PROJECT-ROBOTCONTROL

Robotcontrol groep 2

Dependencies:   Encoder MODSERIAL mbed HIDScope

main.cpp@12:e30bc8446379, 2014-11-04 (annotated)

Committer:
wiesdat
Date:
Tue Nov 04 10:56:19 2014 +0000
Revision:
12:e30bc8446379
Parent:
11:97759ceeb638
Child:
13:74035e04584a
Finale versie!!! <- dat is een bilanguisme

Who changed what in which revision?

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