thijs ruikes / Mbed 2 deprecated PROJECT-ROBOTCONTROL

Robotcontrol groep 2

Dependencies:   Encoder MODSERIAL mbed HIDScope

main.cpp@0:25a2e7ea29f3, 2014-10-31 (annotated)

Committer:
wiesdat
Date:
Fri Oct 31 11:28:10 2014 +0000
Revision:
0:25a2e7ea29f3
Child:
1:a010e434a360
Werkende patje positie

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 0:25a2e7ea29f3 4 #include <iostream>
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 0:25a2e7ea29f3 14
wiesdat 0:25a2e7ea29f3 15 // Outputs naar de motor
wiesdat 0:25a2e7ea29f3 16 PwmOut pwm(PTC8);
wiesdat 0:25a2e7ea29f3 17 DigitalOut dir(PTC9);
wiesdat 0:25a2e7ea29f3 18
wiesdat 0:25a2e7ea29f3 19 // Vaststellen van het datatype van de outputs naar de motor
wiesdat 0:25a2e7ea29f3 20 float s_speed1;
wiesdat 0:25a2e7ea29f3 21 float s_dir1;
wiesdat 0:25a2e7ea29f3 22 float s_speed2;
wiesdat 0:25a2e7ea29f3 23 float s_dir2;
wiesdat 0:25a2e7ea29f3 24
wiesdat 0:25a2e7ea29f3 25 // Alle constantes voor de filters definiëren
wiesdat 0:25a2e7ea29f3 26 // Constantes voor de Low Pass filter
wiesdat 0:25a2e7ea29f3 27 #define A1LP 0.018180963222803
wiesdat 0:25a2e7ea29f3 28 #define A0LP 0.016544013176248
wiesdat 0:25a2e7ea29f3 29 #define B1LP -1.718913340044714
wiesdat 0:25a2e7ea29f3 30 #define B0LP 0.753638316443765
wiesdat 0:25a2e7ea29f3 31
wiesdat 0:25a2e7ea29f3 32 // Constantes voor de High Pass Filter
wiesdat 0:25a2e7ea29f3 33 #define A1HP -1.999801878951505
wiesdat 0:25a2e7ea29f3 34 #define A0HP 0.999801878951505
wiesdat 0:25a2e7ea29f3 35 #define B1HP -1.971717601075000
wiesdat 0:25a2e7ea29f3 36 #define B0HP 0.972111984032897
wiesdat 0:25a2e7ea29f3 37
wiesdat 0:25a2e7ea29f3 38 // Constantes voor het Notch Filter
wiesdat 0:25a2e7ea29f3 39 #define A0N 0.99436777112
wiesdat 0:25a2e7ea29f3 40 #define A1N -1.89139989664
wiesdat 0:25a2e7ea29f3 41 #define A2N 0.99436777112
wiesdat 0:25a2e7ea29f3 42 #define B1N 1.89070035439
wiesdat 0:25a2e7ea29f3 43 #define B2N -0.988036
wiesdat 0:25a2e7ea29f3 44
wiesdat 0:25a2e7ea29f3 45 // Sample Time
wiesdat 0:25a2e7ea29f3 46 #define TSAMP 0.001
wiesdat 0:25a2e7ea29f3 47
wiesdat 0:25a2e7ea29f3 48 // EMG Threshold waarde
wiesdat 0:25a2e7ea29f3 49 #define THRESHOLD 0.05
wiesdat 0:25a2e7ea29f3 50
wiesdat 0:25a2e7ea29f3 51 // EMG inputs
wiesdat 0:25a2e7ea29f3 52 AnalogIn emg1(PTB1);
wiesdat 0:25a2e7ea29f3 53 AnalogIn emg2(PTB2);
wiesdat 0:25a2e7ea29f3 54
wiesdat 0:25a2e7ea29f3 55 // PC communicatie
wiesdat 0:25a2e7ea29f3 56 MODSERIAL pc(USBTX,USBRX);
wiesdat 0:25a2e7ea29f3 57 HIDScope scope(5);
wiesdat 0:25a2e7ea29f3 58 int set;
wiesdat 0:25a2e7ea29f3 59
wiesdat 0:25a2e7ea29f3 60 // Ticker voor de meetgegevens
wiesdat 0:25a2e7ea29f3 61 Ticker timer;
wiesdat 0:25a2e7ea29f3 62 volatile bool looptimerflag;
wiesdat 0:25a2e7ea29f3 63
wiesdat 0:25a2e7ea29f3 64 // Waardes voor de filters reserveren en als float vaststellen
wiesdat 0:25a2e7ea29f3 65 float emg_value2, ylp2, yhp2, yn2;
wiesdat 0:25a2e7ea29f3 66 float emg_value1, ylp1, yhp1, yn1;
wiesdat 0:25a2e7ea29f3 67
wiesdat 0:25a2e7ea29f3 68 float ysum1 = 0, yave1=0 ;
wiesdat 0:25a2e7ea29f3 69 float ysum2 = 0, yave2=0 ;
wiesdat 0:25a2e7ea29f3 70
wiesdat 0:25a2e7ea29f3 71 // 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 72 uint8_t y1, y2;
wiesdat 0:25a2e7ea29f3 73 int check = 0;
wiesdat 0:25a2e7ea29f3 74
wiesdat 0:25a2e7ea29f3 75 // 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 76 int badjestand;
wiesdat 0:25a2e7ea29f3 77 int badjes=1;
wiesdat 0:25a2e7ea29f3 78 int badjedone=0;
wiesdat 0:25a2e7ea29f3 79 int speeding;
wiesdat 0:25a2e7ea29f3 80 int armstand=0;
wiesdat 0:25a2e7ea29f3 81 int armspeed=0;
wiesdat 0:25a2e7ea29f3 82 bool speeddone=0;
wiesdat 0:25a2e7ea29f3 83
wiesdat 0:25a2e7ea29f3 84 // Teller voor hoeveel metingen er zijn gedaan
wiesdat 0:25a2e7ea29f3 85 uint16_t teller=0;
wiesdat 0:25a2e7ea29f3 86
wiesdat 0:25a2e7ea29f3 87 // Random meuk die ik uit batjespositie haal, nog ff opruimen
wiesdat 0:25a2e7ea29f3 88 int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
wiesdat 0:25a2e7ea29f3 89 float speed = 0.1, out =0;
wiesdat 0:25a2e7ea29f3 90 int pos =0,zero =0, fout;
wiesdat 0:25a2e7ea29f3 91 float v=0;
wiesdat 0:25a2e7ea29f3 92 float out_i = 0;
wiesdat 0:25a2e7ea29f3 93 int y;
wiesdat 0:25a2e7ea29f3 94
wiesdat 0:25a2e7ea29f3 95
wiesdat 0:25a2e7ea29f3 96 // Scopes vaststellen
wiesdat 0:25a2e7ea29f3 97 void viewer()
wiesdat 0:25a2e7ea29f3 98 {
wiesdat 0:25a2e7ea29f3 99 scope.set(0,y1);
wiesdat 0:25a2e7ea29f3 100 scope.set(1,y2);
wiesdat 0:25a2e7ea29f3 101 scope.set(2,badjes);
wiesdat 0:25a2e7ea29f3 102 scope.set(3,armstand);
wiesdat 0:25a2e7ea29f3 103 scope.set(4,speeddone);
wiesdat 0:25a2e7ea29f3 104 scope.send();
wiesdat 0:25a2e7ea29f3 105 }
wiesdat 0:25a2e7ea29f3 106
wiesdat 0:25a2e7ea29f3 107 // EMG 1 uitlezen
wiesdat 0:25a2e7ea29f3 108 float readEMG1()
wiesdat 0:25a2e7ea29f3 109 {
wiesdat 0:25a2e7ea29f3 110
wiesdat 0:25a2e7ea29f3 111 emg_value1=emg1.read();
wiesdat 0:25a2e7ea29f3 112 return emg_value1;
wiesdat 0:25a2e7ea29f3 113 }
wiesdat 0:25a2e7ea29f3 114
wiesdat 0:25a2e7ea29f3 115 // EMG 2 uitlezen
wiesdat 0:25a2e7ea29f3 116 float readEMG2()
wiesdat 0:25a2e7ea29f3 117 {
wiesdat 0:25a2e7ea29f3 118
wiesdat 0:25a2e7ea29f3 119 emg_value2=emg2.read();
wiesdat 0:25a2e7ea29f3 120 return emg_value2;
wiesdat 0:25a2e7ea29f3 121 }
wiesdat 0:25a2e7ea29f3 122
wiesdat 0:25a2e7ea29f3 123 // Notch Filter voor EMG 1
wiesdat 0:25a2e7ea29f3 124 float notchfilter1(float ylp1)
wiesdat 0:25a2e7ea29f3 125 {
wiesdat 0:25a2e7ea29f3 126 static float yn1,x1=0,x2=0,y1=0,y2=0,x;
wiesdat 0:25a2e7ea29f3 127 x = ylp1;
wiesdat 0:25a2e7ea29f3 128 yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 0:25a2e7ea29f3 129 x2 = x1;
wiesdat 0:25a2e7ea29f3 130 x1 = x;
wiesdat 0:25a2e7ea29f3 131 y2 = y1;
wiesdat 0:25a2e7ea29f3 132 y1 = yn1;
wiesdat 0:25a2e7ea29f3 133 return yn1;
wiesdat 0:25a2e7ea29f3 134 }
wiesdat 0:25a2e7ea29f3 135
wiesdat 0:25a2e7ea29f3 136 // Notch Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
wiesdat 0:25a2e7ea29f3 137 float notchfilter2(float ylp2)
wiesdat 0:25a2e7ea29f3 138 {
wiesdat 0:25a2e7ea29f3 139 static float x1=0,x2=0,y1=0,y2=0,x;
wiesdat 0:25a2e7ea29f3 140 x = ylp2;
wiesdat 0:25a2e7ea29f3 141 yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
wiesdat 0:25a2e7ea29f3 142 x2 = x1;
wiesdat 0:25a2e7ea29f3 143 x1 = x;
wiesdat 0:25a2e7ea29f3 144 y2 = y1;
wiesdat 0:25a2e7ea29f3 145 y1 = yn2;
wiesdat 0:25a2e7ea29f3 146 return yn2;
wiesdat 0:25a2e7ea29f3 147 }
wiesdat 0:25a2e7ea29f3 148
wiesdat 0:25a2e7ea29f3 149 // High Pass Filter voor EMG 1
wiesdat 0:25a2e7ea29f3 150 float hpfilter1(float yn1)
wiesdat 0:25a2e7ea29f3 151 {
wiesdat 0:25a2e7ea29f3 152 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 153 x = yn1;
wiesdat 0:25a2e7ea29f3 154 yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:25a2e7ea29f3 155 x2 = x1;
wiesdat 0:25a2e7ea29f3 156 x1 = x;
wiesdat 0:25a2e7ea29f3 157 y2 = y1;
wiesdat 0:25a2e7ea29f3 158 y1 = yhp1;
wiesdat 0:25a2e7ea29f3 159 return yhp1;
wiesdat 0:25a2e7ea29f3 160 }
wiesdat 0:25a2e7ea29f3 161
wiesdat 0:25a2e7ea29f3 162 // High Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
wiesdat 0:25a2e7ea29f3 163 float hpfilter2(float yn2)
wiesdat 0:25a2e7ea29f3 164 {
wiesdat 0:25a2e7ea29f3 165 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 166 x = yn2;
wiesdat 0:25a2e7ea29f3 167 yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
wiesdat 0:25a2e7ea29f3 168 x2 = x1;
wiesdat 0:25a2e7ea29f3 169 x1 = x;
wiesdat 0:25a2e7ea29f3 170 y2 = y1;
wiesdat 0:25a2e7ea29f3 171 y1 = yhp2;
wiesdat 0:25a2e7ea29f3 172 return yhp2;
wiesdat 0:25a2e7ea29f3 173 }
wiesdat 0:25a2e7ea29f3 174
wiesdat 0:25a2e7ea29f3 175 // Low Pass Filter voor EMG 1
wiesdat 0:25a2e7ea29f3 176 float lpfilter1(float yhp1)
wiesdat 0:25a2e7ea29f3 177 {
wiesdat 0:25a2e7ea29f3 178 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 179 x = yhp1;
wiesdat 0:25a2e7ea29f3 180 ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 0:25a2e7ea29f3 181 x2 = x1;
wiesdat 0:25a2e7ea29f3 182 x1 = x;
wiesdat 0:25a2e7ea29f3 183 y2 = y1;
wiesdat 0:25a2e7ea29f3 184 y1 = ylp1;
wiesdat 0:25a2e7ea29f3 185 return ylp1;
wiesdat 0:25a2e7ea29f3 186 }
wiesdat 0:25a2e7ea29f3 187
wiesdat 0:25a2e7ea29f3 188 // Low Pass Filter voor EMG 2 (idem aan die voor EMG 1, maar met eigen variabelen)
wiesdat 0:25a2e7ea29f3 189 float lpfilter2(float yhp2)
wiesdat 0:25a2e7ea29f3 190 {
wiesdat 0:25a2e7ea29f3 191 static float x1=0,y1=0,x2=0, y2=0,x;
wiesdat 0:25a2e7ea29f3 192 x = yhp2;
wiesdat 0:25a2e7ea29f3 193 ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
wiesdat 0:25a2e7ea29f3 194 x2 = x1;
wiesdat 0:25a2e7ea29f3 195 x1 = x;
wiesdat 0:25a2e7ea29f3 196 y2 = y1;
wiesdat 0:25a2e7ea29f3 197 y1 = ylp2;
wiesdat 0:25a2e7ea29f3 198 return ylp2;
wiesdat 0:25a2e7ea29f3 199 }
wiesdat 0:25a2e7ea29f3 200
wiesdat 0:25a2e7ea29f3 201 // Stukje nodig voor de ticker
wiesdat 0:25a2e7ea29f3 202 void setlooptimerflag(void)
wiesdat 0:25a2e7ea29f3 203 {
wiesdat 0:25a2e7ea29f3 204 looptimerflag = true;
wiesdat 0:25a2e7ea29f3 205 }
wiesdat 0:25a2e7ea29f3 206
wiesdat 0:25a2e7ea29f3 207 // Combinatie van de drie bovengenoemde filters voor EMG 1, en de middeling van 500 dataputen (nieuwe data 2 keer per seconde)
wiesdat 0:25a2e7ea29f3 208 float filter1(float emg_value1)
wiesdat 0:25a2e7ea29f3 209 {
wiesdat 0:25a2e7ea29f3 210 static uint16_t n;
wiesdat 0:25a2e7ea29f3 211 yn1 = notchfilter1(emg_value1);
wiesdat 0:25a2e7ea29f3 212 yhp1 = hpfilter1(yn1);
wiesdat 0:25a2e7ea29f3 213 ylp1 = lpfilter1(yhp1);
wiesdat 0:25a2e7ea29f3 214 ylp1 = fabs(ylp1);
wiesdat 0:25a2e7ea29f3 215 ysum1 = ysum1+ylp1;
wiesdat 0:25a2e7ea29f3 216 n++;
wiesdat 0:25a2e7ea29f3 217 if(n==500) {
wiesdat 0:25a2e7ea29f3 218 yave1 = ysum1/500;
wiesdat 0:25a2e7ea29f3 219 ysum1 = 0;
wiesdat 0:25a2e7ea29f3 220 n = 0;
wiesdat 0:25a2e7ea29f3 221 }
wiesdat 0:25a2e7ea29f3 222 return yave1;
wiesdat 0:25a2e7ea29f3 223 }
wiesdat 0:25a2e7ea29f3 224
wiesdat 0:25a2e7ea29f3 225 // Idem voor EMG 2
wiesdat 0:25a2e7ea29f3 226 float filter2(float emg_value2)
wiesdat 0:25a2e7ea29f3 227 {
wiesdat 0:25a2e7ea29f3 228 static uint16_t n;
wiesdat 0:25a2e7ea29f3 229 yn2 = notchfilter2(emg_value2);
wiesdat 0:25a2e7ea29f3 230 yhp2 = hpfilter2(yn2);
wiesdat 0:25a2e7ea29f3 231 ylp2 = lpfilter2(yhp2);
wiesdat 0:25a2e7ea29f3 232 ylp2 = fabs(ylp2);
wiesdat 0:25a2e7ea29f3 233 ysum2 = ysum2 + ylp2;
wiesdat 0:25a2e7ea29f3 234 n++;
wiesdat 0:25a2e7ea29f3 235
wiesdat 0:25a2e7ea29f3 236 if(n==500) {
wiesdat 0:25a2e7ea29f3 237 yave2 = ysum2/500;
wiesdat 0:25a2e7ea29f3 238 ysum2 = 0;
wiesdat 0:25a2e7ea29f3 239 n = 0;
wiesdat 0:25a2e7ea29f3 240 }
wiesdat 0:25a2e7ea29f3 241 return yave2;
wiesdat 0:25a2e7ea29f3 242 }
wiesdat 0:25a2e7ea29f3 243
wiesdat 0:25a2e7ea29f3 244 // Check of de waarde van het EMG boven de gedefinieerde threshold waarde uitkomt voor EMG 1
wiesdat 0:25a2e7ea29f3 245 float threshold1 (float yave1)
wiesdat 0:25a2e7ea29f3 246 {
wiesdat 0:25a2e7ea29f3 247 if(yave1>THRESHOLD) {
wiesdat 0:25a2e7ea29f3 248 y1 = 1;
wiesdat 0:25a2e7ea29f3 249 } else {
wiesdat 0:25a2e7ea29f3 250 y1 = 0;
wiesdat 0:25a2e7ea29f3 251 }
wiesdat 0:25a2e7ea29f3 252 return y1;
wiesdat 0:25a2e7ea29f3 253 }
wiesdat 0:25a2e7ea29f3 254
wiesdat 0:25a2e7ea29f3 255 //Idem voor EMG 2
wiesdat 0:25a2e7ea29f3 256 float threshold2 (float yave2)
wiesdat 0:25a2e7ea29f3 257 {
wiesdat 0:25a2e7ea29f3 258 if(yave2>THRESHOLD) {
wiesdat 0:25a2e7ea29f3 259 y2 = 1;
wiesdat 0:25a2e7ea29f3 260 } else {
wiesdat 0:25a2e7ea29f3 261 y2 = 0;
wiesdat 0:25a2e7ea29f3 262 }
wiesdat 0:25a2e7ea29f3 263 return y2;
wiesdat 0:25a2e7ea29f3 264 }
wiesdat 0:25a2e7ea29f3 265
wiesdat 0:25a2e7ea29f3 266 // 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 267 // 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 268 uint8_t badje (uint8_t y1, uint8_t y2)
wiesdat 0:25a2e7ea29f3 269 {
wiesdat 0:25a2e7ea29f3 270 if (y1>0 && y2>0 && check>0) {
wiesdat 0:25a2e7ea29f3 271 badjedone=1;
wiesdat 0:25a2e7ea29f3 272 check=0;
wiesdat 0:25a2e7ea29f3 273 } else if (y1>0 && y2>0) {
wiesdat 0:25a2e7ea29f3 274 check=1;
wiesdat 0:25a2e7ea29f3 275 } else if (y1>0) {
wiesdat 0:25a2e7ea29f3 276 badjes=0;
wiesdat 0:25a2e7ea29f3 277 check=0;
wiesdat 0:25a2e7ea29f3 278 } else if (y2>0 ) {
wiesdat 0:25a2e7ea29f3 279 badjes=1;
wiesdat 0:25a2e7ea29f3 280 check=0;
wiesdat 0:25a2e7ea29f3 281 }
wiesdat 0:25a2e7ea29f3 282
wiesdat 0:25a2e7ea29f3 283 else {
wiesdat 0:25a2e7ea29f3 284 check=0;
wiesdat 0:25a2e7ea29f3 285 badjes = 2;
wiesdat 0:25a2e7ea29f3 286 }
wiesdat 0:25a2e7ea29f3 287 return badjes;
wiesdat 0:25a2e7ea29f3 288 }
wiesdat 0:25a2e7ea29f3 289
wiesdat 0:25a2e7ea29f3 290 // 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 291 // Bij 2 maal achter elkaar EMG 1 en EMG 2 gebruikt, wordt de snelheid vastgezet.
wiesdat 0:25a2e7ea29f3 292 uint8_t velocity (uint8_t y1, uint8_t y2)
wiesdat 0:25a2e7ea29f3 293 {
wiesdat 0:25a2e7ea29f3 294 if (y1>0 && y2>0 && check>0) {
wiesdat 0:25a2e7ea29f3 295 speeddone=1;
wiesdat 0:25a2e7ea29f3 296 check=0;
wiesdat 0:25a2e7ea29f3 297 } else if (y1>0 && y2>0) {
wiesdat 0:25a2e7ea29f3 298 check=1;
wiesdat 0:25a2e7ea29f3 299 } else if (y1>0) {
wiesdat 0:25a2e7ea29f3 300 if (speed>0) {
wiesdat 0:25a2e7ea29f3 301 speed=speed-1;
wiesdat 0:25a2e7ea29f3 302 } else {
wiesdat 0:25a2e7ea29f3 303 check=0;
wiesdat 0:25a2e7ea29f3 304 }
wiesdat 0:25a2e7ea29f3 305 } else if (y2>0 ) {
wiesdat 0:25a2e7ea29f3 306 if (speed<2) {
wiesdat 0:25a2e7ea29f3 307 speed=speed+1;
wiesdat 0:25a2e7ea29f3 308 } else {
wiesdat 0:25a2e7ea29f3 309 check=0;
wiesdat 0:25a2e7ea29f3 310 }
wiesdat 0:25a2e7ea29f3 311 } else {
wiesdat 0:25a2e7ea29f3 312 check=0;
wiesdat 0:25a2e7ea29f3 313 }
wiesdat 0:25a2e7ea29f3 314 return speed;
wiesdat 0:25a2e7ea29f3 315 }
wiesdat 0:25a2e7ea29f3 316
wiesdat 0:25a2e7ea29f3 317 void batposition(int y)
wiesdat 0:25a2e7ea29f3 318 {
wiesdat 0:25a2e7ea29f3 319 switch(y) {
wiesdat 0:25a2e7ea29f3 320 case 0:
wiesdat 0:25a2e7ea29f3 321
wiesdat 0:25a2e7ea29f3 322 dir = 1;
wiesdat 0:25a2e7ea29f3 323 pwm.write(0.4);
wiesdat 0:25a2e7ea29f3 324 wait(0.02);
wiesdat 0:25a2e7ea29f3 325 pwm.write(0);
wiesdat 0:25a2e7ea29f3 326 break;
wiesdat 0:25a2e7ea29f3 327 case 1:
wiesdat 0:25a2e7ea29f3 328
wiesdat 0:25a2e7ea29f3 329 dir = 0;
wiesdat 0:25a2e7ea29f3 330 pwm.write(0.4);
wiesdat 0:25a2e7ea29f3 331 wait(0.02);
wiesdat 0:25a2e7ea29f3 332 pwm.write(0);
wiesdat 0:25a2e7ea29f3 333 break;
wiesdat 0:25a2e7ea29f3 334 case 2:
wiesdat 0:25a2e7ea29f3 335
wiesdat 0:25a2e7ea29f3 336 pwm.write(0);
wiesdat 0:25a2e7ea29f3 337 break;
wiesdat 0:25a2e7ea29f3 338
wiesdat 0:25a2e7ea29f3 339 }
wiesdat 0:25a2e7ea29f3 340
wiesdat 0:25a2e7ea29f3 341 }
wiesdat 0:25a2e7ea29f3 342
wiesdat 0:25a2e7ea29f3 343 // Main code
wiesdat 0:25a2e7ea29f3 344 int main()
wiesdat 0:25a2e7ea29f3 345 {
wiesdat 0:25a2e7ea29f3 346 // Startcondities
wiesdat 0:25a2e7ea29f3 347 pc.baud(115200);
wiesdat 0:25a2e7ea29f3 348 timer.attach(setlooptimerflag,TSAMP);
wiesdat 0:25a2e7ea29f3 349 // 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 350 wait(2);
wiesdat 0:25a2e7ea29f3 351
wiesdat 0:25a2e7ea29f3 352 while(1) {
wiesdat 0:25a2e7ea29f3 353
wiesdat 0:25a2e7ea29f3 354 // Ticker
wiesdat 0:25a2e7ea29f3 355 while(!looptimerflag);
wiesdat 0:25a2e7ea29f3 356
wiesdat 0:25a2e7ea29f3 357 looptimerflag = false;
wiesdat 0:25a2e7ea29f3 358
wiesdat 0:25a2e7ea29f3 359 // EMG uitlezen
wiesdat 0:25a2e7ea29f3 360 emg_value1 = readEMG1();
wiesdat 0:25a2e7ea29f3 361 emg_value2 = readEMG2();
wiesdat 0:25a2e7ea29f3 362
wiesdat 0:25a2e7ea29f3 363 // Filters gebruiken
wiesdat 0:25a2e7ea29f3 364 yave1 = filter1(emg_value1);
wiesdat 0:25a2e7ea29f3 365 yave2 = filter2(emg_value2);
wiesdat 0:25a2e7ea29f3 366
wiesdat 0:25a2e7ea29f3 367 // Checken of de waardes boven de threshold uitkomen
wiesdat 0:25a2e7ea29f3 368 y1=threshold1(yave1);
wiesdat 0:25a2e7ea29f3 369 y2=threshold2(yave2);
wiesdat 0:25a2e7ea29f3 370
wiesdat 0:25a2e7ea29f3 371 // Aantal metingen tellen
wiesdat 0:25a2e7ea29f3 372 teller++;
wiesdat 0:25a2e7ea29f3 373
wiesdat 0:25a2e7ea29f3 374 // Om de 500 metingen controleren
wiesdat 0:25a2e7ea29f3 375 if (teller==500) {
wiesdat 0:25a2e7ea29f3 376
wiesdat 0:25a2e7ea29f3 377 teller=0;
wiesdat 0:25a2e7ea29f3 378 // Als de hoek voor het badje nog niet klaar is, zet badje in de huidig aangegeven stand
wiesdat 0:25a2e7ea29f3 379 if (badjedone==0) {
wiesdat 0:25a2e7ea29f3 380 badjestand=badje(y1,y2);
wiesdat 0:25a2e7ea29f3 381 cout<<"ben voor batjes"<<endl;
wiesdat 0:25a2e7ea29f3 382 batposition(badjestand);
wiesdat 0:25a2e7ea29f3 383 cout<<"batjes gedaan"<<endl;
wiesdat 0:25a2e7ea29f3 384 // Als de hoek voor het badje wel klaar is:
wiesdat 0:25a2e7ea29f3 385 } else if (badjedone==1) {
wiesdat 0:25a2e7ea29f3 386 // Als de snelheid van de arm ook al klaar is, zorg ervoor dat de rest iet meer gebeurt
wiesdat 0:25a2e7ea29f3 387 if (speeddone==1) {
wiesdat 0:25a2e7ea29f3 388 armspeed=armstand+1;
wiesdat 0:25a2e7ea29f3 389 badjedone=0;
wiesdat 0:25a2e7ea29f3 390 speeddone=0;
wiesdat 0:25a2e7ea29f3 391 badjestand=1;
wiesdat 0:25a2e7ea29f3 392 armstand=0;
wiesdat 0:25a2e7ea29f3 393 // Anders, pas de hoek van de arm aan naar de stand die hoort bij de huidig geselecteerde snelheid
wiesdat 0:25a2e7ea29f3 394 } else {
wiesdat 0:25a2e7ea29f3 395 armstand=velocity(y1,y2);
wiesdat 0:25a2e7ea29f3 396 }
wiesdat 0:25a2e7ea29f3 397
wiesdat 0:25a2e7ea29f3 398 }
wiesdat 0:25a2e7ea29f3 399 }
wiesdat 0:25a2e7ea29f3 400 // Verzend data naar de scopes
wiesdat 0:25a2e7ea29f3 401
wiesdat 0:25a2e7ea29f3 402 viewer();
wiesdat 0:25a2e7ea29f3 403
wiesdat 0:25a2e7ea29f3 404 }
wiesdat 0:25a2e7ea29f3 405
wiesdat 0:25a2e7ea29f3 406 }