Diff: main.cpp

Revision:

2:a8ee608177ae

Parent:

0:2a4ed6c6cdc7

Child:

3:f70ec68723df

--- a/main.cpp	Wed Oct 31 17:49:26 2018 +0000
+++ b/main.cpp	Thu Nov 01 10:58:42 2018 +0000
@@ -21,10 +21,10 @@
 
 // Definitie constanten
 double L0 = 0.09;
-double K_p1 = 0.02;
-double K_p2 = 0.20;
-double p_desired_x = 0.2;
-double p_desired_y = 0.2;
+double K_p1 = 0.5;
+double K_p2 = 0.5;
+double p_desired_x = 0.04;
+double p_desired_y = 0.13;
 double r_pulley = 0.015915;
 double pi = 3.141592653589793;
 double gearratio = 3.857142857;
@@ -39,6 +39,8 @@
 double motor1_pwm;
 double length_dot;
 double motor2_pwm;
+double error_length_angle;
+double error_angle;
 
 double p_current_x;
 double p_current_y;
@@ -50,27 +52,42 @@
     angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio;                        // Angle arm [rad]
     length = translatie+L0;
 
-    p_current_x = (length)*cos(angle);
+    p_current_x = (length)*cos(angle)-L0;
     p_current_y = (length)*sin(angle);
     
-    //p_dot_x = v_constant*(p_desired_x - p_current_x);
-    //p_dot_y = v_constant*(p_desired_y - p_current_y);
+    //p_dot_x = K_p1*(p_desired_x - p_current_x);
+    //p_dot_y = K_p2*(p_desired_y - p_current_y);
+    
+    angle_desired = atan2(p_desired_y,p_desired_x+L0);
+    length_desired = sqrt(pow(p_desired_x+L0,2)+pow(p_desired_y,2));
+
+    error_length_angle = (length_desired-length)/r_pulley;
+    error_angle = angle_desired-angle;
     
-    angle_desired = atan2(p_desired_y,p_desired_x);
-    length_desired = sqrt(pow(p_desired_x,2)+pow(p_desired_y,2));
-
-    motor1_pwm = K_p1*(length_desired-length)/r_pulley;
-    motor2_pwm = K_p2*(angle_desired-angle);
+    motor1_pwm = K_p1*error_length_angle;
+    motor2_pwm = K_p2*error_angle;
+    
+    //Motor 1 (Translatie)
+    if (error_length_angle >= 0) {
+        motor1direction = false; //Positieve bewegingsrichting (clockwise, towards end)
+        }
+    else {
+        motor1direction = true; // Negatieve bewegingsrichting
+        }
+     motor1control.write(fabs(motor1_pwm));
         
-    motor1control.write(fabs(motor1_pwm));
+    //Motor 2 (Rotatie)
+    if (error_angle >= 0){
+        motor2direction = false; // counterclockwise (arm clockwise)
+        }
+    else {
+        motor2direction = true; // clockwise (arm counterclockwise)
+        }
     motor2control.write(fabs(motor2_pwm));
-    //motor1direction = motor1_pwm < 0;
-    //motor2direction = motor2_pwm < 0;
- }
+}    
 
-
-int main()
-{
+int main() {
+    
 EncoderTicker.attach(&EncoderFunc, 0.02);
 pc.baud(115200);
 motor1direction = false;
@@ -79,7 +96,7 @@
 while(true)
     {
     wait(0.1);
-    pc.printf("angle = %f, length = %f \r\n", angle, length);
+    pc.printf("angle = %f, length = %f \r\n", angle*180/pi, length);
     pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
     pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n", motor1_pwm, motor2_pwm);
     }