Diff: main.cpp

Revision:

20:4371ed979fbf

Parent:

19:e95f6004f365

Child:

21:302d9043cb4b

--- a/main.cpp	Fri Mar 13 14:29:58 2020 +0000
+++ b/main.cpp	Fri Mar 13 15:00:35 2020 +0000
@@ -30,6 +30,9 @@
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 
+float position_error = 0; 
+float previous_position_error = 0 ;
+float derivative_position = 0; 
 
 // motor controller variables
 float current_position = 0;
@@ -63,15 +66,15 @@
 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
 
 // PARAMETERS //
-const float kp_vel = 0.08;
-const float ki_vel = 0.03; 
-const float kd_vel = 0.03; 
+const float kp_vel = 20;
+const float ki_vel = 0.00; 
+const float kd_vel = 0.00; 
 
 const float kp_pos = 0.05;
 const float kd_pos = 0;
 const float pwm_period  =0.25f;
 
-int target_velocity = -30;
+int target_velocity = 30;
 float target_position = 500;
 
 // SHARED GLOBAL VARIABLES //
@@ -297,10 +300,19 @@
         velocity = ((current_position - previous_position)/6)*velocity_factor;
         last_time_MtrCtlr = timer_velocity.read_ms();
         previous_position = current_position;
+        float velocity_error = target_velocity - velocity;
+
+        
+        
+        position_error = target_position - current_position; 
+        previous_position_error = position_error; 
+        derivative_position = position_error - previous_position_error; 
     
-        float velocity_error = target_velocity - velocity;
-        float velocity_out = get_velocity_out(velocity_error);
-        float motor_out = get_motor_out(velocity_out);
+        float velocity_out = get_velocity_out(position_error);
+        float motor_out = get_motor_out(velocity_out);      
+
+
+
 
         update_lead(velocity_out);