3輪オムニホイール制御 GNCT, B team
Dependencies: nucleo_rotary_encoder
speed_control/speed_control.cpp
- Committer:
- sawai
- Date:
- 2017-09-28
- Revision:
- 0:3bd8aecadafc
File content as of revision 0:3bd8aecadafc:
#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; }