thijs ruikes / Mbed 2 deprecated PROJECT-ROBOTCONTROL

Robotcontrol groep 2

Dependencies:   Encoder MODSERIAL mbed HIDScope

main.cpp@2:f3e8a27d376c, 2014-10-31 (annotated)

Committer:
wiesdat
Date:
Fri Oct 31 13:50:14 2014 +0000
Revision:
2:f3e8a27d376c
Parent:
1:a010e434a360
Child:
3:611fd72c9d46
beide motoren

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wiesdat 0:25a2e7ea29f3 1 #include "mbed.h"
wiesdat 0:25a2e7ea29f3 2 #include "MODSERIAL.h"
wiesdat 0:25a2e7ea29f3 3 #include "HIDScope.h"
wiesdat 1:a010e434a360 4
wiesdat 0:25a2e7ea29f3 5 #include "encoder.h"
wiesdat 0:25a2e7ea29f3 6 #define K_P (0.1)
wiesdat 0:25a2e7ea29f3 7 #define K_I (0.1)
wiesdat 0:25a2e7ea29f3 8 #define K_D (0.0005 /TSAMP)
wiesdat 0:25a2e7ea29f3 9 #define TSAMP 0.001
wiesdat 0:25a2e7ea29f3 10 #define I_LIMIT 1.
wiesdat 0:25a2e7ea29f3 11
wiesdat 0:25a2e7ea29f3 12 // Outputs van de motor
wiesdat 0:25a2e7ea29f3 13 Encoder encoderA(PTD5,PTA13);
wiesdat 2:f3e8a27d376c 14 Encoder encoder1(PTD0,PTD2);
wiesdat 2:f3e8a27d376c 15
wiesdat 0:25a2e7ea29f3 16
wiesdat 0:25a2e7ea29f3 17 // Outputs naar de motor
wiesdat 0:25a2e7ea29f3 18 PwmOut pwm(PTC8);
wiesdat 0:25a2e7ea29f3 19 DigitalOut dir(PTC9);
wiesdat 2:f3e8a27d376c 20 PwmOut pwm1(PTA5);
wiesdat 2:f3e8a27d376c 21 DigitalOut dir1(PTA4);
wiesdat 0:25a2e7ea29f3 22
wiesdat 0:25a2e7ea29f3 23 // Vaststellen van het datatype van de outputs naar de motor
wiesdat 0:25a2e7ea29f3 24 float s_speed1;
wiesdat 0:25a2e7ea29f3 25 float s_dir1;
wiesdat 0:25a2e7ea29f3 26 float s_speed2;
wiesdat 0:25a2e7ea29f3 27 float s_dir2;
wiesdat 0:25a2e7ea29f3 28
wiesdat 1:a010e434a360 29 //lampjes
wiesdat 1:a010e434a360 30
wiesdat 1:a010e434a360 31 DigitalOut rood(LED1);
wiesdat 1:a010e434a360 32 DigitalOut blauw(LED3);
wiesdat 1:a010e434a360 33 DigitalOut groen(LED2);
wiesdat 0:25a2e7ea29f3 34 // Alle constantes voor de filters definiëren
wiesdat 0:25a2e7ea29f3 35 // Constantes voor de Low Pass filter
wiesdat 0:25a2e7ea29f3 36 #define A1LP 0.018180963222803
wiesdat 0:25a2e7ea29f3 37 #define A0LP 0.016544013176248
wiesdat 0:25a2e7ea29f3 38 #define B1LP -1.718913340044714
wiesdat 0:25a2e7ea29f3 39 #define B0LP 0.753638316443765
wiesdat 0:25a2e7ea29f3 40
wiesdat 0:25a2e7ea29f3 41 // Constantes voor de High Pass Filter
wiesdat 0:25a2e7ea29f3 42 #define A1HP -1.999801878951505
wiesdat 0:25a2e7ea29f3 43 #define A0HP 0.999801878951505
wiesdat 0:25a2e7ea29f3 44 #define B1HP -1.971717601075000
wiesdat 0:25a2e7ea29f3 45 #define B0HP 0.972111984032897
wiesdat 0:25a2e7ea29f3 46
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 0:25a2e7ea29f3 56
wiesdat 0:25a2e7ea29f3 57 // EMG Threshold waarde
wiesdat 0:25a2e7ea29f3 58 #define THRESHOLD 0.05
wiesdat 0:25a2e7ea29f3 59
wiesdat 0:25a2e7ea29f3 60 // EMG inputs
wiesdat 0:25a2e7ea29f3 61 AnalogIn emg1(PTB1);
wiesdat 0:25a2e7ea29f3 62 AnalogIn emg2(PTB2);
wiesdat 0:25a2e7ea29f3 63
wiesdat 0:25a2e7ea29f3 64 // PC communicatie
wiesdat 0:25a2e7ea29f3 65 MODSERIAL pc(USBTX,USBRX);
wiesdat 0:25a2e7ea29f3 66 HIDScope scope(5);
wiesdat 0:25a2e7ea29f3 67 int set;
wiesdat 0:25a2e7ea29f3 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 0:25a2e7ea29f3 74 float emg_value2, ylp2, yhp2, yn2;
wiesdat 0:25a2e7ea29f3 75 float emg_value1, ylp1, yhp1, yn1;
wiesdat 0:25a2e7ea29f3 76
wiesdat 0:25a2e7ea29f3 77 float ysum1 = 0, yave1=0 ;
wiesdat 0:25a2e7ea29f3 78 float ysum2 = 0, yave2=0 ;
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 0:25a2e7ea29f3 85 int badjestand;
wiesdat 0:25a2e7ea29f3 86 int badjes=1;
wiesdat 0:25a2e7ea29f3 87 int badjedone=0;
wiesdat 0:25a2e7ea29f3 88 int speeding;
wiesdat 0:25a2e7ea29f3 89 int armstand=0;
wiesdat 0:25a2e7ea29f3 90 int armspeed=0;
wiesdat 0:25a2e7ea29f3 91 bool speeddone=0;
wiesdat 2:f3e8a27d376c 92 int a; //wordt gebruikt in gotopos
wiesdat 2:f3e8a27d376c 93 float out1; //pid voor armpositie
wiesdat 0:25a2e7ea29f3 94 // Teller voor hoeveel metingen er zijn gedaan
wiesdat 0:25a2e7ea29f3 95 uint16_t teller=0;
wiesdat 0:25a2e7ea29f3 96
wiesdat 0:25a2e7ea29f3 97 // Random meuk die ik uit batjespositie haal, nog ff opruimen
wiesdat 0:25a2e7ea29f3 98 int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
wiesdat 0:25a2e7ea29f3 99 float speed = 0.1, out =0;
wiesdat 0:25a2e7ea29f3 100 int pos =0,zero =0, fout;
wiesdat 0:25a2e7ea29f3 101 float v=0;
wiesdat 0:25a2e7ea29f3 102 float out_i = 0;
wiesdat 0:25a2e7ea29f3 103 int y;
wiesdat 0:25a2e7ea29f3 104
wiesdat 0:25a2e7ea29f3 105
wiesdat 0:25a2e7ea29f3 106 // Scopes vaststellen
wiesdat 0:25a2e7ea29f3 107 void viewer()
wiesdat 0:25a2e7ea29f3 108 {
wiesdat 0:25a2e7ea29f3 109 scope.set(0,y1);
wiesdat 0:25a2e7ea29f3 110 scope.set(1,y2);
wiesdat 0:25a2e7ea29f3 111 scope.set(2,badjes);
wiesdat 0:25a2e7ea29f3 112 scope.set(3,armstand);
wiesdat 0:25a2e7ea29f3 113 scope.set(4,speeddone);
wiesdat 0:25a2e7ea29f3 114 scope.send();
wiesdat 0:25a2e7ea29f3 115 }
wiesdat 0:25a2e7ea29f3 116
wiesdat 0:25a2e7ea29f3 117 // EMG 1 uitlezen
wiesdat 0:25a2e7ea29f3 118 float readEMG1()
wiesdat 0:25a2e7ea29f3 119 {
wiesdat 0:25a2e7ea29f3 120
wiesdat 0:25a2e7ea29f3 121 emg_value1=emg1.read();
wiesdat 0:25a2e7ea29f3 122 return emg_value1;
wiesdat 0:25a2e7ea29f3 123 }
wiesdat 0:25a2e7ea29f3 124
wiesdat 0:25a2e7ea29f3 125 // EMG 2 uitlezen
wiesdat 0:25a2e7ea29f3 126 float readEMG2()
wiesdat 0:25a2e7ea29f3 127 {
wiesdat 0:25a2e7ea29f3 128
wiesdat 0:25a2e7ea29f3 129 emg_value2=emg2.read();
wiesdat 0:25a2e7ea29f3 130 return emg_value2;
wiesdat 0:25a2e7ea29f3 131 }
wiesdat 0:25a2e7ea29f3 132
wiesdat 0:25a2e7ea29f3 133 // Notch Filter voor EMG 1
wiesdat 0:25a2e7ea29f3 134 float notchfilter1(float ylp1)
wiesdat 0:25a2e7ea29f3 135 {
wiesdat 0:25a2e7ea29f3 136 static float yn1,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 0:25a2e7ea29f3 137 x = ylp1;
wiesdat 0:25a2e7ea29f3 138 yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 0:25a2e7ea29f3 139 x2 = x1;
wiesdat 0:25a2e7ea29f3 140 x1 = x;
wiesdat 0:25a2e7ea29f3 141 y2 = y1;
wiesdat 0:25a2e7ea29f3 142 y1 = yn1;
wiesdat 0:25a2e7ea29f3 143 return yn1;
wiesdat 0:25a2e7ea29f3 144 }
wiesdat 0:25a2e7ea29f3 145
wiesdat 0:25a2e7ea29f3 146 // Notch Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
wiesdat 0:25a2e7ea29f3 147 float notchfilter2(float ylp2)
wiesdat 0:25a2e7ea29f3 148 {
wiesdat 0:25a2e7ea29f3 149 static float x1=0,x2=0,y1=0,y2=0,x;
wiesdat 0:25a2e7ea29f3 150 x = ylp2;
wiesdat 0:25a2e7ea29f3 151 yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 0:25a2e7ea29f3 152 x2 = x1;
wiesdat 0:25a2e7ea29f3 153 x1 = x;
wiesdat 0:25a2e7ea29f3 154 y2 = y1;
wiesdat 0:25a2e7ea29f3 155 y1 = yn2;
wiesdat 0:25a2e7ea29f3 156 return yn2;
wiesdat 0:25a2e7ea29f3 157 }
wiesdat 0:25a2e7ea29f3 158
wiesdat 0:25a2e7ea29f3 159 // High Pass Filter voor EMG 1
wiesdat 0:25a2e7ea29f3 160 float hpfilter1(float yn1)
wiesdat 0:25a2e7ea29f3 161 {
wiesdat 0:25a2e7ea29f3 162 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 163 x = yn1;
wiesdat 0:25a2e7ea29f3 164 yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:25a2e7ea29f3 165 x2 = x1;
wiesdat 0:25a2e7ea29f3 166 x1 = x;
wiesdat 0:25a2e7ea29f3 167 y2 = y1;
wiesdat 0:25a2e7ea29f3 168 y1 = yhp1;
wiesdat 0:25a2e7ea29f3 169 return yhp1;
wiesdat 0:25a2e7ea29f3 170 }
wiesdat 0:25a2e7ea29f3 171
wiesdat 0:25a2e7ea29f3 172 // High Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
wiesdat 0:25a2e7ea29f3 173 float hpfilter2(float yn2)
wiesdat 0:25a2e7ea29f3 174 {
wiesdat 0:25a2e7ea29f3 175 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 176 x = yn2;
wiesdat 0:25a2e7ea29f3 177 yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:25a2e7ea29f3 178 x2 = x1;
wiesdat 0:25a2e7ea29f3 179 x1 = x;
wiesdat 0:25a2e7ea29f3 180 y2 = y1;
wiesdat 0:25a2e7ea29f3 181 y1 = yhp2;
wiesdat 0:25a2e7ea29f3 182 return yhp2;
wiesdat 0:25a2e7ea29f3 183 }
wiesdat 0:25a2e7ea29f3 184
wiesdat 0:25a2e7ea29f3 185 // Low Pass Filter voor EMG 1
wiesdat 0:25a2e7ea29f3 186 float lpfilter1(float yhp1)
wiesdat 0:25a2e7ea29f3 187 {
wiesdat 0:25a2e7ea29f3 188 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 189 x = yhp1;
wiesdat 0:25a2e7ea29f3 190 ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 0:25a2e7ea29f3 191 x2 = x1;
wiesdat 0:25a2e7ea29f3 192 x1 = x;
wiesdat 0:25a2e7ea29f3 193 y2 = y1;
wiesdat 0:25a2e7ea29f3 194 y1 = ylp1;
wiesdat 0:25a2e7ea29f3 195 return ylp1;
wiesdat 0:25a2e7ea29f3 196 }
wiesdat 0:25a2e7ea29f3 197
wiesdat 0:25a2e7ea29f3 198 // Low Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
wiesdat 0:25a2e7ea29f3 199 float lpfilter2(float yhp2)
wiesdat 0:25a2e7ea29f3 200 {
wiesdat 0:25a2e7ea29f3 201 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 202 x = yhp2;
wiesdat 0:25a2e7ea29f3 203 ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 0:25a2e7ea29f3 204 x2 = x1;
wiesdat 0:25a2e7ea29f3 205 x1 = x;
wiesdat 0:25a2e7ea29f3 206 y2 = y1;
wiesdat 0:25a2e7ea29f3 207 y1 = ylp2;
wiesdat 0:25a2e7ea29f3 208 return ylp2;
wiesdat 0:25a2e7ea29f3 209 }
wiesdat 0:25a2e7ea29f3 210
wiesdat 0:25a2e7ea29f3 211 // Stukje nodig voor de ticker
wiesdat 0:25a2e7ea29f3 212 void setlooptimerflag(void)
wiesdat 0:25a2e7ea29f3 213 {
wiesdat 0:25a2e7ea29f3 214 looptimerflag = true;
wiesdat 0:25a2e7ea29f3 215 }
wiesdat 0:25a2e7ea29f3 216
wiesdat 0:25a2e7ea29f3 217 // Combinatie van de drie bovengenoemde filters voor EMG 1, en de middeling van 500 dataputen (nieuwe data 2 keer per seconde)
wiesdat 0:25a2e7ea29f3 218 float filter1(float emg_value1)
wiesdat 0:25a2e7ea29f3 219 {
wiesdat 0:25a2e7ea29f3 220 static uint16_t n;
wiesdat 0:25a2e7ea29f3 221 yn1 = notchfilter1(emg_value1);
wiesdat 0:25a2e7ea29f3 222 yhp1 = hpfilter1(yn1);
wiesdat 0:25a2e7ea29f3 223 ylp1 = lpfilter1(yhp1);
wiesdat 0:25a2e7ea29f3 224 ylp1 = fabs(ylp1);
wiesdat 0:25a2e7ea29f3 225 ysum1 = ysum1+ylp1;
wiesdat 0:25a2e7ea29f3 226 n++;
wiesdat 0:25a2e7ea29f3 227 if(n==500) {
wiesdat 0:25a2e7ea29f3 228 yave1 = ysum1/500;
wiesdat 0:25a2e7ea29f3 229 ysum1 = 0;
wiesdat 0:25a2e7ea29f3 230 n = 0;
wiesdat 0:25a2e7ea29f3 231 }
wiesdat 0:25a2e7ea29f3 232 return yave1;
wiesdat 0:25a2e7ea29f3 233 }
wiesdat 0:25a2e7ea29f3 234
wiesdat 0:25a2e7ea29f3 235 // Idem voor EMG 2
wiesdat 0:25a2e7ea29f3 236 float filter2(float emg_value2)
wiesdat 0:25a2e7ea29f3 237 {
wiesdat 0:25a2e7ea29f3 238 static uint16_t n;
wiesdat 0:25a2e7ea29f3 239 yn2 = notchfilter2(emg_value2);
wiesdat 0:25a2e7ea29f3 240 yhp2 = hpfilter2(yn2);
wiesdat 0:25a2e7ea29f3 241 ylp2 = lpfilter2(yhp2);
wiesdat 0:25a2e7ea29f3 242 ylp2 = fabs(ylp2);
wiesdat 0:25a2e7ea29f3 243 ysum2 = ysum2 + ylp2;
wiesdat 0:25a2e7ea29f3 244 n++;
wiesdat 0:25a2e7ea29f3 245
wiesdat 0:25a2e7ea29f3 246 if(n==500) {
wiesdat 0:25a2e7ea29f3 247 yave2 = ysum2/500;
wiesdat 0:25a2e7ea29f3 248 ysum2 = 0;
wiesdat 0:25a2e7ea29f3 249 n = 0;
wiesdat 0:25a2e7ea29f3 250 }
wiesdat 0:25a2e7ea29f3 251 return yave2;
wiesdat 0:25a2e7ea29f3 252 }
wiesdat 0:25a2e7ea29f3 253
wiesdat 0:25a2e7ea29f3 254 // Check of de waarde van het EMG boven de gedefinieerde threshold waarde uitkomt voor EMG 1
wiesdat 0:25a2e7ea29f3 255 float threshold1 (float yave1)
wiesdat 0:25a2e7ea29f3 256 {
wiesdat 0:25a2e7ea29f3 257 if(yave1>THRESHOLD) {
wiesdat 0:25a2e7ea29f3 258 y1 = 1;
wiesdat 0:25a2e7ea29f3 259 } else {
wiesdat 0:25a2e7ea29f3 260 y1 = 0;
wiesdat 0:25a2e7ea29f3 261 }
wiesdat 0:25a2e7ea29f3 262 return y1;
wiesdat 0:25a2e7ea29f3 263 }
wiesdat 0:25a2e7ea29f3 264
wiesdat 0:25a2e7ea29f3 265 //Idem voor EMG 2
wiesdat 0:25a2e7ea29f3 266 float threshold2 (float yave2)
wiesdat 0:25a2e7ea29f3 267 {
wiesdat 0:25a2e7ea29f3 268 if(yave2>THRESHOLD) {
wiesdat 0:25a2e7ea29f3 269 y2 = 1;
wiesdat 0:25a2e7ea29f3 270 } else {
wiesdat 0:25a2e7ea29f3 271 y2 = 0;
wiesdat 0:25a2e7ea29f3 272 }
wiesdat 0:25a2e7ea29f3 273 return y2;
wiesdat 0:25a2e7ea29f3 274 }
wiesdat 0:25a2e7ea29f3 275
wiesdat 0:25a2e7ea29f3 276 // Hoek van het badje vaststelen: begint met waarde 1, als EMG 1 alleen gebruikt wordt, gaat er 1 vanaf (maximaal 0), wordt EMG 2 alleen gebruikt, dan gaat er 1 bij (maximaal 2).
wiesdat 0:25a2e7ea29f3 277 // Als beide tegelijkertijd 2 maal achter elkaar gebruikt worden, dan wordt de hoek vastgezet (kan niet meer verandert worden tot de reset).
wiesdat 0:25a2e7ea29f3 278 uint8_t badje (uint8_t y1, uint8_t y2)
wiesdat 0:25a2e7ea29f3 279 {
wiesdat 0:25a2e7ea29f3 280 if (y1>0 && y2>0 && check>0) {
wiesdat 0:25a2e7ea29f3 281 badjedone=1;
wiesdat 0:25a2e7ea29f3 282 check=0;
wiesdat 1:a010e434a360 283 rood = 1;
wiesdat 1:a010e434a360 284 groen = 0;
wiesdat 0:25a2e7ea29f3 285 } else if (y1>0 && y2>0) {
wiesdat 0:25a2e7ea29f3 286 check=1;
wiesdat 0:25a2e7ea29f3 287 } else if (y1>0) {
wiesdat 0:25a2e7ea29f3 288 badjes=0;
wiesdat 0:25a2e7ea29f3 289 check=0;
wiesdat 0:25a2e7ea29f3 290 } else if (y2>0 ) {
wiesdat 0:25a2e7ea29f3 291 badjes=1;
wiesdat 0:25a2e7ea29f3 292 check=0;
wiesdat 0:25a2e7ea29f3 293 }
wiesdat 0:25a2e7ea29f3 294 else {
wiesdat 0:25a2e7ea29f3 295 check=0;
wiesdat 0:25a2e7ea29f3 296 badjes = 2;
wiesdat 0:25a2e7ea29f3 297 }
wiesdat 0:25a2e7ea29f3 298 return badjes;
wiesdat 0:25a2e7ea29f3 299 }
wiesdat 0:25a2e7ea29f3 300
wiesdat 0:25a2e7ea29f3 301 // Stukje dat hetzelfde als bij de hoek van het badje regelt, maar dan voor de snelheid waarmee de arm moet bewegen. Heeft ook de waardes 0,1, of 2.
wiesdat 0:25a2e7ea29f3 302 // Bij 2 maal achter elkaar EMG 1 en EMG 2 gebruikt, wordt de snelheid vastgezet.
wiesdat 0:25a2e7ea29f3 303 uint8_t velocity (uint8_t y1, uint8_t y2)
wiesdat 0:25a2e7ea29f3 304 {
wiesdat 0:25a2e7ea29f3 305 if (y1>0 && y2>0 && check>0) {
wiesdat 0:25a2e7ea29f3 306 speeddone=1;
wiesdat 0:25a2e7ea29f3 307 check=0;
wiesdat 1:a010e434a360 308 rood = 0;
wiesdat 1:a010e434a360 309 groen = 1;
wiesdat 0:25a2e7ea29f3 310 } else if (y1>0 && y2>0) {
wiesdat 0:25a2e7ea29f3 311 check=1;
wiesdat 0:25a2e7ea29f3 312 } else if (y1>0) {
wiesdat 0:25a2e7ea29f3 313 if (speed>0) {
wiesdat 2:f3e8a27d376c 314 speed+=1;
wiesdat 0:25a2e7ea29f3 315 } else {
wiesdat 0:25a2e7ea29f3 316 check=0;
wiesdat 0:25a2e7ea29f3 317 }
wiesdat 0:25a2e7ea29f3 318 } else if (y2>0 ) {
wiesdat 0:25a2e7ea29f3 319 if (speed<2) {
wiesdat 2:f3e8a27d376c 320 speed-=1;
wiesdat 0:25a2e7ea29f3 321 } else {
wiesdat 0:25a2e7ea29f3 322 check=0;
wiesdat 0:25a2e7ea29f3 323 }
wiesdat 0:25a2e7ea29f3 324 } else {
wiesdat 0:25a2e7ea29f3 325 check=0;
wiesdat 2:f3e8a27d376c 326
wiesdat 0:25a2e7ea29f3 327 }
wiesdat 0:25a2e7ea29f3 328 return speed;
wiesdat 0:25a2e7ea29f3 329 }
wiesdat 0:25a2e7ea29f3 330
wiesdat 0:25a2e7ea29f3 331 void batposition(int y)
wiesdat 0:25a2e7ea29f3 332 {
wiesdat 0:25a2e7ea29f3 333 switch(y) {
wiesdat 0:25a2e7ea29f3 334 case 0:
wiesdat 0:25a2e7ea29f3 335
wiesdat 0:25a2e7ea29f3 336 dir = 1;
wiesdat 0:25a2e7ea29f3 337 pwm.write(0.4);
wiesdat 0:25a2e7ea29f3 338 wait(0.02);
wiesdat 0:25a2e7ea29f3 339 pwm.write(0);
wiesdat 0:25a2e7ea29f3 340 break;
wiesdat 0:25a2e7ea29f3 341 case 1:
wiesdat 0:25a2e7ea29f3 342
wiesdat 0:25a2e7ea29f3 343 dir = 0;
wiesdat 0:25a2e7ea29f3 344 pwm.write(0.4);
wiesdat 0:25a2e7ea29f3 345 wait(0.02);
wiesdat 0:25a2e7ea29f3 346 pwm.write(0);
wiesdat 0:25a2e7ea29f3 347 break;
wiesdat 0:25a2e7ea29f3 348 case 2:
wiesdat 0:25a2e7ea29f3 349
wiesdat 0:25a2e7ea29f3 350 pwm.write(0);
wiesdat 0:25a2e7ea29f3 351 break;
wiesdat 0:25a2e7ea29f3 352
wiesdat 0:25a2e7ea29f3 353 }
wiesdat 0:25a2e7ea29f3 354
wiesdat 0:25a2e7ea29f3 355 }
wiesdat 0:25a2e7ea29f3 356
wiesdat 2:f3e8a27d376c 357 void clamp(float * in, float min, float max)
wiesdat 2:f3e8a27d376c 358 {
wiesdat 2:f3e8a27d376c 359 *in > min ? *in < max? : *in = max: *in = min;
wiesdat 2:f3e8a27d376c 360 }
wiesdat 2:f3e8a27d376c 361
wiesdat 2:f3e8a27d376c 362 float pid1(float setpoint, float measurement)
wiesdat 2:f3e8a27d376c 363 {
wiesdat 2:f3e8a27d376c 364 float error;
wiesdat 2:f3e8a27d376c 365 static float prev_error = 0;
wiesdat 2:f3e8a27d376c 366 float out_p = 0;
wiesdat 2:f3e8a27d376c 367
wiesdat 2:f3e8a27d376c 368 float out_d = 0;
wiesdat 2:f3e8a27d376c 369 error = setpoint-measurement;
wiesdat 2:f3e8a27d376c 370 out_p = error*K_P;
wiesdat 2:f3e8a27d376c 371 out_i += error*K_I;
wiesdat 2:f3e8a27d376c 372 out_d = (error-prev_error)*K_D;
wiesdat 2:f3e8a27d376c 373 clamp(&out_i,-I_LIMIT,I_LIMIT);
wiesdat 2:f3e8a27d376c 374 prev_error = error;
wiesdat 2:f3e8a27d376c 375 out1 = out_i+out_p+out_d;
wiesdat 2:f3e8a27d376c 376 return out1;
wiesdat 2:f3e8a27d376c 377 }
wiesdat 2:f3e8a27d376c 378
wiesdat 2:f3e8a27d376c 379 float getv(float delta_t)
wiesdat 2:f3e8a27d376c 380 {
wiesdat 2:f3e8a27d376c 381 int n =0 ;
wiesdat 2:f3e8a27d376c 382 while(n<3) {
wiesdat 2:f3e8a27d376c 383 wait(delta_t);
wiesdat 2:f3e8a27d376c 384 enc = encoderA.getPosition();
wiesdat 2:f3e8a27d376c 385 enca2 = enca1;
wiesdat 2:f3e8a27d376c 386 enca1 = enc;
wiesdat 2:f3e8a27d376c 387 n++;
wiesdat 2:f3e8a27d376c 388
wiesdat 2:f3e8a27d376c 389 }
wiesdat 2:f3e8a27d376c 390
wiesdat 2:f3e8a27d376c 391 counts = (enca1 - enca2)/delta_t;
wiesdat 2:f3e8a27d376c 392 v = (counts)*((2*3.14159265359)/1550);
wiesdat 2:f3e8a27d376c 393 return v;
wiesdat 2:f3e8a27d376c 394 }
wiesdat 2:f3e8a27d376c 395
wiesdat 2:f3e8a27d376c 396
wiesdat 2:f3e8a27d376c 397 float gotopos(float pos)
wiesdat 2:f3e8a27d376c 398 {
wiesdat 2:f3e8a27d376c 399 switch(a) {
wiesdat 2:f3e8a27d376c 400 case 1:
wiesdat 2:f3e8a27d376c 401 pos = 100;
wiesdat 2:f3e8a27d376c 402 break;
wiesdat 2:f3e8a27d376c 403
wiesdat 2:f3e8a27d376c 404 case 2:
wiesdat 2:f3e8a27d376c 405 pos = 200;
wiesdat 2:f3e8a27d376c 406 break;
wiesdat 2:f3e8a27d376c 407
wiesdat 2:f3e8a27d376c 408 case 3:
wiesdat 2:f3e8a27d376c 409 pos = 250;
wiesdat 2:f3e8a27d376c 410 break;
wiesdat 2:f3e8a27d376c 411 }
wiesdat 2:f3e8a27d376c 412 enc = encoder1.getPosition();
wiesdat 2:f3e8a27d376c 413 while((abs(pos-encoder1.getPosition()) >6)|| (v!= 0)) {
wiesdat 2:f3e8a27d376c 414
wiesdat 2:f3e8a27d376c 415 while(!looptimerflag);
wiesdat 2:f3e8a27d376c 416 looptimerflag = false;
wiesdat 2:f3e8a27d376c 417 out1 = pid1(pos,encoder1.getPosition());
wiesdat 2:f3e8a27d376c 418
wiesdat 2:f3e8a27d376c 419 if(out>0) {
wiesdat 2:f3e8a27d376c 420 dir1 = 1;
wiesdat 2:f3e8a27d376c 421
wiesdat 2:f3e8a27d376c 422 } else if(out<0) {
wiesdat 2:f3e8a27d376c 423 dir1 = 0;
wiesdat 2:f3e8a27d376c 424 }
wiesdat 2:f3e8a27d376c 425 pwm1 = fabs(out);
wiesdat 2:f3e8a27d376c 426 v = getv(0.001);
wiesdat 2:f3e8a27d376c 427 }
wiesdat 2:f3e8a27d376c 428 pwm1 =0;
wiesdat 2:f3e8a27d376c 429
wiesdat 2:f3e8a27d376c 430 return pwm1;
wiesdat 2:f3e8a27d376c 431 }
wiesdat 2:f3e8a27d376c 432
wiesdat 2:f3e8a27d376c 433 float reset()
wiesdat 2:f3e8a27d376c 434 {
wiesdat 2:f3e8a27d376c 435 v = 1;
wiesdat 2:f3e8a27d376c 436 while(v !=0) {
wiesdat 2:f3e8a27d376c 437
wiesdat 2:f3e8a27d376c 438 dir = 0;
wiesdat 2:f3e8a27d376c 439 speed = 0.1;
wiesdat 2:f3e8a27d376c 440 pwm = speed;
wiesdat 2:f3e8a27d376c 441 v =getv(0.1);
wiesdat 2:f3e8a27d376c 442 }
wiesdat 2:f3e8a27d376c 443 pwm = 0;
wiesdat 2:f3e8a27d376c 444 dir =1;
wiesdat 2:f3e8a27d376c 445 encoderA.setPosition(0);
wiesdat 2:f3e8a27d376c 446 zero = encoderA.getPosition();
wiesdat 2:f3e8a27d376c 447
wiesdat 2:f3e8a27d376c 448 return pwm;
wiesdat 2:f3e8a27d376c 449 }
wiesdat 2:f3e8a27d376c 450
wiesdat 0:25a2e7ea29f3 451 // Main code
wiesdat 0:25a2e7ea29f3 452 int main()
wiesdat 0:25a2e7ea29f3 453 {
wiesdat 2:f3e8a27d376c 454 v = 1;
wiesdat 1:a010e434a360 455 rood = 0;
wiesdat 1:a010e434a360 456 blauw = 1;
wiesdat 1:a010e434a360 457 groen = 1;
wiesdat 0:25a2e7ea29f3 458 // Startcondities
wiesdat 0:25a2e7ea29f3 459 pc.baud(115200);
wiesdat 0:25a2e7ea29f3 460 timer.attach(setlooptimerflag,TSAMP);
wiesdat 0:25a2e7ea29f3 461 // 20 secondes wachten, zodat er geen effecten van het opstarten van het bordje, alvast waardes voor het badje of de arm vastzetten.
wiesdat 0:25a2e7ea29f3 462 wait(2);
wiesdat 0:25a2e7ea29f3 463
wiesdat 0:25a2e7ea29f3 464 while(1) {
wiesdat 0:25a2e7ea29f3 465
wiesdat 0:25a2e7ea29f3 466 // Ticker
wiesdat 0:25a2e7ea29f3 467 while(!looptimerflag);
wiesdat 0:25a2e7ea29f3 468
wiesdat 0:25a2e7ea29f3 469 looptimerflag = false;
wiesdat 0:25a2e7ea29f3 470
wiesdat 0:25a2e7ea29f3 471 // EMG uitlezen
wiesdat 0:25a2e7ea29f3 472 emg_value1 = readEMG1();
wiesdat 0:25a2e7ea29f3 473 emg_value2 = readEMG2();
wiesdat 0:25a2e7ea29f3 474
wiesdat 0:25a2e7ea29f3 475 // Filters gebruiken
wiesdat 0:25a2e7ea29f3 476 yave1 = filter1(emg_value1);
wiesdat 0:25a2e7ea29f3 477 yave2 = filter2(emg_value2);
wiesdat 0:25a2e7ea29f3 478
wiesdat 0:25a2e7ea29f3 479 // Checken of de waardes boven de threshold uitkomen
wiesdat 0:25a2e7ea29f3 480 y1=threshold1(yave1);
wiesdat 0:25a2e7ea29f3 481 y2=threshold2(yave2);
wiesdat 0:25a2e7ea29f3 482
wiesdat 0:25a2e7ea29f3 483 // Aantal metingen tellen
wiesdat 0:25a2e7ea29f3 484 teller++;
wiesdat 0:25a2e7ea29f3 485
wiesdat 0:25a2e7ea29f3 486 // Om de 500 metingen controleren
wiesdat 0:25a2e7ea29f3 487 if (teller==500) {
wiesdat 0:25a2e7ea29f3 488
wiesdat 0:25a2e7ea29f3 489 teller=0;
wiesdat 0:25a2e7ea29f3 490 // Als de hoek voor het badje nog niet klaar is, zet badje in de huidig aangegeven stand
wiesdat 0:25a2e7ea29f3 491 if (badjedone==0) {
wiesdat 0:25a2e7ea29f3 492 badjestand=badje(y1,y2);
wiesdat 1:a010e434a360 493
wiesdat 0:25a2e7ea29f3 494 batposition(badjestand);
wiesdat 1:a010e434a360 495
wiesdat 0:25a2e7ea29f3 496 // Als de hoek voor het badje wel klaar is:
wiesdat 0:25a2e7ea29f3 497 } else if (badjedone==1) {
wiesdat 0:25a2e7ea29f3 498 // Als de snelheid van de arm ook al klaar is, zorg ervoor dat de rest iet meer gebeurt
wiesdat 0:25a2e7ea29f3 499 if (speeddone==1) {
wiesdat 2:f3e8a27d376c 500
wiesdat 0:25a2e7ea29f3 501 badjedone=0;
wiesdat 0:25a2e7ea29f3 502 speeddone=0;
wiesdat 0:25a2e7ea29f3 503 badjestand=1;
wiesdat 0:25a2e7ea29f3 504 armstand=0;
wiesdat 0:25a2e7ea29f3 505 // Anders, pas de hoek van de arm aan naar de stand die hoort bij de huidig geselecteerde snelheid
wiesdat 0:25a2e7ea29f3 506 } else {
wiesdat 0:25a2e7ea29f3 507 armstand=velocity(y1,y2);
wiesdat 2:f3e8a27d376c 508 gotopos(armstand);
wiesdat 0:25a2e7ea29f3 509 }
wiesdat 0:25a2e7ea29f3 510
wiesdat 0:25a2e7ea29f3 511 }
wiesdat 0:25a2e7ea29f3 512 }
wiesdat 0:25a2e7ea29f3 513 // Verzend data naar de scopes
wiesdat 0:25a2e7ea29f3 514
wiesdat 0:25a2e7ea29f3 515 viewer();
wiesdat 2:f3e8a27d376c 516 reset();
wiesdat 2:f3e8a27d376c 517
wiesdat 0:25a2e7ea29f3 518
wiesdat 0:25a2e7ea29f3 519 }
wiesdat 0:25a2e7ea29f3 520
wiesdat 0:25a2e7ea29f3 521 }