Diff: main.cpp

Revision:

1:907507671a38

Parent:

0:2a4ed6c6cdc7

--- a/main.cpp	Wed Oct 31 17:49:26 2018 +0000
+++ b/main.cpp	Wed Oct 31 19:13:22 2018 +0000
@@ -15,16 +15,14 @@
 DigitalOut motor2direction(D4);
 PwmOut motor2control(D5);
 
-InterruptIn button1(D10);
-
 Serial pc(USBTX, USBRX);
 
 // 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.30;
+double K_p2 = 0.30;
+double p_desired_x = 0.15;
+double p_desired_y = 0.05;
 double r_pulley = 0.015915;
 double pi = 3.141592653589793;
 double gearratio = 3.857142857;
@@ -56,14 +54,14 @@
     //p_dot_x = v_constant*(p_desired_x - p_current_x);
     //p_dot_y = v_constant*(p_desired_y - p_current_y);
     
-    angle_desired = atan2(p_desired_y,p_desired_x);
-    length_desired = sqrt(pow(p_desired_x,2)+pow(p_desired_y,2));
+    angle_desired = atan2(p_desired_y,p_desired_x+L0);
+    length_desired = sqrt(pow(p_desired_x+L0,2)+pow(p_desired_y,2));
 
     motor1_pwm = K_p1*(length_desired-length)/r_pulley;
     motor2_pwm = K_p2*(angle_desired-angle);
         
-    motor1control.write(fabs(motor1_pwm));
-    motor2control.write(fabs(motor2_pwm));
+    motor1control.write((motor1_pwm));
+    motor2control.write((motor2_pwm));
     //motor1direction = motor1_pwm < 0;
     //motor2direction = motor2_pwm < 0;
  }
@@ -81,6 +79,6 @@
     wait(0.1);
     pc.printf("angle = %f, length = %f \r\n", angle, 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);
+    pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n\r\n", motor1_pwm, motor2_pwm);
     }
 }
\ No newline at end of file