3輪オムニホイール制御 GNCT, B team
Dependencies: nucleo_rotary_encoder
Diff: speed_control/speed_control.cpp
- Revision:
- 0:3bd8aecadafc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/speed_control/speed_control.cpp Thu Sep 28 10:50:37 2017 +0000 @@ -0,0 +1,107 @@ +#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; + + rev = false; + + e->start(); + t.start(); +} + +void speed_control::change_direction(bool _rev) +{ + rev = _rev; +} + +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]; + } + + if(rev)duty *= -1; + + return duty; +} + +float speed_control::get_speed() +{ + return sensor_value; +} \ No newline at end of file