Diff: shared/MD_PID/MD_PID.cpp

Revision:

0:ee7e9405e1c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/MD_PID/MD_PID.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,57 @@
+#include "MD_PID.h"
+
+MD_PID::MD_PID( MD *md, QEI *qei,
+                double k, double ti, double td,
+				double max_speed)
+                : _md(md), _qei(qei), _pid(new PI_D(k, ti, td))
+{
+    _duty = 0.0;
+    set_max_speed(max_speed);
+}
+
+void MD_PID::drive(double target_vel, double interval)
+{
+    double present_vel = read_vel();
+
+    if(fabs(target_vel) > _max_speed){
+        if(target_vel >= 0)
+            target_vel = _max_speed;
+        else
+            target_vel = -_max_speed;
+    }
+    //PID_Control::PID(present_vel, target_vel, interval);
+    //PID_Control::VPID(present_vel, target_vel, interval);
+    //PID::PI_D(present_vel, target_vel, interval);
+    _pid->set_state(present_vel, target_vel);
+    _duty = -1 * _pid->get_control();
+    
+    if( fabs(_duty) > 1 )
+        _duty /= fabs(_duty);
+        
+    _md->drive(_duty);
+    //printf("presnet_vel:\t%lf\tduty:\t%lf\r\n", present_vel, _duty);
+}
+
+double MD_PID::get_duty(){
+	return _duty;
+}
+
+void MD_PID::brake(double target)
+{
+    
+}
+
+void MD_PID::free()
+{
+    
+}
+
+void MD_PID::set_max_speed(double max_speed)
+{
+	_max_speed = max_speed;
+}
+
+double MD_PID::read_vel()
+{
+    return _qei->get_ang_vel();
+}