shinji sawai / omuni

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?

UserRevisionLine numberNew 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 }