3輪オムニホイール制御 GNCT, B team
Dependencies: nucleo_rotary_encoder
speed_control/speed_control.cpp@0:3bd8aecadafc, 2017-09-28 (annotated)
- Committer:
- sawai
- Date:
- Thu Sep 28 10:50:37 2017 +0000
- Revision:
- 0:3bd8aecadafc
?????????xy?????; ???????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sawai | 0:3bd8aecadafc | 1 | #include "mbed.h" |
sawai | 0:3bd8aecadafc | 2 | #include "speed_control.hpp" |
sawai | 0:3bd8aecadafc | 3 | #include "rotary_encoder_ab_phase.hpp" |
sawai | 0:3bd8aecadafc | 4 | |
sawai | 0:3bd8aecadafc | 5 | speed_control::speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms) |
sawai | 0:3bd8aecadafc | 6 | { |
sawai | 0:3bd8aecadafc | 7 | e = new rotary_encoder_ab_phase(timer_type, _pulse_per_revol); |
sawai | 0:3bd8aecadafc | 8 | pulse_per_revol = _pulse_per_revol; |
sawai | 0:3bd8aecadafc | 9 | cy_ms = _cy_ms; |
sawai | 0:3bd8aecadafc | 10 | f1 = f2 = 0; |
sawai | 0:3bd8aecadafc | 11 | target_value = 0.0f; |
sawai | 0:3bd8aecadafc | 12 | |
sawai | 0:3bd8aecadafc | 13 | rev = false; |
sawai | 0:3bd8aecadafc | 14 | |
sawai | 0:3bd8aecadafc | 15 | e->start(); |
sawai | 0:3bd8aecadafc | 16 | t.start(); |
sawai | 0:3bd8aecadafc | 17 | } |
sawai | 0:3bd8aecadafc | 18 | |
sawai | 0:3bd8aecadafc | 19 | void speed_control::change_direction(bool _rev) |
sawai | 0:3bd8aecadafc | 20 | { |
sawai | 0:3bd8aecadafc | 21 | rev = _rev; |
sawai | 0:3bd8aecadafc | 22 | } |
sawai | 0:3bd8aecadafc | 23 | |
sawai | 0:3bd8aecadafc | 24 | void speed_control::set_speed(float _target_value) |
sawai | 0:3bd8aecadafc | 25 | { |
sawai | 0:3bd8aecadafc | 26 | target_value = _target_value; |
sawai | 0:3bd8aecadafc | 27 | } |
sawai | 0:3bd8aecadafc | 28 | |
sawai | 0:3bd8aecadafc | 29 | void speed_control::set_pid(float _kp, float _ki, float _kd) |
sawai | 0:3bd8aecadafc | 30 | { |
sawai | 0:3bd8aecadafc | 31 | kp = _kp; ki = _ki; kd = _kd; |
sawai | 0:3bd8aecadafc | 32 | f1 = kp + ki / 2.0f + kd; |
sawai | 0:3bd8aecadafc | 33 | f2 = -1.0f * kp + ki / 2.0f - 2.0f * kd; |
sawai | 0:3bd8aecadafc | 34 | |
sawai | 0:3bd8aecadafc | 35 | e->reset(); |
sawai | 0:3bd8aecadafc | 36 | t.reset(); |
sawai | 0:3bd8aecadafc | 37 | time_counter = 0; |
sawai | 0:3bd8aecadafc | 38 | pre_counts = 0; |
sawai | 0:3bd8aecadafc | 39 | } |
sawai | 0:3bd8aecadafc | 40 | |
sawai | 0:3bd8aecadafc | 41 | bool speed_control::check_get_speed() |
sawai | 0:3bd8aecadafc | 42 | { |
sawai | 0:3bd8aecadafc | 43 | if(t.read_ms() > time_counter) |
sawai | 0:3bd8aecadafc | 44 | { |
sawai | 0:3bd8aecadafc | 45 | time_counter += cy_ms; |
sawai | 0:3bd8aecadafc | 46 | |
sawai | 0:3bd8aecadafc | 47 | for(int i = 9; i > 0; i--) |
sawai | 0:3bd8aecadafc | 48 | { |
sawai | 0:3bd8aecadafc | 49 | e_value[i] = e_value[i-1]; |
sawai | 0:3bd8aecadafc | 50 | } |
sawai | 0:3bd8aecadafc | 51 | int32_t ec = e->get_counts(); |
sawai | 0:3bd8aecadafc | 52 | e_value[0] = ec - pre_counts; |
sawai | 0:3bd8aecadafc | 53 | if(ec > 20000 || ec < -20000) |
sawai | 0:3bd8aecadafc | 54 | { |
sawai | 0:3bd8aecadafc | 55 | e->reset(); |
sawai | 0:3bd8aecadafc | 56 | pre_counts = pre_counts - ec; |
sawai | 0:3bd8aecadafc | 57 | } |
sawai | 0:3bd8aecadafc | 58 | else |
sawai | 0:3bd8aecadafc | 59 | { |
sawai | 0:3bd8aecadafc | 60 | pre_counts = ec; |
sawai | 0:3bd8aecadafc | 61 | } |
sawai | 0:3bd8aecadafc | 62 | int32_t esum = 0; |
sawai | 0:3bd8aecadafc | 63 | for(int i = 0; i < 10; i++) |
sawai | 0:3bd8aecadafc | 64 | { |
sawai | 0:3bd8aecadafc | 65 | esum += e_value[i]; |
sawai | 0:3bd8aecadafc | 66 | } |
sawai | 0:3bd8aecadafc | 67 | sensor_value = esum / pulse_per_revol / (10.0f * cy_ms) * 1000; |
sawai | 0:3bd8aecadafc | 68 | |
sawai | 0:3bd8aecadafc | 69 | return true; |
sawai | 0:3bd8aecadafc | 70 | } |
sawai | 0:3bd8aecadafc | 71 | else |
sawai | 0:3bd8aecadafc | 72 | { |
sawai | 0:3bd8aecadafc | 73 | return false; |
sawai | 0:3bd8aecadafc | 74 | } |
sawai | 0:3bd8aecadafc | 75 | } |
sawai | 0:3bd8aecadafc | 76 | |
sawai | 0:3bd8aecadafc | 77 | char speed_control::get_duty() |
sawai | 0:3bd8aecadafc | 78 | { |
sawai | 0:3bd8aecadafc | 79 | if(t.read() > 1800) |
sawai | 0:3bd8aecadafc | 80 | { |
sawai | 0:3bd8aecadafc | 81 | t.reset(); |
sawai | 0:3bd8aecadafc | 82 | time_counter = 0; |
sawai | 0:3bd8aecadafc | 83 | } |
sawai | 0:3bd8aecadafc | 84 | if(check_get_speed()) |
sawai | 0:3bd8aecadafc | 85 | { |
sawai | 0:3bd8aecadafc | 86 | diff[0] = sensor_value - target_value; |
sawai | 0:3bd8aecadafc | 87 | dm = f1 * diff[0] + j; |
sawai | 0:3bd8aecadafc | 88 | m = m + dm; |
sawai | 0:3bd8aecadafc | 89 | |
sawai | 0:3bd8aecadafc | 90 | int m_int = (int)m * -1; |
sawai | 0:3bd8aecadafc | 91 | if(m_int <= -126) m_int = -126; |
sawai | 0:3bd8aecadafc | 92 | if(m_int >= 126) m_int = 126; |
sawai | 0:3bd8aecadafc | 93 | duty = m_int; |
sawai | 0:3bd8aecadafc | 94 | |
sawai | 0:3bd8aecadafc | 95 | j = f2 * diff[0] + kd * diff[1]; |
sawai | 0:3bd8aecadafc | 96 | diff[1] = diff[0]; |
sawai | 0:3bd8aecadafc | 97 | } |
sawai | 0:3bd8aecadafc | 98 | |
sawai | 0:3bd8aecadafc | 99 | if(rev)duty *= -1; |
sawai | 0:3bd8aecadafc | 100 | |
sawai | 0:3bd8aecadafc | 101 | return duty; |
sawai | 0:3bd8aecadafc | 102 | } |
sawai | 0:3bd8aecadafc | 103 | |
sawai | 0:3bd8aecadafc | 104 | float speed_control::get_speed() |
sawai | 0:3bd8aecadafc | 105 | { |
sawai | 0:3bd8aecadafc | 106 | return sensor_value; |
sawai | 0:3bd8aecadafc | 107 | } |