Diff: omuni/speed_control/speed_control.cpp

Revision:

0:6da7d0e457a2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omuni/speed_control/speed_control.cpp	Thu Aug 17 03:49:34 2017 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+#include "speed_control.hpp"
+#include "rotary_encoder_ab_phase.hpp"
+
+speed_control::speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms)
+{
+    e = new rotary_encoder_ab_phase(timer_type, _pulse_per_revol);
+    pulse_per_revol = _pulse_per_revol;
+    cy_ms = _cy_ms;
+    f1 = f2 = 0;
+    target_value = 0.0f;
+    
+    e->start();
+    t.start();
+}
+
+void speed_control::set_speed(float _target_value)
+{
+    target_value = _target_value;
+}
+
+void speed_control::set_pid(float _kp, float _ki, float _kd)
+{
+    kp = _kp; ki = _ki; kd = _kd;
+    f1 = kp + ki / 2.0f + kd;
+    f2 = -1.0f * kp + ki / 2.0f - 2.0f * kd;
+    
+    e->reset();
+    t.reset();
+    time_counter = 0;
+    pre_counts = 0;
+}
+
+bool speed_control::check_get_speed()
+{
+    if(t.read_ms() > time_counter)
+    {
+        time_counter += cy_ms;
+        
+        for(int i = 9; i > 0; i--)
+        {
+            e_value[i] = e_value[i-1];
+        }
+        int32_t ec = e->get_counts();
+        e_value[0] = ec - pre_counts;
+        if(ec > 20000 || ec < -20000)
+        {
+            e->reset();
+            pre_counts = pre_counts - ec;
+        }
+        else
+        {
+            pre_counts = ec;
+        }
+        int32_t esum = 0;
+        for(int i = 0; i < 10; i++)
+        {
+            esum += e_value[i];
+        }
+        sensor_value = esum / pulse_per_revol / (10.0f * cy_ms) * 1000;
+        
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+char speed_control::get_duty()
+{
+    if(t.read() > 1800)
+    {
+        t.reset();
+        time_counter = 0;
+    }
+    if(check_get_speed())
+    {
+        diff[0] = sensor_value - target_value;
+        dm = f1 * diff[0] + j;
+        m = m + dm;
+        
+        int m_int = (int)m * -1;
+        if(m_int <= -126)   m_int = -126;
+        if(m_int >=  126)   m_int =  126;
+        duty = m_int;
+        
+        j = f2 * diff[0] + kd * diff[1];
+        diff[1] = diff[0];
+    }
+    return duty;
+}
+
+float speed_control::get_speed()
+{
+    return sensor_value;
+}
\ No newline at end of file