thijs ruikes / Mbed 2 deprecated PROJECT-ROBOTCONTROL

Robotcontrol groep 2

Dependencies:   Encoder MODSERIAL mbed HIDScope

main.cpp@3:611fd72c9d46, 2014-11-01 (annotated)

Committer:
wiesdat
Date:
Sat Nov 01 19:40:46 2014 +0000
Revision:
3:611fd72c9d46
Parent:
2:f3e8a27d376c
Child:
4:c22f3095b130
Voor opruimsessie

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