Akito / Mbed 2 deprecated ROBOCON2017_archan

あーちゃん制御

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?

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