speed_control.cpp

00001 #include "mbed.h"
00002 #include "speed_control.hpp"
00003 #include "rotary_encoder_ab_phase.hpp"
00004 
00005 speed_control::speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms)
00006 {
00007     e = new rotary_encoder_ab_phase(timer_type, _pulse_per_revol);
00008     pulse_per_revol = _pulse_per_revol;
00009     cy_ms = _cy_ms;
00010     f1 = f2 = 0;
00011     target_value = 0.0f;
00012     
00013     rev = false;
00014     
00015     e->start();
00016     t.start();
00017 }
00018 
00019 void speed_control::change_direction(bool _rev)
00020 {
00021     rev = _rev;
00022 }
00023 
00024 void speed_control::set_speed(float _target_value)
00025 {
00026     target_value = _target_value;
00027 }
00028 
00029 void speed_control::set_pid(float _kp, float _ki, float _kd)
00030 {
00031     kp = _kp; ki = _ki; kd = _kd;
00032     f1 = kp + ki / 2.0f + kd;
00033     f2 = -1.0f * kp + ki / 2.0f - 2.0f * kd;
00034     
00035     e->reset();
00036     t.reset();
00037     time_counter = 0;
00038     pre_counts = 0;
00039 }
00040 
00041 bool speed_control::check_get_speed()
00042 {
00043     if(t.read_ms() > time_counter)
00044     {
00045         time_counter += cy_ms;
00046         
00047         for(int i = 9; i > 0; i--)
00048         {
00049             e_value[i] = e_value[i-1];
00050         }
00051         int32_t ec = e->get_counts();
00052         e_value[0] = ec - pre_counts;
00053         if(ec > 20000 || ec < -20000)
00054         {
00055             e->reset();
00056             pre_counts = pre_counts - ec;
00057         }
00058         else
00059         {
00060             pre_counts = ec;
00061         }
00062         int32_t esum = 0;
00063         for(int i = 0; i < 10; i++)
00064         {
00065             esum += e_value[i];
00066         }
00067         sensor_value = esum / pulse_per_revol / (10.0f * cy_ms) * 1000;
00068         
00069         return true;
00070     }
00071     else
00072     {
00073         return false;
00074     }
00075 }
00076 
00077 char speed_control::get_duty()
00078 {
00079     if(t.read() > 1800)
00080     {
00081         t.reset();
00082         time_counter = 0;
00083     }
00084     if(check_get_speed())
00085     {
00086         diff[0] = sensor_value - target_value;
00087         dm = f1 * diff[0] + j;
00088         m = m + dm;
00089         
00090         int m_int = (int)m * -1;
00091         if(m_int <= -126)   m_int = -126;
00092         if(m_int >=  126)   m_int =  126;
00093         duty = m_int;
00094         
00095         j = f2 * diff[0] + kd * diff[1];
00096         diff[1] = diff[0];
00097     }
00098     
00099     if(rev)duty *= -1;
00100     
00101     return duty;
00102 }
00103 
00104 float speed_control::get_speed()
00105 {
00106     return sensor_value;
00107 }