Diff: main.cpp

Revision:

21:302d9043cb4b

Parent:

20:4371ed979fbf

Child:

22:af49b9e79d9c

--- a/main.cpp	Fri Mar 13 15:00:35 2020 +0000
+++ b/main.cpp	Fri Mar 13 17:35:19 2020 +0000
@@ -304,23 +304,25 @@
 
         
         
-        position_error = target_position - current_position; 
-        previous_position_error = position_error; 
-        derivative_position = position_error - previous_position_error; 
+        position_error = target_position - (current_position/6); 
     
         float velocity_out = get_velocity_out(position_error);
+        float position_out = (6*position_error) +  (40*derivative_position); 
+        derivative_position = position_error - previous_position_error; 
+        previous_position_error = position_error; 
+
         float motor_out = get_motor_out(velocity_out);      
 
 
 
 
-        update_lead(velocity_out);
+        update_lead(position_out);
                         
         MotorPWM.period(pwm_period);
         MotorPWM.write(motor_out);
 
             if(i > 10) {
-                pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", velocity, current_position, motor_out, velocity_out, lead);
+                pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", velocity, (current_position/6), motor_out, derivative_position, lead);
                 i = 0;
             }
         i++;