thijs ruikes / Mbed 2 deprecated PROJECT-ROBOTCONTROL

Robotcontrol groep 2

Dependencies:   Encoder MODSERIAL mbed HIDScope

main.cpp@6:b69b9597d4fc, 2014-11-03 (annotated)

Committer:
wiesdat
Date:
Mon Nov 03 15:01:09 2014 +0000
Revision:
6:b69b9597d4fc
Parent:
5:36df561f3ac1
Child:
7:2e4eb23700b0
arm instellen ok

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 5:36df561f3ac1 20 #define THRESHOLD 0.04
wiesdat 5:36df561f3ac1 21 #define NOSAMPL 500
wiesdat 0:25a2e7ea29f3 22
wiesdat 3:611fd72c9d46 23 #define K_P1 (0.004)
wiesdat 3:611fd72c9d46 24 #define K_I1 (0.00001*TSAMP1)
wiesdat 3:611fd72c9d46 25 #define K_D1 (0.0001/TSAMP1)
wiesdat 3:611fd72c9d46 26 #define TSAMP1 0.01
wiesdat 3:611fd72c9d46 27 #define I_LIMIT1 1.
wiesdat 4:c22f3095b130 28 // Constantes voor de filters definiëren:
wiesdat 0:25a2e7ea29f3 29 // Constantes voor de Low Pass filter
wiesdat 0:25a2e7ea29f3 30 #define A1LP 0.018180963222803
wiesdat 0:25a2e7ea29f3 31 #define A0LP 0.016544013176248
wiesdat 0:25a2e7ea29f3 32 #define B1LP -1.718913340044714
wiesdat 0:25a2e7ea29f3 33 #define B0LP 0.753638316443765
wiesdat 0:25a2e7ea29f3 34 // Constantes voor de High Pass Filter
wiesdat 0:25a2e7ea29f3 35 #define A1HP -1.999801878951505
wiesdat 0:25a2e7ea29f3 36 #define A0HP 0.999801878951505
wiesdat 0:25a2e7ea29f3 37 #define B1HP -1.971717601075000
wiesdat 0:25a2e7ea29f3 38 #define B0HP 0.972111984032897
wiesdat 0:25a2e7ea29f3 39 // Constantes voor het Notch Filter
wiesdat 0:25a2e7ea29f3 40 #define A0N 0.99436777112
wiesdat 0:25a2e7ea29f3 41 #define A1N -1.89139989664
wiesdat 0:25a2e7ea29f3 42 #define A2N 0.99436777112
wiesdat 0:25a2e7ea29f3 43 #define B1N 1.89070035439
wiesdat 0:25a2e7ea29f3 44 #define B2N -0.988036
wiesdat 0:25a2e7ea29f3 45
wiesdat 0:25a2e7ea29f3 46 // Sample Time
wiesdat 0:25a2e7ea29f3 47 #define TSAMP 0.001
wiesdat 4:c22f3095b130 48
wiesdat 3:611fd72c9d46 49 // Outputs van de motor
wiesdat 3:611fd72c9d46 50 Encoder encoderA(PTD5,PTA13);
wiesdat 3:611fd72c9d46 51 Encoder encoder1(PTD0,PTD2);
wiesdat 0:25a2e7ea29f3 52
wiesdat 3:611fd72c9d46 53 // Outputs naar de motor
wiesdat 3:611fd72c9d46 54 PwmOut pwm(PTC8);
wiesdat 3:611fd72c9d46 55 DigitalOut dir(PTC9);
wiesdat 3:611fd72c9d46 56 PwmOut pwm1(PTA5);
wiesdat 3:611fd72c9d46 57 DigitalOut dir1(PTA4);
wiesdat 3:611fd72c9d46 58
wiesdat 3:611fd72c9d46 59 //lampjes
wiesdat 3:611fd72c9d46 60 DigitalOut rood(LED1);
wiesdat 3:611fd72c9d46 61 DigitalOut blauw(LED3);
wiesdat 3:611fd72c9d46 62 DigitalOut groen(LED2);
wiesdat 0:25a2e7ea29f3 63 AnalogIn emg1(PTB1);
wiesdat 0:25a2e7ea29f3 64 AnalogIn emg2(PTB2);
wiesdat 0:25a2e7ea29f3 65
wiesdat 0:25a2e7ea29f3 66 // PC communicatie
wiesdat 0:25a2e7ea29f3 67 MODSERIAL pc(USBTX,USBRX);
wiesdat 3:611fd72c9d46 68
wiesdat 0:25a2e7ea29f3 69 // Ticker voor de meetgegevens
wiesdat 0:25a2e7ea29f3 70 Ticker timer;
wiesdat 0:25a2e7ea29f3 71 volatile bool looptimerflag;
wiesdat 0:25a2e7ea29f3 72
wiesdat 0:25a2e7ea29f3 73 // Waardes voor de filters reserveren en als float vaststellen
wiesdat 3:611fd72c9d46 74 float emg_value2, ylp2, yhp2, yn2,ysum1 = 0, yave1=0;
wiesdat 3:611fd72c9d46 75 float emg_value1, ylp1, yhp1, yn1,ysum2 = 0, yave2=0 ;
wiesdat 3:611fd72c9d46 76
wiesdat 3:611fd72c9d46 77 //variabvelen voor positie motor1
wiesdat 4:c22f3095b130 78 float v1=0,out_i1 = 0; //out_i1 globaal gedef om reset
wiesdat 0:25a2e7ea29f3 79
wiesdat 0:25a2e7ea29f3 80 // 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 81 uint8_t y1, y2;
wiesdat 0:25a2e7ea29f3 82 int check = 0;
wiesdat 0:25a2e7ea29f3 83
wiesdat 0:25a2e7ea29f3 84 // 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 85 int badjestand, badjes=1,badjedone=0,speeding,armstand=0,armspeed=0;
wiesdat 0:25a2e7ea29f3 86 bool speeddone=0;
wiesdat 3:611fd72c9d46 87
wiesdat 0:25a2e7ea29f3 88 // Teller voor hoeveel metingen er zijn gedaan
wiesdat 0:25a2e7ea29f3 89 uint16_t teller=0;
wiesdat 0:25a2e7ea29f3 90
wiesdat 4:c22f3095b130 91
wiesdat 4:c22f3095b130 92 /*FUNCTIES
wiesdat 4:c22f3095b130 93 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 94 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 95 door de functies met 1, en signaal 2 wordt door de functie met 2, bewerkt.
wiesdat 4:c22f3095b130 96
wiesdat 4:c22f3095b130 97 GLOBAL FUNCTIONS
wiesdat 4:c22f3095b130 98 Clamp - "clampt" een waarde binnen grenzen, als de geclampte variabele over de grens heen gaat krijgt de variabele de grenswaarde.
wiesdat 4:c22f3095b130 99 setLooptimerflag - Hiermee wordt de ticker aangeroepen.
wiesdat 0:25a2e7ea29f3 100
wiesdat 4:c22f3095b130 101 EMG FUNCTIONS
wiesdat 4:c22f3095b130 102 readEMG - Leest het voltage af van de geselecteerde pin, in ons geval een EMG waarde.
wiesdat 4:c22f3095b130 103 (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 104 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 105 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 106
wiesdat 4:c22f3095b130 107 MOTORCONTROLFUNCTIONS
wiesdat 4:c22f3095b130 108 resetarm - Brengt arm terug naar de nul positie, zet de encoder op 0.
wiesdat 4:c22f3095b130 109 getv(delta_t) - Geeft de huidige draaisnelheid van motor1, delta_t is het tijdsinterval waarover gemeten wordt
wiesdat 4:c22f3095b130 110 badje - Vergelijkt met dubbele input, y1 en y2. Er zijn vier situaties, beide = 0, y1=1, y2=1, beide =1.
wiesdat 4:c22f3095b130 111 badposition - Leest de output van badje, laat het badje links, recht, niet draaien.
wiesdat 4:c22f3095b130 112 pid... - Pid regelaar, de k waardes wordenin de define sectie gedefineerd.
wiesdat 4:c22f3095b130 113 armposition - Op basis van de emgsignalen wordt hier een positie ingesteld.
wiesdat 4:c22f3095b130 114 armtopos - Regelt de arm naar de opgegeven positie toe.
wiesdat 4:c22f3095b130 115 */
wiesdat 5:36df561f3ac1 116 //HIDScope scope(5);
wiesdat 4:c22f3095b130 117 //GLOBAL FUNCTIOMS
wiesdat 5:36df561f3ac1 118 /*void viewer(){
wiesdat 5:36df561f3ac1 119 scope.set(0,yave1);
wiesdat 5:36df561f3ac1 120 scope.set(1,y1);
wiesdat 5:36df561f3ac1 121 scope.set(2,yave2);
wiesdat 5:36df561f3ac1 122 scope.set(3,y2);
wiesdat 5:36df561f3ac1 123 scope.send();
wiesdat 5:36df561f3ac1 124 }*/
wiesdat 5:36df561f3ac1 125
wiesdat 3:611fd72c9d46 126 void clamp(float * in, float min, float max)
wiesdat 0:25a2e7ea29f3 127 {
wiesdat 3:611fd72c9d46 128 *in > min ? *in < max? : *in = max: *in = min;
wiesdat 0:25a2e7ea29f3 129 }
wiesdat 0:25a2e7ea29f3 130
wiesdat 5:36df561f3ac1 131 void clampint(int * in, int min, int max)
wiesdat 5:36df561f3ac1 132 {
wiesdat 5:36df561f3ac1 133 *in > min ? *in < max? : *in = max: *in = min;
wiesdat 5:36df561f3ac1 134 }
wiesdat 5:36df561f3ac1 135
wiesdat 4:c22f3095b130 136 void setlooptimerflag(void)
wiesdat 4:c22f3095b130 137 {
wiesdat 4:c22f3095b130 138 looptimerflag = true;
wiesdat 4:c22f3095b130 139 }
wiesdat 3:611fd72c9d46 140
wiesdat 4:c22f3095b130 141 //EMG FUNCTIONS
wiesdat 0:25a2e7ea29f3 142 float readEMG1()
wiesdat 0:25a2e7ea29f3 143 {
wiesdat 0:25a2e7ea29f3 144 emg_value1=emg1.read();
wiesdat 0:25a2e7ea29f3 145 return emg_value1;
wiesdat 0:25a2e7ea29f3 146 }
wiesdat 0:25a2e7ea29f3 147
wiesdat 0:25a2e7ea29f3 148 float readEMG2()
wiesdat 0:25a2e7ea29f3 149 {
wiesdat 0:25a2e7ea29f3 150 emg_value2=emg2.read();
wiesdat 0:25a2e7ea29f3 151 return emg_value2;
wiesdat 0:25a2e7ea29f3 152 }
wiesdat 0:25a2e7ea29f3 153
wiesdat 0:25a2e7ea29f3 154 float notchfilter1(float ylp1)
wiesdat 0:25a2e7ea29f3 155 {
wiesdat 0:25a2e7ea29f3 156 static float yn1,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 0:25a2e7ea29f3 157 x = ylp1;
wiesdat 0:25a2e7ea29f3 158 yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 0:25a2e7ea29f3 159 x2 = x1;
wiesdat 0:25a2e7ea29f3 160 x1 = x;
wiesdat 0:25a2e7ea29f3 161 y2 = y1;
wiesdat 0:25a2e7ea29f3 162 y1 = yn1;
wiesdat 0:25a2e7ea29f3 163 return yn1;
wiesdat 0:25a2e7ea29f3 164 }
wiesdat 0:25a2e7ea29f3 165
wiesdat 0:25a2e7ea29f3 166 float notchfilter2(float ylp2)
wiesdat 0:25a2e7ea29f3 167 {
wiesdat 0:25a2e7ea29f3 168 static float x1=0,x2=0,y1=0,y2=0,x;
wiesdat 0:25a2e7ea29f3 169 x = ylp2;
wiesdat 0:25a2e7ea29f3 170 yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 0:25a2e7ea29f3 171 x2 = x1;
wiesdat 0:25a2e7ea29f3 172 x1 = x;
wiesdat 0:25a2e7ea29f3 173 y2 = y1;
wiesdat 0:25a2e7ea29f3 174 y1 = yn2;
wiesdat 0:25a2e7ea29f3 175 return yn2;
wiesdat 0:25a2e7ea29f3 176 }
wiesdat 0:25a2e7ea29f3 177
wiesdat 0:25a2e7ea29f3 178 float hpfilter1(float yn1)
wiesdat 0:25a2e7ea29f3 179 {
wiesdat 0:25a2e7ea29f3 180 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 181 x = yn1;
wiesdat 0:25a2e7ea29f3 182 yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:25a2e7ea29f3 183 x2 = x1;
wiesdat 0:25a2e7ea29f3 184 x1 = x;
wiesdat 0:25a2e7ea29f3 185 y2 = y1;
wiesdat 0:25a2e7ea29f3 186 y1 = yhp1;
wiesdat 0:25a2e7ea29f3 187 return yhp1;
wiesdat 0:25a2e7ea29f3 188 }
wiesdat 0:25a2e7ea29f3 189
wiesdat 0:25a2e7ea29f3 190 float hpfilter2(float yn2)
wiesdat 0:25a2e7ea29f3 191 {
wiesdat 0:25a2e7ea29f3 192 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 193 x = yn2;
wiesdat 0:25a2e7ea29f3 194 yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:25a2e7ea29f3 195 x2 = x1;
wiesdat 0:25a2e7ea29f3 196 x1 = x;
wiesdat 0:25a2e7ea29f3 197 y2 = y1;
wiesdat 0:25a2e7ea29f3 198 y1 = yhp2;
wiesdat 0:25a2e7ea29f3 199 return yhp2;
wiesdat 0:25a2e7ea29f3 200 }
wiesdat 0:25a2e7ea29f3 201
wiesdat 0:25a2e7ea29f3 202 float lpfilter1(float yhp1)
wiesdat 0:25a2e7ea29f3 203 {
wiesdat 0:25a2e7ea29f3 204 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 205 x = yhp1;
wiesdat 0:25a2e7ea29f3 206 ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 0:25a2e7ea29f3 207 x2 = x1;
wiesdat 0:25a2e7ea29f3 208 x1 = x;
wiesdat 0:25a2e7ea29f3 209 y2 = y1;
wiesdat 0:25a2e7ea29f3 210 y1 = ylp1;
wiesdat 0:25a2e7ea29f3 211 return ylp1;
wiesdat 0:25a2e7ea29f3 212 }
wiesdat 0:25a2e7ea29f3 213
wiesdat 0:25a2e7ea29f3 214 float lpfilter2(float yhp2)
wiesdat 0:25a2e7ea29f3 215 {
wiesdat 0:25a2e7ea29f3 216 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 217 x = yhp2;
wiesdat 0:25a2e7ea29f3 218 ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 0:25a2e7ea29f3 219 x2 = x1;
wiesdat 0:25a2e7ea29f3 220 x1 = x;
wiesdat 0:25a2e7ea29f3 221 y2 = y1;
wiesdat 0:25a2e7ea29f3 222 y1 = ylp2;
wiesdat 0:25a2e7ea29f3 223 return ylp2;
wiesdat 0:25a2e7ea29f3 224 }
wiesdat 0:25a2e7ea29f3 225
wiesdat 0:25a2e7ea29f3 226 float filter1(float emg_value1)
wiesdat 0:25a2e7ea29f3 227 {
wiesdat 0:25a2e7ea29f3 228 static uint16_t n;
wiesdat 0:25a2e7ea29f3 229 yn1 = notchfilter1(emg_value1);
wiesdat 0:25a2e7ea29f3 230 yhp1 = hpfilter1(yn1);
wiesdat 0:25a2e7ea29f3 231 ylp1 = lpfilter1(yhp1);
wiesdat 0:25a2e7ea29f3 232 ylp1 = fabs(ylp1);
wiesdat 0:25a2e7ea29f3 233 ysum1 = ysum1+ylp1;
wiesdat 0:25a2e7ea29f3 234 n++;
wiesdat 0:25a2e7ea29f3 235 if(n==500) {
wiesdat 0:25a2e7ea29f3 236 yave1 = ysum1/500;
wiesdat 0:25a2e7ea29f3 237 ysum1 = 0;
wiesdat 0:25a2e7ea29f3 238 n = 0;
wiesdat 0:25a2e7ea29f3 239 }
wiesdat 0:25a2e7ea29f3 240 return yave1;
wiesdat 0:25a2e7ea29f3 241 }
wiesdat 0:25a2e7ea29f3 242
wiesdat 0:25a2e7ea29f3 243 float filter2(float emg_value2)
wiesdat 0:25a2e7ea29f3 244 {
wiesdat 0:25a2e7ea29f3 245 static uint16_t n;
wiesdat 0:25a2e7ea29f3 246 yn2 = notchfilter2(emg_value2);
wiesdat 0:25a2e7ea29f3 247 yhp2 = hpfilter2(yn2);
wiesdat 0:25a2e7ea29f3 248 ylp2 = lpfilter2(yhp2);
wiesdat 0:25a2e7ea29f3 249 ylp2 = fabs(ylp2);
wiesdat 0:25a2e7ea29f3 250 ysum2 = ysum2 + ylp2;
wiesdat 0:25a2e7ea29f3 251 n++;
wiesdat 0:25a2e7ea29f3 252
wiesdat 5:36df561f3ac1 253 if(n==NOSAMPL) {
wiesdat 5:36df561f3ac1 254 yave2 = ysum2/NOSAMPL;
wiesdat 0:25a2e7ea29f3 255 ysum2 = 0;
wiesdat 0:25a2e7ea29f3 256 n = 0;
wiesdat 0:25a2e7ea29f3 257 }
wiesdat 0:25a2e7ea29f3 258 return yave2;
wiesdat 0:25a2e7ea29f3 259 }
wiesdat 0:25a2e7ea29f3 260
wiesdat 0:25a2e7ea29f3 261 float threshold1 (float yave1)
wiesdat 0:25a2e7ea29f3 262 {
wiesdat 0:25a2e7ea29f3 263 if(yave1>THRESHOLD) {
wiesdat 0:25a2e7ea29f3 264 y1 = 1;
wiesdat 0:25a2e7ea29f3 265 } else {
wiesdat 0:25a2e7ea29f3 266 y1 = 0;
wiesdat 0:25a2e7ea29f3 267 }
wiesdat 0:25a2e7ea29f3 268 return y1;
wiesdat 0:25a2e7ea29f3 269 }
wiesdat 0:25a2e7ea29f3 270
wiesdat 0:25a2e7ea29f3 271 float threshold2 (float yave2)
wiesdat 0:25a2e7ea29f3 272 {
wiesdat 0:25a2e7ea29f3 273 if(yave2>THRESHOLD) {
wiesdat 0:25a2e7ea29f3 274 y2 = 1;
wiesdat 0:25a2e7ea29f3 275 } else {
wiesdat 0:25a2e7ea29f3 276 y2 = 0;
wiesdat 0:25a2e7ea29f3 277 }
wiesdat 0:25a2e7ea29f3 278 return y2;
wiesdat 0:25a2e7ea29f3 279 }
wiesdat 0:25a2e7ea29f3 280
wiesdat 4:c22f3095b130 281 //MOTORCONTROL FUNCTIONS
wiesdat 4:c22f3095b130 282 float getv(float delta_t)
wiesdat 0:25a2e7ea29f3 283 {
wiesdat 4:c22f3095b130 284 int enca1=0,enca2=0,counts=0;
wiesdat 3:611fd72c9d46 285 float v;
wiesdat 2:f3e8a27d376c 286 int n =0 ;
wiesdat 2:f3e8a27d376c 287 while(n<3) {
wiesdat 2:f3e8a27d376c 288 wait(delta_t);
wiesdat 2:f3e8a27d376c 289 enca2 = enca1;
wiesdat 4:c22f3095b130 290 enca1 = encoder1.getPosition();
wiesdat 2:f3e8a27d376c 291 n++;
wiesdat 2:f3e8a27d376c 292 }
wiesdat 2:f3e8a27d376c 293 counts = (enca1 - enca2)/delta_t;
wiesdat 4:c22f3095b130 294 v = (counts)*((2*3.14159265359)/1550)*0.5;
wiesdat 2:f3e8a27d376c 295 return v;
wiesdat 2:f3e8a27d376c 296 }
wiesdat 4:c22f3095b130 297
wiesdat 4:c22f3095b130 298 float resetarm()
wiesdat 3:611fd72c9d46 299 {
wiesdat 3:611fd72c9d46 300 v1 = 1;
wiesdat 3:611fd72c9d46 301 while(v1 !=0) {
wiesdat 3:611fd72c9d46 302 dir1 = 0;
wiesdat 3:611fd72c9d46 303 pwm1.write(0.1);
wiesdat 4:c22f3095b130 304 v1 =getv(0.1);
wiesdat 3:611fd72c9d46 305 }
wiesdat 3:611fd72c9d46 306 pwm1 = 0;
wiesdat 3:611fd72c9d46 307 dir1 =1;
wiesdat 3:611fd72c9d46 308 encoder1.setPosition(0);
wiesdat 3:611fd72c9d46 309 return pwm1;
wiesdat 3:611fd72c9d46 310 }
wiesdat 3:611fd72c9d46 311 uint8_t badje (uint8_t y1, uint8_t y2)
wiesdat 3:611fd72c9d46 312 {
wiesdat 3:611fd72c9d46 313 if (y1>0 && y2>0 && check>0) {
wiesdat 3:611fd72c9d46 314 badjedone=1;
wiesdat 3:611fd72c9d46 315 check=0;
wiesdat 3:611fd72c9d46 316 rood = 1;
wiesdat 3:611fd72c9d46 317 groen = 0;
wiesdat 3:611fd72c9d46 318 cout<<"ga naar mode 2"<<endl;
wiesdat 5:36df561f3ac1 319 wait(0.2);
wiesdat 3:611fd72c9d46 320 } else if (y1>0 && y2>0) {
wiesdat 3:611fd72c9d46 321 check=1;
wiesdat 3:611fd72c9d46 322 } else if (y1>0) {
wiesdat 3:611fd72c9d46 323 badjes=0;
wiesdat 3:611fd72c9d46 324 check=0;
wiesdat 3:611fd72c9d46 325 } else if (y2>0 ) {
wiesdat 3:611fd72c9d46 326 badjes=1;
wiesdat 3:611fd72c9d46 327 check=0;
wiesdat 4:c22f3095b130 328 } else {
wiesdat 3:611fd72c9d46 329 check=0;
wiesdat 3:611fd72c9d46 330 badjes = 2;
wiesdat 3:611fd72c9d46 331 }
wiesdat 3:611fd72c9d46 332 return badjes;
wiesdat 3:611fd72c9d46 333 }
wiesdat 3:611fd72c9d46 334
wiesdat 4:c22f3095b130 335 void batposition(int y)
wiesdat 4:c22f3095b130 336 {
wiesdat 4:c22f3095b130 337 switch(y) {
wiesdat 4:c22f3095b130 338 case 0:
wiesdat 4:c22f3095b130 339
wiesdat 4:c22f3095b130 340 dir = 1;
wiesdat 4:c22f3095b130 341 pwm.write(0.4);
wiesdat 4:c22f3095b130 342 wait(0.04);
wiesdat 4:c22f3095b130 343 pwm.write(0);
wiesdat 5:36df561f3ac1 344 cout<<"links"<<endl;
wiesdat 4:c22f3095b130 345 break;
wiesdat 4:c22f3095b130 346 case 1:
wiesdat 4:c22f3095b130 347 dir = 0;
wiesdat 4:c22f3095b130 348 pwm.write(0.4);
wiesdat 4:c22f3095b130 349 wait(0.04);
wiesdat 4:c22f3095b130 350 pwm.write(0);
wiesdat 5:36df561f3ac1 351 cout<<"rechts"<<endl;
wiesdat 4:c22f3095b130 352 break;
wiesdat 4:c22f3095b130 353 case 2:
wiesdat 4:c22f3095b130 354 pwm.write(0);
wiesdat 4:c22f3095b130 355 break;
wiesdat 4:c22f3095b130 356
wiesdat 4:c22f3095b130 357 }
wiesdat 4:c22f3095b130 358
wiesdat 4:c22f3095b130 359 }
wiesdat 4:c22f3095b130 360
wiesdat 4:c22f3095b130 361 float pidposition(float setpoint, float measurement)
wiesdat 3:611fd72c9d46 362 {
wiesdat 4:c22f3095b130 363 float error, out, out_p=0,out_d=0;
wiesdat 4:c22f3095b130 364 static float prev_error = 0;
wiesdat 4:c22f3095b130 365 error = setpoint-measurement;
wiesdat 4:c22f3095b130 366 out_p = error*K_P1;
wiesdat 4:c22f3095b130 367 out_i1 += error*K_I1;
wiesdat 4:c22f3095b130 368 out_d = (error-prev_error)*K_D1;
wiesdat 4:c22f3095b130 369 clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
wiesdat 4:c22f3095b130 370 prev_error = error;
wiesdat 4:c22f3095b130 371 out = out_i1+out_p+out_d;
wiesdat 5:36df561f3ac1 372 cout<<"out: "<<out<<endl;
wiesdat 4:c22f3095b130 373 return out;
wiesdat 4:c22f3095b130 374 }
wiesdat 4:c22f3095b130 375
wiesdat 4:c22f3095b130 376
wiesdat 4:c22f3095b130 377
wiesdat 5:36df561f3ac1 378
wiesdat 4:c22f3095b130 379 uint8_t armposition (uint8_t y1, uint8_t y2)
wiesdat 4:c22f3095b130 380 {
wiesdat 4:c22f3095b130 381 static int stand=0;
wiesdat 4:c22f3095b130 382 if (y1>0 && y2>0 && check>0) {
wiesdat 5:36df561f3ac1 383 badjedone=2;
wiesdat 4:c22f3095b130 384 check=0;
wiesdat 4:c22f3095b130 385 rood = 0;
wiesdat 4:c22f3095b130 386 groen = 1;
wiesdat 4:c22f3095b130 387 cout<<"ga naar mode 1"<<endl;
wiesdat 4:c22f3095b130 388 } else if (y1>0 && y2>0) {
wiesdat 4:c22f3095b130 389 check=1;
wiesdat 5:36df561f3ac1 390 cout<<"zo naar mode 1"<<endl;
wiesdat 4:c22f3095b130 391 } else if (y1>0) {
wiesdat 4:c22f3095b130 392 stand=stand+1;
wiesdat 4:c22f3095b130 393 check=0;
wiesdat 4:c22f3095b130 394 cout<<"stand "<<stand<<endl;
wiesdat 4:c22f3095b130 395 } else if (y2>0 ) {
wiesdat 4:c22f3095b130 396 stand=stand-1;
wiesdat 4:c22f3095b130 397 check=0;
wiesdat 4:c22f3095b130 398 cout<<"stand "<<stand<<endl;
wiesdat 4:c22f3095b130 399 } else {
wiesdat 5:36df561f3ac1 400
wiesdat 4:c22f3095b130 401 check =0;
wiesdat 4:c22f3095b130 402 }
wiesdat 6:b69b9597d4fc 403 clampint(&stand,1,3);
wiesdat 4:c22f3095b130 404 return stand;
wiesdat 4:c22f3095b130 405 }
wiesdat 4:c22f3095b130 406
wiesdat 6:b69b9597d4fc 407 float gotopos(int pos)
wiesdat 4:c22f3095b130 408 {
wiesdat 6:b69b9597d4fc 409 float out1;
wiesdat 6:b69b9597d4fc 410
wiesdat 6:b69b9597d4fc 411 switch(pos) {
wiesdat 2:f3e8a27d376c 412 case 1:
wiesdat 6:b69b9597d4fc 413 pos = 100;
wiesdat 2:f3e8a27d376c 414 break;
wiesdat 2:f3e8a27d376c 415
wiesdat 2:f3e8a27d376c 416 case 2:
wiesdat 6:b69b9597d4fc 417 pos = 200;
wiesdat 2:f3e8a27d376c 418 break;
wiesdat 2:f3e8a27d376c 419
wiesdat 2:f3e8a27d376c 420 case 3:
wiesdat 6:b69b9597d4fc 421 pos = 250;
wiesdat 2:f3e8a27d376c 422 break;
wiesdat 2:f3e8a27d376c 423 }
wiesdat 6:b69b9597d4fc 424
wiesdat 6:b69b9597d4fc 425 while((abs(pos-encoder1.getPosition()) >6)|| (v1!= 0)) {
wiesdat 2:f3e8a27d376c 426
wiesdat 2:f3e8a27d376c 427 while(!looptimerflag);
wiesdat 2:f3e8a27d376c 428 looptimerflag = false;
wiesdat 6:b69b9597d4fc 429 out1 = pidposition(pos,encoder1.getPosition());
wiesdat 2:f3e8a27d376c 430
wiesdat 6:b69b9597d4fc 431 if(out1>0) {
wiesdat 2:f3e8a27d376c 432 dir1 = 1;
wiesdat 2:f3e8a27d376c 433
wiesdat 6:b69b9597d4fc 434 } else if(out1<0) {
wiesdat 2:f3e8a27d376c 435 dir1 = 0;
wiesdat 2:f3e8a27d376c 436 }
wiesdat 6:b69b9597d4fc 437 pwm1 = fabs(out1);
wiesdat 4:c22f3095b130 438 v1 = getv(0.001);
wiesdat 2:f3e8a27d376c 439 }
wiesdat 2:f3e8a27d376c 440 pwm1 =0;
wiesdat 2:f3e8a27d376c 441
wiesdat 6:b69b9597d4fc 442 return pwm1;
wiesdat 2:f3e8a27d376c 443 }
wiesdat 2:f3e8a27d376c 444
wiesdat 4:c22f3095b130 445 // MAIN SCRIPT
wiesdat 0:25a2e7ea29f3 446 int main()
wiesdat 0:25a2e7ea29f3 447 {
wiesdat 1:a010e434a360 448 rood = 0;
wiesdat 1:a010e434a360 449 blauw = 1;
wiesdat 1:a010e434a360 450 groen = 1;
wiesdat 4:c22f3095b130 451 resetarm();
wiesdat 0:25a2e7ea29f3 452 pc.baud(115200);
wiesdat 0:25a2e7ea29f3 453 timer.attach(setlooptimerflag,TSAMP);
wiesdat 0:25a2e7ea29f3 454 wait(2);
wiesdat 5:36df561f3ac1 455 cout<<"Begin programma"<<endl;
wiesdat 0:25a2e7ea29f3 456 while(1) {
wiesdat 4:c22f3095b130 457 //Per TSAMP word EMG uitgelzen, gefilterd en gemiddeld
wiesdat 0:25a2e7ea29f3 458 while(!looptimerflag);
wiesdat 0:25a2e7ea29f3 459 looptimerflag = false;
wiesdat 0:25a2e7ea29f3 460 emg_value1 = readEMG1();
wiesdat 0:25a2e7ea29f3 461 emg_value2 = readEMG2();
wiesdat 0:25a2e7ea29f3 462 yave1 = filter1(emg_value1);
wiesdat 0:25a2e7ea29f3 463 yave2 = filter2(emg_value2);
wiesdat 0:25a2e7ea29f3 464 y1=threshold1(yave1);
wiesdat 0:25a2e7ea29f3 465 y2=threshold2(yave2);
wiesdat 0:25a2e7ea29f3 466 teller++;
wiesdat 0:25a2e7ea29f3 467
wiesdat 4:c22f3095b130 468 /*Dit gedeelte voert de bewegingen uit. In de eerste fase kan met de linker spier het batje naar links gedraaid worden,
wiesdat 4:c22f3095b130 469 en met de rechter spier het batje naar recht. Als het badje in de juiste positie staat kunnen beide spieren tegelijk
wiesdat 4:c22f3095b130 470 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 471 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 472 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 473 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 474 fase 1.*/
wiesdat 0:25a2e7ea29f3 475
wiesdat 5:36df561f3ac1 476 if (teller==NOSAMPL) {
wiesdat 0:25a2e7ea29f3 477 teller=0;
wiesdat 5:36df561f3ac1 478 switch(badjedone) {
wiesdat 5:36df561f3ac1 479 case 0:
wiesdat 5:36df561f3ac1 480 cout<<"fase1"<<endl;
wiesdat 5:36df561f3ac1 481 badjestand=badje(y1,y2);
wiesdat 5:36df561f3ac1 482 batposition(badjestand);
wiesdat 5:36df561f3ac1 483 break;
wiesdat 5:36df561f3ac1 484
wiesdat 5:36df561f3ac1 485 case 1:
wiesdat 5:36df561f3ac1 486 cout<<"fase2"<<endl;
wiesdat 4:c22f3095b130 487 armstand=armposition(y1,y2);
wiesdat 6:b69b9597d4fc 488 gotopos(armstand);
wiesdat 3:611fd72c9d46 489 cout<<"armstand "<<armstand<<endl;
wiesdat 5:36df561f3ac1 490
wiesdat 5:36df561f3ac1 491 //armtopos(armstand);
wiesdat 5:36df561f3ac1 492 //cout<<"badjedont: "<<badjedone<<endl;
wiesdat 5:36df561f3ac1 493 break;
wiesdat 5:36df561f3ac1 494
wiesdat 5:36df561f3ac1 495 case 2:
wiesdat 5:36df561f3ac1 496 cout<<"terug"<<endl;
wiesdat 5:36df561f3ac1 497 badjedone=0;
wiesdat 5:36df561f3ac1 498 break;
wiesdat 5:36df561f3ac1 499 default:
wiesdat 5:36df561f3ac1 500 break;
wiesdat 0:25a2e7ea29f3 501 }
wiesdat 5:36df561f3ac1 502
wiesdat 0:25a2e7ea29f3 503 }
wiesdat 0:25a2e7ea29f3 504 }
wiesdat 5:36df561f3ac1 505
wiesdat 5:36df561f3ac1 506 }
wiesdat 0:25a2e7ea29f3 507
wiesdat 5:36df561f3ac1 508