Diff: shared/MD_PID/MD_PID.cpp.txt

Revision:

0:ee7e9405e1c7

diff -r 000000000000 -r ee7e9405e1c7 shared/MD_PID/MD_PID.cpp.txt
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/MD_PID/MD_PID.cpp.txt	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,58 @@
+#include "MD_PID.h"
+
+MD_PID::MD_PID( MD *md, QEI *qei,
+                double kp, double ki, double kd,
+				double max_speed)
+                : _md(md), _qei(qei), PID_Control(kp, ki, kd)
+{
+    _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_Control::PI_D(present_vel, target_vel, interval);
+    _duty = PID_Control::get_control();
+    
+    //printf("%lf\r\n", _duty);
+    
+    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();
+}