Esther Keulers / Mbed 2 deprecated Begintotaalscript

g

Dependencies:   MODSERIAL mbed Encoder

main.cpp@7:5380d17310e8, 2013-11-06 (annotated)

Committer:
gerjan
Date:
Wed Nov 06 17:02:58 2013 +0000
Revision:
7:5380d17310e8
Broncode projectgroep 2; ; Belinda Brandwacht s1226290; Esther Keulers s1255444; Maaike Schotman s1274104; Gerjan Wolterink s1197797; Roelof van Zwol s1240811

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gerjan 7:5380d17310e8 1 /* - - - - - - - - - - - - - - - - - - - - - - */
gerjan 7:5380d17310e8 2 /* */
gerjan 7:5380d17310e8 3 /* BRONCODE TEKENROBOT GROEP2: THE DRAWESOME */
gerjan 7:5380d17310e8 4 /* ----------------------------------------- */
gerjan 7:5380d17310e8 5 /* Belinda Brandwacht s1226290 */
gerjan 7:5380d17310e8 6 /* Esther Keulers s1255444 */
gerjan 7:5380d17310e8 7 /* Maaike Schotman s1274104 */
gerjan 7:5380d17310e8 8 /* Gerjan Wolterink s1197797 */
gerjan 7:5380d17310e8 9 /* Roelof van Zwol s1240811 */
gerjan 7:5380d17310e8 10 /* */
gerjan 7:5380d17310e8 11 /* - - - - - - - - - - - - - - - - - - - - - - */
gerjan 7:5380d17310e8 12
gerjan 7:5380d17310e8 13 //libraries aanroepen.
gerjan 7:5380d17310e8 14 #include "mbed.h"
gerjan 7:5380d17310e8 15 #include "MODSERIAL.h"
gerjan 7:5380d17310e8 16 #include "encoder.h"
gerjan 7:5380d17310e8 17
gerjan 7:5380d17310e8 18 // Looptimerflag instellen
gerjan 7:5380d17310e8 19
gerjan 7:5380d17310e8 20 volatile bool looptimerflag;
gerjan 7:5380d17310e8 21
gerjan 7:5380d17310e8 22 void setlooptimerflag(void)
gerjan 7:5380d17310e8 23 {
gerjan 7:5380d17310e8 24 looptimerflag=true;
gerjan 7:5380d17310e8 25 }
gerjan 7:5380d17310e8 26
gerjan 7:5380d17310e8 27
gerjan 7:5380d17310e8 28 int main()
gerjan 7:5380d17310e8 29 {
gerjan 7:5380d17310e8 30 //Communicatie met de pc
gerjan 7:5380d17310e8 31 MODSERIAL pc(USBTX,USBRX,64,1024);
gerjan 7:5380d17310e8 32 pc.baud(115200); //zet de baud in realterm ook op dit nummer.
gerjan 7:5380d17310e8 33
gerjan 7:5380d17310e8 34 //Benoemen inputs van de drie rode knoppen op de robot
gerjan 7:5380d17310e8 35 DigitalIn knop1(PTC2);
gerjan 7:5380d17310e8 36 DigitalIn knop2(PTB3);
gerjan 7:5380d17310e8 37 DigitalIn knop3(PTB2);
gerjan 7:5380d17310e8 38
gerjan 7:5380d17310e8 39 //Input mode van de knoppen instellen.
gerjan 7:5380d17310e8 40 knop1.mode(PullUp);
gerjan 7:5380d17310e8 41 knop2.mode(PullUp);
gerjan 7:5380d17310e8 42 knop3.mode(PullUp);
gerjan 7:5380d17310e8 43
gerjan 7:5380d17310e8 44 AnalogIn emg1(PTB0); //Analog input emg1
gerjan 7:5380d17310e8 45 AnalogIn emg2(PTB1); //Analog input emg2
gerjan 7:5380d17310e8 46
gerjan 7:5380d17310e8 47 //Benoemen pinnen waarop de encoder van de motoren is aangeloten.
gerjan 7:5380d17310e8 48 Encoder motor1(PTD0,PTC9);
gerjan 7:5380d17310e8 49 Encoder motor2(PTD2,PTC8);
gerjan 7:5380d17310e8 50
gerjan 7:5380d17310e8 51 /* PWM control to motor */
gerjan 7:5380d17310e8 52 PwmOut pwm_motor1(PTA12);
gerjan 7:5380d17310e8 53 PwmOut pwm_motor2(PTA5);
gerjan 7:5380d17310e8 54
gerjan 7:5380d17310e8 55 /* Direction pin */
gerjan 7:5380d17310e8 56 DigitalOut motor1dir(PTD3);
gerjan 7:5380d17310e8 57 DigitalOut motor2dir(PTD1);
gerjan 7:5380d17310e8 58
gerjan 7:5380d17310e8 59
gerjan 7:5380d17310e8 60 //Variabelen voor menu
gerjan 7:5380d17310e8 61 int state = 1;
gerjan 7:5380d17310e8 62
gerjan 7:5380d17310e8 63 // Met bool wordt gezegd dat variablen 'true' of 'false' kunnen zijn.
gerjan 7:5380d17310e8 64 bool calibratie_rust = false;
gerjan 7:5380d17310e8 65 bool calibratie_max = false;
gerjan 7:5380d17310e8 66 bool calibratie_motor = false;
gerjan 7:5380d17310e8 67 bool meting = false;
gerjan 7:5380d17310e8 68
gerjan 7:5380d17310e8 69 //Variabelen EMG/motoren
gerjan 7:5380d17310e8 70 double yy,z,zabs,w,y1,z1,zabs1,w1,y2,z2,zabs2,w2,e1,e2,e3,f1,f2,g1,g2,g3,h1,h2,byy,bz,bzabs,bw,by1,bz1,bzabs1,bw1,by2,bz2,bzabs2,bw2,be1,be2,be3,bf1,bf2,bg1,bg2,bg3,bh1,bh2;
gerjan 7:5380d17310e8 71 double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;
gerjan 7:5380d17310e8 72
gerjan 7:5380d17310e8 73 // Variabelen benoemen voor regelaar motor.
gerjan 7:5380d17310e8 74 double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm,pi;
gerjan 7:5380d17310e8 75 double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm, dri, dri_1, utot_r, inputsinus;
gerjan 7:5380d17310e8 76 double motor1_maxu, motor2_maxu;
gerjan 7:5380d17310e8 77
gerjan 7:5380d17310e8 78
gerjan 7:5380d17310e8 79 pi=3.14159265359;
gerjan 7:5380d17310e8 80
gerjan 7:5380d17310e8 81 e1= 0.855930601814815;
gerjan 7:5380d17310e8 82 e2= -1.711861203629630;
gerjan 7:5380d17310e8 83 e3= 0.855930601814815;
gerjan 7:5380d17310e8 84 f1=-1.690996376887443;
gerjan 7:5380d17310e8 85 f2= 0.732726030371816;
gerjan 7:5380d17310e8 86
gerjan 7:5380d17310e8 87 g1= 0.098259168204717*pow(10.0,-4.0);
gerjan 7:5380d17310e8 88 g2= 0.196518336409435*pow(10.0,-4.0);
gerjan 7:5380d17310e8 89 g3=0.098259168204717*pow(10.0,-4.0);
gerjan 7:5380d17310e8 90 h1= -1.991114292201654;
gerjan 7:5380d17310e8 91 h2=0.991153595868935;
gerjan 7:5380d17310e8 92
gerjan 7:5380d17310e8 93 be1=0.914969144113083;
gerjan 7:5380d17310e8 94 be2= -1.829938288226165;
gerjan 7:5380d17310e8 95 be3= 0.914969144113083;
gerjan 7:5380d17310e8 96 bf1=-1.822694925196308;
gerjan 7:5380d17310e8 97 bf2=0.837181651256022;
gerjan 7:5380d17310e8 98
gerjan 7:5380d17310e8 99 bg1= 0.246193004641015*pow(10.0,-5.0);
gerjan 7:5380d17310e8 100 bg2= 0.492386009282031 *pow(10.0,-5.0);
gerjan 7:5380d17310e8 101 bg3= 0.246193004641015*pow(10.0,-5.0);
gerjan 7:5380d17310e8 102 bh1= -1.995557124345789;
gerjan 7:5380d17310e8 103 bh2= 0.995566972065975;
gerjan 7:5380d17310e8 104
gerjan 7:5380d17310e8 105
gerjan 7:5380d17310e8 106 y1 = 0; /*Initialize y(n-1) = 0*/
gerjan 7:5380d17310e8 107 y2 = 0; /*Initialize y(n-2) = 0*/
gerjan 7:5380d17310e8 108 z1 = 0; /*Initialize z(n-1) = 0*/
gerjan 7:5380d17310e8 109 z2 = 0; /*Initialize z(n-2) = 0*/
gerjan 7:5380d17310e8 110 zabs1 = 0; /*Initialize zabs(n-1) = 0*/
gerjan 7:5380d17310e8 111 zabs2 = 0; /*Initialize zabs(n-2) = 0*/
gerjan 7:5380d17310e8 112 w1 = 0; /*Initialize w(n-1) = 0*/
gerjan 7:5380d17310e8 113 w2 = 0; /*Initialize w(n-2) = 0*/
gerjan 7:5380d17310e8 114
gerjan 7:5380d17310e8 115 by1 = 0; /*Initialize by(n-1) = 0*/
gerjan 7:5380d17310e8 116 by2 = 0; /*Initialize by(n-2) = 0*/
gerjan 7:5380d17310e8 117 bz1 = 0; /*Initialize bz(n-1) = 0*/
gerjan 7:5380d17310e8 118 bz2 = 0; /*Initialize bz(n-2) = 0*/
gerjan 7:5380d17310e8 119 bzabs1 = 0; /*Initialize bzabs(n-1) = 0*/
gerjan 7:5380d17310e8 120 bzabs2 = 0; /*Initialize bzabs(n-2) = 0*/
gerjan 7:5380d17310e8 121 bw1 = 0; /*Initialize bw(n-1) = 0*/
gerjan 7:5380d17310e8 122 bw2 = 0; /*Initialize bw(n-2) = 0*/
gerjan 7:5380d17310e8 123 const float Ts = 0.001; /*Sample Time, const(ant)*/
gerjan 7:5380d17310e8 124
gerjan 7:5380d17310e8 125 //Constanten voor tekening: EMGdeel
gerjan 7:5380d17310e8 126 xbegin= 0; //beginpositie
gerjan 7:5380d17310e8 127 ybegin=0; //beginpositie
gerjan 7:5380d17310e8 128 qbegin=5.50; //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
gerjan 7:5380d17310e8 129 MaxsnelheidV=150.0; //in mm/s
gerjan 7:5380d17310e8 130 MaxsnelheidD=0.5*pi; //in rad/s eerst 0.26 rad/s (15 deg/sec)
gerjan 7:5380d17310e8 131
gerjan 7:5380d17310e8 132 //constanten regelaar
gerjan 7:5380d17310e8 133 kp_r = 0.006;
gerjan 7:5380d17310e8 134 ki_r = 0.005;
gerjan 7:5380d17310e8 135
gerjan 7:5380d17310e8 136 Ticker looptimer;
gerjan 7:5380d17310e8 137 looptimer.attach(setlooptimerflag,Ts);
gerjan 7:5380d17310e8 138
gerjan 7:5380d17310e8 139 while (true) { //oneindige while loop
gerjan 7:5380d17310e8 140
gerjan 7:5380d17310e8 141 //Loop die allen gestart wordt als de looptimer is verlopen.
gerjan 7:5380d17310e8 142 while(looptimerflag != true);
gerjan 7:5380d17310e8 143 looptimerflag = false;
gerjan 7:5380d17310e8 144
gerjan 7:5380d17310e8 145
gerjan 7:5380d17310e8 146 // state 1 RUST
gerjan 7:5380d17310e8 147 if (state == 1) {
gerjan 7:5380d17310e8 148
gerjan 7:5380d17310e8 149 pc.printf("state 1 RUST | knop1 = cal_motor knop2 = MENU EMG knop3 = Tekenen \n\r");
gerjan 7:5380d17310e8 150
gerjan 7:5380d17310e8 151
gerjan 7:5380d17310e8 152 if (knop1 == false ) { //als knop 1 is ingedrukt
gerjan 7:5380d17310e8 153 state=2;
gerjan 7:5380d17310e8 154 wait(0.05); //wacht goed contact knopje
gerjan 7:5380d17310e8 155
gerjan 7:5380d17310e8 156 while(knop1 == false) {} //wacht tot knop is losgelaten
gerjan 7:5380d17310e8 157 wait(0.05); // wacht goed contact knopje
gerjan 7:5380d17310e8 158
gerjan 7:5380d17310e8 159 }
gerjan 7:5380d17310e8 160
gerjan 7:5380d17310e8 161 if (knop2 == false ) { //als knop 2 wordt ingedrukt: state is 3
gerjan 7:5380d17310e8 162 state=3;
gerjan 7:5380d17310e8 163 wait(0.05);
gerjan 7:5380d17310e8 164
gerjan 7:5380d17310e8 165 while(knop2 == false) {}
gerjan 7:5380d17310e8 166 wait(0.05);
gerjan 7:5380d17310e8 167 }
gerjan 7:5380d17310e8 168 if (knop3 == false && calibratie_rust == true && calibratie_max == true && calibratie_motor == true) {
gerjan 7:5380d17310e8 169 state=6; //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn.
gerjan 7:5380d17310e8 170 wait(0.05);
gerjan 7:5380d17310e8 171
gerjan 7:5380d17310e8 172 while(knop3 == false) {}
gerjan 7:5380d17310e8 173 wait(0.05);
gerjan 7:5380d17310e8 174 meting=true;
gerjan 7:5380d17310e8 175
gerjan 7:5380d17310e8 176 }
gerjan 7:5380d17310e8 177 if (knop3 == false && state !=6) {
gerjan 7:5380d17310e8 178 wait(0.05);
gerjan 7:5380d17310e8 179 pc.printf("Niet alle calibraties uitgevoerd! \n");
gerjan 7:5380d17310e8 180 wait(1);
gerjan 7:5380d17310e8 181 }
gerjan 7:5380d17310e8 182
gerjan 7:5380d17310e8 183
gerjan 7:5380d17310e8 184 }
gerjan 7:5380d17310e8 185
gerjan 7:5380d17310e8 186
gerjan 7:5380d17310e8 187 //state2 CAL MOTOR
gerjan 7:5380d17310e8 188 if (state == 2) {
gerjan 7:5380d17310e8 189 pc.printf("state 2 cal_motor | knop1 = terug naar rust \n\r");
gerjan 7:5380d17310e8 190
gerjan 7:5380d17310e8 191 //Als de robot arm met pen met de hand op de begin posisie gezet is moet knop 1 weer worden ingedrukt.
gerjan 7:5380d17310e8 192 if (knop1 == false ) {
gerjan 7:5380d17310e8 193 state=1;
gerjan 7:5380d17310e8 194 wait(0.05);
gerjan 7:5380d17310e8 195
gerjan 7:5380d17310e8 196 while(knop1 == false) {}
gerjan 7:5380d17310e8 197 wait(0.05);
gerjan 7:5380d17310e8 198
gerjan 7:5380d17310e8 199 motor1.setPosition(200.0); //Zeg dat de motor die de arm verplaatst 200 counts heeft, dit komt overeen met (200/CPR)/gearatio = (200/CPR)/50=0.125 dat komt overeen met een hoek van pi/4
gerjan 7:5380d17310e8 200 motor2.setPosition(597.15); //Zeg dat de motor die de loper verplaatst 597.15 counts heeft, dit komt overeen met r=98,71 mm
gerjan 7:5380d17310e8 201
gerjan 7:5380d17310e8 202 x=0; //Zeg dat de pen op x=0 staat
gerjan 7:5380d17310e8 203 y=0; //Zeg dat de pen op y=0 staat
gerjan 7:5380d17310e8 204
gerjan 7:5380d17310e8 205 dri_1=0; //vorige integraal van de fout, 0 voor de eerste keer.
gerjan 7:5380d17310e8 206
gerjan 7:5380d17310e8 207 calibratie_motor = true; //zeg dat de calibratie van de motor is uitgevoerd.
gerjan 7:5380d17310e8 208
gerjan 7:5380d17310e8 209
gerjan 7:5380d17310e8 210 }
gerjan 7:5380d17310e8 211 }
gerjan 7:5380d17310e8 212 // state 3 MENU EMG
gerjan 7:5380d17310e8 213 if (state == 3) {
gerjan 7:5380d17310e8 214 pc.printf("state 3 MENU EMG | knop1= cal EMG rust knop2= rust knop3= cal EMG max \n\r");
gerjan 7:5380d17310e8 215
gerjan 7:5380d17310e8 216
gerjan 7:5380d17310e8 217 if (knop2 == false ) { // terug naar state 1
gerjan 7:5380d17310e8 218 state=1;
gerjan 7:5380d17310e8 219 wait(0.05);
gerjan 7:5380d17310e8 220
gerjan 7:5380d17310e8 221 while(knop2 == false) {}
gerjan 7:5380d17310e8 222 wait(0.05);
gerjan 7:5380d17310e8 223 }
gerjan 7:5380d17310e8 224
gerjan 7:5380d17310e8 225 if (knop1 == false ) { // naar state 4
gerjan 7:5380d17310e8 226 state=4;
gerjan 7:5380d17310e8 227 wait(0.05);
gerjan 7:5380d17310e8 228
gerjan 7:5380d17310e8 229 while(knop1 == false) {}
gerjan 7:5380d17310e8 230 wait(0.05);
gerjan 7:5380d17310e8 231
gerjan 7:5380d17310e8 232 // begingrenswaarden stellen:
gerjan 7:5380d17310e8 233 GrenswaardeV=0;
gerjan 7:5380d17310e8 234 GrenswaardeD=0;
gerjan 7:5380d17310e8 235 meting=true; //Zet de meting 'aan'
gerjan 7:5380d17310e8 236 }
gerjan 7:5380d17310e8 237
gerjan 7:5380d17310e8 238 if (knop3 == false ) { // naar state 5
gerjan 7:5380d17310e8 239 state=5;
gerjan 7:5380d17310e8 240 wait(0.05);
gerjan 7:5380d17310e8 241
gerjan 7:5380d17310e8 242 while(knop3 == false) {}
gerjan 7:5380d17310e8 243 wait(0.05);
gerjan 7:5380d17310e8 244
gerjan 7:5380d17310e8 245 // begingrenswaarden stellen:
gerjan 7:5380d17310e8 246 MaxwaardeV=0;
gerjan 7:5380d17310e8 247 MaxwaardeD=0;
gerjan 7:5380d17310e8 248 meting=true;
gerjan 7:5380d17310e8 249
gerjan 7:5380d17310e8 250 }
gerjan 7:5380d17310e8 251
gerjan 7:5380d17310e8 252
gerjan 7:5380d17310e8 253 }
gerjan 7:5380d17310e8 254
gerjan 7:5380d17310e8 255
gerjan 7:5380d17310e8 256 //METEN VAN EMG
gerjan 7:5380d17310e8 257 if (meting == true) {
gerjan 7:5380d17310e8 258 // pc.printf("TEST CODE: Meting in gang\n");
gerjan 7:5380d17310e8 259 yy = emg1.read(); //<measurement function, read from pin>
gerjan 7:5380d17310e8 260 byy= emg2.read();
gerjan 7:5380d17310e8 261
gerjan 7:5380d17310e8 262 z=-f1*z1-f2*z2+e1*yy+e2*y1+e3*y2; /*hoogdoorlaatfilteroverdacht*/
gerjan 7:5380d17310e8 263 bz=-bf1*bz1-bf2*bz2+be1*byy+be2*by1+be3*by2; /*hoogdoorlaatfilteroverdacht van biceps*/
gerjan 7:5380d17310e8 264
gerjan 7:5380d17310e8 265 y2 = y1; /*y(n-1) will be y(n-2) on next loop*/
gerjan 7:5380d17310e8 266 y1= yy; /*y(n) will be y(n-1) on next loop*/
gerjan 7:5380d17310e8 267 z2 = z1; /*z(n-1) will be z(n-2) on next loop*/
gerjan 7:5380d17310e8 268 z1 = z; /*z(n) will be z(n-1) on next loop*/
gerjan 7:5380d17310e8 269
gerjan 7:5380d17310e8 270 by2 = by1; /*y(n-1) will be y(n-2) on next loop*/
gerjan 7:5380d17310e8 271 by1= byy; /*y(n) will be y(n-1) on next loop*/
gerjan 7:5380d17310e8 272 bz2 = bz1; /*z(n-1) will be z(n-2) on next loop*/
gerjan 7:5380d17310e8 273 bz1 = bz; /*z(n) will be z(n-1) on next loop*/
gerjan 7:5380d17310e8 274
gerjan 7:5380d17310e8 275 zabs=abs(z);/*rectify*/
gerjan 7:5380d17310e8 276 bzabs=abs(bz);/*rectify*/
gerjan 7:5380d17310e8 277
gerjan 7:5380d17310e8 278 w=-h1*w1-h2*w2+g1*zabs+g2*zabs1+g3*zabs2; /*laagdoorlaatfilteroverdracht*/
gerjan 7:5380d17310e8 279 bw=-bh1*bw1-bh2*bw2+bg1*bzabs+bg2*bzabs1+bg3*bzabs2; /*laagdoorlaatfilteroverdracht van biceps*/
gerjan 7:5380d17310e8 280
gerjan 7:5380d17310e8 281 zabs2 = zabs1; /*z(n-1) will be z(n-2) on next loop*/
gerjan 7:5380d17310e8 282 zabs1 = zabs; /*z(n) will be z(n-1) on next loop*/
gerjan 7:5380d17310e8 283 w2 = w1; /*w(n-1) will be w(n-2) on next loop*/
gerjan 7:5380d17310e8 284 w1 = w; /*w(n) will be w(n-1) on next loop*/
gerjan 7:5380d17310e8 285
gerjan 7:5380d17310e8 286 bzabs2 = bzabs1; /*z(n-1) will be z(n-2) on next loop*/
gerjan 7:5380d17310e8 287 bzabs1 = bzabs; /*z(n) will be z(n-1) on next loop*/
gerjan 7:5380d17310e8 288 bw2 = bw1; /*w(n-1) will be w(n-2) on next loop*/
gerjan 7:5380d17310e8 289 bw1 = bw; /*w(n) will be w(n-1) on next loop*/
gerjan 7:5380d17310e8 290
gerjan 7:5380d17310e8 291 //pc.printf("%f,%f\n",w,bw);
gerjan 7:5380d17310e8 292
gerjan 7:5380d17310e8 293
gerjan 7:5380d17310e8 294 if (w>1.0) {
gerjan 7:5380d17310e8 295 w=1.0;
gerjan 7:5380d17310e8 296 }
gerjan 7:5380d17310e8 297 if (bw>1.0) {
gerjan 7:5380d17310e8 298 bw=1.0;
gerjan 7:5380d17310e8 299 }
gerjan 7:5380d17310e8 300
gerjan 7:5380d17310e8 301 }
gerjan 7:5380d17310e8 302
gerjan 7:5380d17310e8 303
gerjan 7:5380d17310e8 304
gerjan 7:5380d17310e8 305 // state 4 EMG CAL IN RUST
gerjan 7:5380d17310e8 306 if (state == 4) {
gerjan 7:5380d17310e8 307 pc.printf("state 4 cal EMG RUST | knop1= MEMNU EMG \n\r");
gerjan 7:5380d17310e8 308 pc.printf("%f,%f\n",GrenswaardeV,GrenswaardeD);
gerjan 7:5380d17310e8 309
gerjan 7:5380d17310e8 310 // Nieuwe grenswaarden bepalen
gerjan 7:5380d17310e8 311 if (w > GrenswaardeV) {
gerjan 7:5380d17310e8 312 GrenswaardeV = w;
gerjan 7:5380d17310e8 313 }
gerjan 7:5380d17310e8 314 if (bw > GrenswaardeD) {
gerjan 7:5380d17310e8 315 GrenswaardeD = bw;
gerjan 7:5380d17310e8 316 }
gerjan 7:5380d17310e8 317 if (knop1 == false ) { // terug naar state 1
gerjan 7:5380d17310e8 318 state=3;
gerjan 7:5380d17310e8 319 wait(0.05);
gerjan 7:5380d17310e8 320
gerjan 7:5380d17310e8 321 while(knop1 == false) {}
gerjan 7:5380d17310e8 322 wait(0.05);
gerjan 7:5380d17310e8 323 calibratie_rust = true; //Zeg dat de calibratie van de emg in rust in uitgevoerd.
gerjan 7:5380d17310e8 324 meting=false; //Zet de meting weer 'uit'
gerjan 7:5380d17310e8 325
gerjan 7:5380d17310e8 326 }
gerjan 7:5380d17310e8 327 }
gerjan 7:5380d17310e8 328 // state 5 EMG CAL MAX
gerjan 7:5380d17310e8 329 if (state == 5) {
gerjan 7:5380d17310e8 330 pc.printf("state 5 cal EMG MAX | knop3= EMG MENU \n\r");
gerjan 7:5380d17310e8 331 pc.printf("%f,%f\n",MaxwaardeV,MaxwaardeD);
gerjan 7:5380d17310e8 332 // Nieuwe maxwaarden bepalen
gerjan 7:5380d17310e8 333 if (w > MaxwaardeV) {
gerjan 7:5380d17310e8 334 MaxwaardeV = w;
gerjan 7:5380d17310e8 335 }
gerjan 7:5380d17310e8 336 if (bw > MaxwaardeD) {
gerjan 7:5380d17310e8 337 MaxwaardeD = bw;
gerjan 7:5380d17310e8 338 }
gerjan 7:5380d17310e8 339
gerjan 7:5380d17310e8 340 // einde maxwaarde bepalen:
gerjan 7:5380d17310e8 341 if (knop3 == false ) { // terug naar state 3
gerjan 7:5380d17310e8 342 state=3;
gerjan 7:5380d17310e8 343 wait(0.05);
gerjan 7:5380d17310e8 344
gerjan 7:5380d17310e8 345 while(knop3 == false) {}
gerjan 7:5380d17310e8 346 wait(0.05);
gerjan 7:5380d17310e8 347 calibratie_max = true;
gerjan 7:5380d17310e8 348 meting=false;
gerjan 7:5380d17310e8 349
gerjan 7:5380d17310e8 350 }
gerjan 7:5380d17310e8 351
gerjan 7:5380d17310e8 352 }
gerjan 7:5380d17310e8 353
gerjan 7:5380d17310e8 354 // state 6 TEKENEN
gerjan 7:5380d17310e8 355
gerjan 7:5380d17310e8 356 if (state == 6) {
gerjan 7:5380d17310e8 357 // pc.printf("state 6 tekenen | knop3= rust \n\r");
gerjan 7:5380d17310e8 358
gerjan 7:5380d17310e8 359 // EMG
gerjan 7:5380d17310e8 360 BereikD = MaxwaardeD-GrenswaardeD; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
gerjan 7:5380d17310e8 361 BereikV = MaxwaardeV-GrenswaardeV; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
gerjan 7:5380d17310e8 362
gerjan 7:5380d17310e8 363 SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV); // Snelheid van pen (in cm/sec), lineair verband met het Bereik
gerjan 7:5380d17310e8 364 SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
gerjan 7:5380d17310e8 365
gerjan 7:5380d17310e8 366 if (SnelheidV<0) {
gerjan 7:5380d17310e8 367 SnelheidV=0;
gerjan 7:5380d17310e8 368 }
gerjan 7:5380d17310e8 369 if (SnelheidD<0) {
gerjan 7:5380d17310e8 370 SnelheidD=0;
gerjan 7:5380d17310e8 371 }
gerjan 7:5380d17310e8 372 if (SnelheidV>MaxsnelheidV) {
gerjan 7:5380d17310e8 373 SnelheidV=MaxsnelheidV;
gerjan 7:5380d17310e8 374 }
gerjan 7:5380d17310e8 375 if (SnelheidD>MaxsnelheidV) {
gerjan 7:5380d17310e8 376 SnelheidD=MaxsnelheidV;
gerjan 7:5380d17310e8 377 }
gerjan 7:5380d17310e8 378
gerjan 7:5380d17310e8 379 deltas = SnelheidV*Ts; //afstand tussen twee meetpunten
gerjan 7:5380d17310e8 380 deltaq = SnelheidD*Ts; //verandering hoek tov. de rechtdoor
gerjan 7:5380d17310e8 381 q = deltaq+qbegin; // hoekverandering tov. het assenstelsel
gerjan 7:5380d17310e8 382
gerjan 7:5380d17310e8 383 deltay = deltas*cos(q); //veranding van pen in de y-richting
gerjan 7:5380d17310e8 384 deltax = deltas*sin(q); // verandering van pen in de x-richting
gerjan 7:5380d17310e8 385
gerjan 7:5380d17310e8 386 x = xbegin+deltax; // nieuwe postie pen, x-coördinaat
gerjan 7:5380d17310e8 387 y = ybegin+deltay; // nieuwe positie pen, y-coöridnaat
gerjan 7:5380d17310e8 388
gerjan 7:5380d17310e8 389 if (x>297) { //grenzen A4
gerjan 7:5380d17310e8 390 x=297;
gerjan 7:5380d17310e8 391 }
gerjan 7:5380d17310e8 392 if (y>210) {
gerjan 7:5380d17310e8 393 y=210;
gerjan 7:5380d17310e8 394 }
gerjan 7:5380d17310e8 395
gerjan 7:5380d17310e8 396 if (x<0) { //grenzen A4
gerjan 7:5380d17310e8 397 x=0;
gerjan 7:5380d17310e8 398 }
gerjan 7:5380d17310e8 399 if (y<0) {
gerjan 7:5380d17310e8 400 y=0;
gerjan 7:5380d17310e8 401 }
gerjan 7:5380d17310e8 402
gerjan 7:5380d17310e8 403 xbegin=x; // x zal xbegin worden in de volgende loop oftewel:(x(n) will be x(n-1) on next loop)
gerjan 7:5380d17310e8 404 ybegin=y; // y zal ybgin worden in de volgende loop oftewel:(y(n) will be y(n-1) on next loop)
gerjan 7:5380d17310e8 405 qbegin=q; // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)
gerjan 7:5380d17310e8 406
gerjan 7:5380d17310e8 407 pc.printf("bV=%f, bD=%f, sV=%f, sD=%f, x=%f, y=%f, hoek=%f\n",BereikV,BereikD,SnelheidV,SnelheidD,x,y,(q*57.3));
gerjan 7:5380d17310e8 408
gerjan 7:5380d17310e8 409
gerjan 7:5380d17310e8 410 // motoren
gerjan 7:5380d17310e8 411
gerjan 7:5380d17310e8 412 if (y>210.0) {
gerjan 7:5380d17310e8 413 y=210.0;
gerjan 7:5380d17310e8 414 }
gerjan 7:5380d17310e8 415 if(y<=0.0) {
gerjan 7:5380d17310e8 416 y=0.0;
gerjan 7:5380d17310e8 417 }
gerjan 7:5380d17310e8 418 if(x>297.0) {
gerjan 7:5380d17310e8 419 x=297.0;
gerjan 7:5380d17310e8 420 }
gerjan 7:5380d17310e8 421 if(x<=0.0) {
gerjan 7:5380d17310e8 422 x=0.0;
gerjan 7:5380d17310e8 423 }
gerjan 7:5380d17310e8 424
gerjan 7:5380d17310e8 425
gerjan 7:5380d17310e8 426 //x en y omzetten in hoek en straal.
gerjan 7:5380d17310e8 427 theta = atan((x+69.80)/(y+69.80));
gerjan 7:5380d17310e8 428 r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) );
gerjan 7:5380d17310e8 429
gerjan 7:5380d17310e8 430 //lees de hoek en de straal af uit de motoren.
gerjan 7:5380d17310e8 431 theta_pen = motor1.getPosition() * ((.5*pi)/400.0); //Zet de CPR om in een hoek.
gerjan 7:5380d17310e8 432 r_pen = motor2.getPosition() * (461.34/2790.13); //zet de CPRM om in een straaal van de pen.
gerjan 7:5380d17310e8 433
gerjan 7:5380d17310e8 434 // Berekende het verschil in gewensete en werkelijk hoek en straal.
gerjan 7:5380d17310e8 435 dtheta = (theta - theta_pen);
gerjan 7:5380d17310e8 436 dr = (r - r_pen);
gerjan 7:5380d17310e8 437
gerjan 7:5380d17310e8 438 //REGELAAR
gerjan 7:5380d17310e8 439 up_r = kp_r * dr; //P-actie
gerjan 7:5380d17310e8 440 dri = dri_1 + dr*Ts;
gerjan 7:5380d17310e8 441 ui_r = dri_1 * ki_r; //I-actie
gerjan 7:5380d17310e8 442 utot_r = up_r + ui_r; //
gerjan 7:5380d17310e8 443
gerjan 7:5380d17310e8 444 dri_1 = dri; //nieuwe waardes oud maken.
gerjan 7:5380d17310e8 445
gerjan 7:5380d17310e8 446 theta_pwm = (dtheta)*3.0;
gerjan 7:5380d17310e8 447 r_pwm = (utot_r/1.0);
gerjan 7:5380d17310e8 448
gerjan 7:5380d17310e8 449 //NAAR MOTOR
gerjan 7:5380d17310e8 450
gerjan 7:5380d17310e8 451 //Zorgen dat pwm tussen -1 en 1 blijft.
gerjan 7:5380d17310e8 452 if(theta_pwm > 1) {
gerjan 7:5380d17310e8 453 theta_pwm=1;
gerjan 7:5380d17310e8 454 }
gerjan 7:5380d17310e8 455 if(theta_pwm < -1) {
gerjan 7:5380d17310e8 456 theta_pwm=-1;
gerjan 7:5380d17310e8 457 }
gerjan 7:5380d17310e8 458 if(r_pwm > 1) {
gerjan 7:5380d17310e8 459 r_pwm=1;
gerjan 7:5380d17310e8 460 }
gerjan 7:5380d17310e8 461 if(r_pwm < -1) {
gerjan 7:5380d17310e8 462 r_pwm=-1;
gerjan 7:5380d17310e8 463 }
gerjan 7:5380d17310e8 464
gerjan 7:5380d17310e8 465 // Bepaal richting waarin motoren moeten draaien
gerjan 7:5380d17310e8 466 if(theta_pwm > 0)
gerjan 7:5380d17310e8 467 motor1dir.write(1);
gerjan 7:5380d17310e8 468 else
gerjan 7:5380d17310e8 469 motor1dir.write(0);
gerjan 7:5380d17310e8 470 if(r_pwm > 0)
gerjan 7:5380d17310e8 471 motor2dir.write(1);
gerjan 7:5380d17310e8 472 else
gerjan 7:5380d17310e8 473 motor2dir.write(0);
gerjan 7:5380d17310e8 474
gerjan 7:5380d17310e8 475 // Testcode - print naar pc
gerjan 7:5380d17310e8 476 //pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f inputsin=%0.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm, inputsinus);
gerjan 7:5380d17310e8 477
gerjan 7:5380d17310e8 478 //schrijf PWM naar motor
gerjan 7:5380d17310e8 479 pwm_motor1.write(abs(theta_pwm));
gerjan 7:5380d17310e8 480 pwm_motor2.write(abs(r_pwm));
gerjan 7:5380d17310e8 481
gerjan 7:5380d17310e8 482 // terug naar state 1 knop
gerjan 7:5380d17310e8 483 if (knop3 == false ) { // terug naar state 1
gerjan 7:5380d17310e8 484 state=1;
gerjan 7:5380d17310e8 485 meting=false;
gerjan 7:5380d17310e8 486 wait(0.05);
gerjan 7:5380d17310e8 487
gerjan 7:5380d17310e8 488 while(knop3 == false) {}
gerjan 7:5380d17310e8 489 wait(0.05);
gerjan 7:5380d17310e8 490 }
gerjan 7:5380d17310e8 491
gerjan 7:5380d17310e8 492 }
gerjan 7:5380d17310e8 493
gerjan 7:5380d17310e8 494 } //sluiten oneindige while loop
gerjan 7:5380d17310e8 495
gerjan 7:5380d17310e8 496 } //afsluiten main