shinji sawai / Mbed 2 deprecated omuni_speed_pid

omuni

Dependencies:   mbed nucleo_rotary_encoder

omuni/speed_control/speed_control.cpp@0:6da7d0e457a2, 2017-08-17 (annotated)

Committer:
sawai
Date:
Thu Aug 17 03:49:34 2017 +0000
Revision:
0:6da7d0e457a2
omuni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sawai 0:6da7d0e457a2 1 #include "mbed.h"
sawai 0:6da7d0e457a2 2 #include "speed_control.hpp"
sawai 0:6da7d0e457a2 3 #include "rotary_encoder_ab_phase.hpp"
sawai 0:6da7d0e457a2 4
sawai 0:6da7d0e457a2 5 speed_control::speed_control(TIM_TypeDef *timer_type, float _pulse_per_revol, float _cy_ms)
sawai 0:6da7d0e457a2 6 {
sawai 0:6da7d0e457a2 7 e = new rotary_encoder_ab_phase(timer_type, _pulse_per_revol);
sawai 0:6da7d0e457a2 8 pulse_per_revol = _pulse_per_revol;
sawai 0:6da7d0e457a2 9 cy_ms = _cy_ms;
sawai 0:6da7d0e457a2 10 f1 = f2 = 0;
sawai 0:6da7d0e457a2 11 target_value = 0.0f;
sawai 0:6da7d0e457a2 12
sawai 0:6da7d0e457a2 13 e->start();
sawai 0:6da7d0e457a2 14 t.start();
sawai 0:6da7d0e457a2 15 }
sawai 0:6da7d0e457a2 16
sawai 0:6da7d0e457a2 17 void speed_control::set_speed(float _target_value)
sawai 0:6da7d0e457a2 18 {
sawai 0:6da7d0e457a2 19 target_value = _target_value;
sawai 0:6da7d0e457a2 20 }
sawai 0:6da7d0e457a2 21
sawai 0:6da7d0e457a2 22 void speed_control::set_pid(float _kp, float _ki, float _kd)
sawai 0:6da7d0e457a2 23 {
sawai 0:6da7d0e457a2 24 kp = _kp; ki = _ki; kd = _kd;
sawai 0:6da7d0e457a2 25 f1 = kp + ki / 2.0f + kd;
sawai 0:6da7d0e457a2 26 f2 = -1.0f * kp + ki / 2.0f - 2.0f * kd;
sawai 0:6da7d0e457a2 27
sawai 0:6da7d0e457a2 28 e->reset();
sawai 0:6da7d0e457a2 29 t.reset();
sawai 0:6da7d0e457a2 30 time_counter = 0;
sawai 0:6da7d0e457a2 31 pre_counts = 0;
sawai 0:6da7d0e457a2 32 }
sawai 0:6da7d0e457a2 33
sawai 0:6da7d0e457a2 34 bool speed_control::check_get_speed()
sawai 0:6da7d0e457a2 35 {
sawai 0:6da7d0e457a2 36 if(t.read_ms() > time_counter)
sawai 0:6da7d0e457a2 37 {
sawai 0:6da7d0e457a2 38 time_counter += cy_ms;
sawai 0:6da7d0e457a2 39
sawai 0:6da7d0e457a2 40 for(int i = 9; i > 0; i--)
sawai 0:6da7d0e457a2 41 {
sawai 0:6da7d0e457a2 42 e_value[i] = e_value[i-1];
sawai 0:6da7d0e457a2 43 }
sawai 0:6da7d0e457a2 44 int32_t ec = e->get_counts();
sawai 0:6da7d0e457a2 45 e_value[0] = ec - pre_counts;
sawai 0:6da7d0e457a2 46 if(ec > 20000 || ec < -20000)
sawai 0:6da7d0e457a2 47 {
sawai 0:6da7d0e457a2 48 e->reset();
sawai 0:6da7d0e457a2 49 pre_counts = pre_counts - ec;
sawai 0:6da7d0e457a2 50 }
sawai 0:6da7d0e457a2 51 else
sawai 0:6da7d0e457a2 52 {
sawai 0:6da7d0e457a2 53 pre_counts = ec;
sawai 0:6da7d0e457a2 54 }
sawai 0:6da7d0e457a2 55 int32_t esum = 0;
sawai 0:6da7d0e457a2 56 for(int i = 0; i < 10; i++)
sawai 0:6da7d0e457a2 57 {
sawai 0:6da7d0e457a2 58 esum += e_value[i];
sawai 0:6da7d0e457a2 59 }
sawai 0:6da7d0e457a2 60 sensor_value = esum / pulse_per_revol / (10.0f * cy_ms) * 1000;
sawai 0:6da7d0e457a2 61
sawai 0:6da7d0e457a2 62 return true;
sawai 0:6da7d0e457a2 63 }
sawai 0:6da7d0e457a2 64 else
sawai 0:6da7d0e457a2 65 {
sawai 0:6da7d0e457a2 66 return false;
sawai 0:6da7d0e457a2 67 }
sawai 0:6da7d0e457a2 68 }
sawai 0:6da7d0e457a2 69
sawai 0:6da7d0e457a2 70 char speed_control::get_duty()
sawai 0:6da7d0e457a2 71 {
sawai 0:6da7d0e457a2 72 if(t.read() > 1800)
sawai 0:6da7d0e457a2 73 {
sawai 0:6da7d0e457a2 74 t.reset();
sawai 0:6da7d0e457a2 75 time_counter = 0;
sawai 0:6da7d0e457a2 76 }
sawai 0:6da7d0e457a2 77 if(check_get_speed())
sawai 0:6da7d0e457a2 78 {
sawai 0:6da7d0e457a2 79 diff[0] = sensor_value - target_value;
sawai 0:6da7d0e457a2 80 dm = f1 * diff[0] + j;
sawai 0:6da7d0e457a2 81 m = m + dm;
sawai 0:6da7d0e457a2 82
sawai 0:6da7d0e457a2 83 int m_int = (int)m * -1;
sawai 0:6da7d0e457a2 84 if(m_int <= -126) m_int = -126;
sawai 0:6da7d0e457a2 85 if(m_int >= 126) m_int = 126;
sawai 0:6da7d0e457a2 86 duty = m_int;
sawai 0:6da7d0e457a2 87
sawai 0:6da7d0e457a2 88 j = f2 * diff[0] + kd * diff[1];
sawai 0:6da7d0e457a2 89 diff[1] = diff[0];
sawai 0:6da7d0e457a2 90 }
sawai 0:6da7d0e457a2 91 return duty;
sawai 0:6da7d0e457a2 92 }
sawai 0:6da7d0e457a2 93
sawai 0:6da7d0e457a2 94 float speed_control::get_speed()
sawai 0:6da7d0e457a2 95 {
sawai 0:6da7d0e457a2 96 return sensor_value;
sawai 0:6da7d0e457a2 97 }