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