Gerjan Wolterink / Mbed 2 deprecated Broncode_robot_groep2

Broncode projectgroep 2 Belinda Brandwacht s1226290 Esther Keulers s1255444 Maaike Schotman s1274104 Gerjan Wolterink s1197797 Roelof van Zwol s1240811

Dependencies:   Encoder MODSERIAL mbed

Fork of Begintotaalscript by Esther Keulers

begintotaalscript.cpp@6:d4f355d72f66, 2013-11-06 (annotated)

Committer:
gerjan
Date:
Wed Nov 06 16:37:36 2013 +0000
Revision:
6:d4f355d72f66
Parent:
5:4adfd729a291 
net begonnen met typen extra commentaar

Who changed what in which revision?

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