Diff: HorizontalController/HorizontalController.cpp

Revision:

21:169cc2b1d2ff

Parent:

17:f682b4a5686d

--- a/HorizontalController/HorizontalController.cpp	Thu Oct 18 13:38:10 2018 +0000
+++ b/HorizontalController/HorizontalController.cpp	Wed Oct 24 14:24:56 2018 +0000
@@ -8,22 +8,16 @@
     theta_r = 0.0f;
 }
 
-// Control reference roll and pitch angles (rad) given reference horizontal velocities (m/s) and current horizontal velocities (m/s)
-void HorizontalController::control(float u_r, float v_r, float u, float v)
+// Control reference roll and pitch angles (rad) given reference positions (m) and current positions (m) and velocities (m/s)
+void HorizontalController::control(float x_r, float y_r, float x, float y, float u, float v)
 {
-    phi_r = -control_single(v_r,v,T_v)/g;
-    theta_r = control_single(u_r,u,T_u)/g;
+    phi_r = -control_state_regulator(y_r,y,v,kp_y,kd_y)/g;
+    theta_r = control_state_regulator(x_r,x,u,kp_x,kd_x)/g;
 }
 
-// Control roll and pitch references (rad) given reference horizontal velocities (m/s) and current horizontal velocities (m/s) on take-off
-void HorizontalController::control_take_off(float u_r, float v_r, float u, float v)
+// Control acceleration given reference position (m) and current position (m) and velocity (m/s) with given controller gains
+float HorizontalController::control_state_regulator(float pos_r, float pos, float vel, float kp, float kd)
 {
-    phi_r = -control_single(v_r,v,T_v_take_off)/g;
-    theta_r = control_single(u_r,u,T_u_take_off)/g;
-}
-
-// Control reference angle (rad) given reference horizontal velocity (m/s) and current horizontal velocity (m/s) with given time constant (s)
-float HorizontalController::control_single(float velocity_r, float velocity, float T_velocity)
-{
-    return (1.0f/T_velocity)*(velocity_r-velocity);    
+    float acc_r = kp*(pos_r-pos) + kd*(0.0f-vel); 
+    return acc_r;
 }
\ No newline at end of file