Diff: begintotaalscript.cpp

Revision:

3:a3e4580af271

Parent:

2:103de31c8803

Child:

4:c79922420b70

diff -r 103de31c8803 -r a3e4580af271 begintotaalscript.cpp
--- a/begintotaalscript.cpp	Mon Nov 04 21:51:21 2013 +0000
+++ b/begintotaalscript.cpp	Tue Nov 05 08:42:42 2013 +0000
@@ -49,12 +49,12 @@
     //Variabelen EMG/motoren
     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;
     double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;
-    
+
 // Variabelen benoemen voor regelaar motor.
     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;
     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;
     double motor1_maxu, motor2_maxu;
-    
+
 
     pi=3.14159265359;
 
@@ -105,13 +105,13 @@
 //Constanten voor tekening: EMGdeel
     xbegin= 0; //beginpositie
     ybegin=0; //beginpositie
-    qbegin=315;  //beginhoek (in graden) vanaf de y-as, rechtsom gedraaid is positief
-    MaxsnelheidV=15;     //in cm/s
-    MaxsnelheidD=15;    //in graden/s
+    qbegin=5.50;  //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
+    MaxsnelheidV=150.0;     //in mm/s
+    MaxsnelheidD=0.26;    //in rad/s
 //constanten regelaar
     kp_r = 0.006;
     ki_r = 0.005;
-
+    
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,Ts);
 
@@ -180,7 +180,7 @@
                 x=0;
                 y=0;
 
-                dri=0;
+                dri_1=0;
 
                 calibratie_motor = true;
 
@@ -339,7 +339,7 @@
             //  EMG
             BereikD = MaxwaardeD-GrenswaardeD;    //Bereik van grafiek EMG (Voltage t.o.v. tijd)
             BereikV = MaxwaardeV-GrenswaardeV;      //Bereik van grafiek EMG (Voltage t.o.v. tijd)
-
+                       
             SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV);  // Snelheid van pen (in cm/sec), lineair verband met het Bereik
             SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
 
@@ -369,11 +369,11 @@
             x = xbegin+deltax;       // nieuwe postie pen, x-coördinaat
             y = ybegin+deltay;       // nieuwe positie pen, y-coöridnaat
 
-            if (x>29.7) {       //grenzen A4
-                x=29.7;
+            if (x>297) {       //grenzen A4
+                x=297;
             }
-            if (y>21.0) {
-                y=21.0;
+            if (y>210) {
+                y=210;
             }
 
             if (x<0) {       //grenzen A4
@@ -387,7 +387,7 @@
             ybegin=y;    // y zal ybgin worden in de volgende loop  oftewel:(y(n) will be y(n-1) on next loop)
             qbegin=q;   // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)
 
-            pc.printf("%f,%f,%f,%f\n",SnelheidV,SnelheidD,x,y);
+            pc.printf("bV=%f, bD=%f, sV=%f, sD=%f, x=%f, y=%f, hoek=%f\n",BereikV,BereikD,SnelheidV,SnelheidD,x,y,(d*57.3));
 
 
 // motoren
@@ -458,7 +458,7 @@
                 motor2dir.write(0);
 
             // print naar pc
-            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);
+            //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);
 
             //schrijf PWM naar motor
             pwm_motor1.write(abs(theta_pwm));
@@ -467,6 +467,7 @@
             // terug naar state 1 knop
             if (knop3 == false ) {  // terug naar state 1
                 state=1;
+                
                 wait(0.05);
 
                 while(knop3 == false) {}