Diff: main.cpp

Revision:

2:3f849fd62ebb

Parent:

1:b9595e136a00

Child:

3:f87769ba4a9b

--- a/main.cpp	Mon Oct 29 16:07:09 2018 +0000
+++ b/main.cpp	Tue Oct 30 08:57:51 2018 +0000
@@ -16,14 +16,20 @@
 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //X4 encoding
 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //X4 encoding
 
+// motor1 is motor voor rotatie
+// motor2 is motor voor translatie 
+
 double tijd = 0.00006; // sec
 double delta_t = 17*tijd;
 const double pi = 3.14159265358979323846; // constant pi
 volatile double positie_x;
 volatile double positie_y;
-volatile double delta_y; // verandering in y positie
-volatile double dc2; // gewenste counts2 verandering
-volatile double dc1; // gewenste counts1 verandering
+volatile double delta_y; // verandering in y positie, nodig door x-verandering
+volatile double delta_x; // verandering in x positie, nodig door y-verandering
+volatile double dc2_x; // gewenste counts2 verandering in x-rii
+volatile double dc1_x; // gewenste counts1 verandering in x-rii
+volatile double dc2_y; // gewenste counts2 verandering in y-rii
+volatile double dc1_y; // gewenste counts1 verandering in y-rii
 volatile double A; // matrix a is de positie van de pen tov het midden
 volatile double counts_per_round = 8400;
 volatile double gamma = (2*pi)/(25*counts_per_round); // gamma is de afstand afgelegd per count door het grote tandwiel
@@ -65,20 +71,26 @@
     pc.printf("counts1=%i counts2=%i ", counts1,counts2);
     
     // verwerking potmeter output tot richtingen motor
-    positie_x = out_2*delta_t;
-    positie_y = out_4*delta_t;
+    positie_x = out_2*20.0*delta_t; // de max is 20 mm/s
+    positie_y = out_4*20.0*delta_t;
     theta = gamma*counts2;
     Lpc_x = (r-(alpha*counts1+l_beta))*sin(theta);
     Lpc_y = (r-(alpha*counts1+l_beta))*cos(theta);
     A = sqrt(pow(Lpc_x,2)+pow(Lpc_y,2));
     
-    dc2 = (asin(positie_x/A)/gamma)-counts2;
-    delta_y = A*cos(gamma*dc2+theta);
-    dc1 = ((r-delta_y)/cos(theta)-l_beta)/alpha;
+    // x-richting 
+    dc2_x = (asin(positie_x/A)/gamma)-counts2;
+    delta_y = A*cos(gamma*dc2_x+theta);
+    dc1_x = (r-(delta_y/cos(theta))-l_beta)/alpha;
+        
+    // y-richting 
+    dc2_y = (acos(delta_y/A)/gamma)-counts2;
+    delta_x = A*sin(gamma*dc1_y+theta);
+    dc1_y = (r-(delta_x/cos(theta))-l_beta)/alpha;
     
     // van delta counts naar pwm
-    speed_desired_motor1 = (dc1*alpha)/delta_t; // voor max 2cm/s translatie -> range pwm: -1/pi - 1/pi
-    speed_desired_motor2 = (dc2*gamma)/delta_t; // voor max rotatie -> range pwm: -2/4,65 - 2/4,65
+    speed_desired_motor1 = ((dc1_x+dc1_y)*alpha)/delta_t; // voor max 2cm/s translatie -> range pwm: -1/pi - 1/pi
+    speed_desired_motor2 = ((dc2_x+dc2_y)*gamma)/delta_t; // voor max rotatie -> range pwm: -2/4,65 - 2/4,65
     max_speed_motor2 = 46.5; // moet nog berekening in
     
     pc.printf("speed1=%f speed2=%f ", speed_desired_motor1,speed_desired_motor2);
@@ -86,7 +98,7 @@
     double pwm1 = speed_desired_motor1/(pi); // 20.0 mm/s
     double pwm2 = (speed_desired_motor2*2)/(max_speed_motor2);
     
-    pc.printf("pwm1=%f pwm2=%f\r\n",pwm1,pwm2);
+    pc.printf("pwm1=%f pwm2=%f\r\n",pwm1,pwm2);    
         
     // zeg richtingen aan motor
     dirpin.write(pwm2 < 0);  
@@ -94,7 +106,6 @@
     dirpin_2.write(pwm1 < 0);
     pwmpin_2 = fabs (pwm1); 
  
-    
     wait(0.01); // aantal keer dat een signaal wordt gecheckt en doorgestuurd    
     } 
 }