주 형진 / Mbed 2 deprecated ai_car1

ai_car1

Dependencies:   mbed ai_car ros_lib_melodic

Controller/SpeedController.cpp@0:a35213e1e14e, 2021-05-03 (annotated)

Committer:
wngudwls000
Date:
Mon May 03 07:22:52 2021 +0000
Revision:
0:a35213e1e14e
45

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wngudwls000 0:a35213e1e14e 1 #include "SpeedController.h"
wngudwls000 0:a35213e1e14e 2
wngudwls000 0:a35213e1e14e 3
wngudwls000 0:a35213e1e14e 4 PIDController::PIDController(float kp, float ki, float kd, float max_windup,
wngudwls000 0:a35213e1e14e 5 float start_time, float umin, float umax)
wngudwls000 0:a35213e1e14e 6 {
wngudwls000 0:a35213e1e14e 7 // The PID controller can be initalized with a specific kp value
wngudwls000 0:a35213e1e14e 8 // ki value, and kd value
wngudwls000 0:a35213e1e14e 9 this->kp = kp;
wngudwls000 0:a35213e1e14e 10 this->ki = ki;
wngudwls000 0:a35213e1e14e 11 this->kd = kd;
wngudwls000 0:a35213e1e14e 12
wngudwls000 0:a35213e1e14e 13 this->max_windup = max_windup;
wngudwls000 0:a35213e1e14e 14
wngudwls000 0:a35213e1e14e 15 this->umin = umin;
wngudwls000 0:a35213e1e14e 16 this->umax = umax;
wngudwls000 0:a35213e1e14e 17
wngudwls000 0:a35213e1e14e 18 //Store relevant data
wngudwls000 0:a35213e1e14e 19 this->m_last_timestamp = 0.0;
wngudwls000 0:a35213e1e14e 20 this->m_set_point = 0.0;
wngudwls000 0:a35213e1e14e 21 this->m_start_time = start_time;
wngudwls000 0:a35213e1e14e 22 this->m_error_sum = 0.0;
wngudwls000 0:a35213e1e14e 23 this->m_last_error = 0.0;
wngudwls000 0:a35213e1e14e 24 }
wngudwls000 0:a35213e1e14e 25
wngudwls000 0:a35213e1e14e 26
wngudwls000 0:a35213e1e14e 27
wngudwls000 0:a35213e1e14e 28 float PIDController::update(float measured_value, float timestamp)
wngudwls000 0:a35213e1e14e 29 {
wngudwls000 0:a35213e1e14e 30 float delta_time = timestamp - m_last_timestamp;
wngudwls000 0:a35213e1e14e 31 float error = m_set_point - measured_value;
wngudwls000 0:a35213e1e14e 32
wngudwls000 0:a35213e1e14e 33 m_last_timestamp = timestamp;
wngudwls000 0:a35213e1e14e 34
wngudwls000 0:a35213e1e14e 35
wngudwls000 0:a35213e1e14e 36 m_error_sum += error * delta_time;
wngudwls000 0:a35213e1e14e 37
wngudwls000 0:a35213e1e14e 38 float delta_error = error - m_last_error;
wngudwls000 0:a35213e1e14e 39
wngudwls000 0:a35213e1e14e 40 if(delta_error > 10.0) {
wngudwls000 0:a35213e1e14e 41 delta_error = 0.0;
wngudwls000 0:a35213e1e14e 42 }
wngudwls000 0:a35213e1e14e 43
wngudwls000 0:a35213e1e14e 44 m_last_error = error;
wngudwls000 0:a35213e1e14e 45
wngudwls000 0:a35213e1e14e 46 if(m_error_sum > max_windup) {
wngudwls000 0:a35213e1e14e 47 m_error_sum = max_windup;
wngudwls000 0:a35213e1e14e 48 } else if(m_error_sum < -1.0*max_windup) {
wngudwls000 0:a35213e1e14e 49 m_error_sum = max_windup*-1.0;
wngudwls000 0:a35213e1e14e 50 }
wngudwls000 0:a35213e1e14e 51
wngudwls000 0:a35213e1e14e 52 float p = kp * error;
wngudwls000 0:a35213e1e14e 53 float i = ki * m_error_sum;
wngudwls000 0:a35213e1e14e 54 float d = kd * (delta_error/delta_time);
wngudwls000 0:a35213e1e14e 55
wngudwls000 0:a35213e1e14e 56 float u=p+i+d;
wngudwls000 0:a35213e1e14e 57
wngudwls000 0:a35213e1e14e 58 if(u > umax) {
wngudwls000 0:a35213e1e14e 59 u = umax;
wngudwls000 0:a35213e1e14e 60 } else if(u < umin) {
wngudwls000 0:a35213e1e14e 61 u = umin;
wngudwls000 0:a35213e1e14e 62 }
wngudwls000 0:a35213e1e14e 63
wngudwls000 0:a35213e1e14e 64 return u;
wngudwls000 0:a35213e1e14e 65 }
wngudwls000 0:a35213e1e14e 66
wngudwls000 0:a35213e1e14e 67 void PIDController::setTarget(float target)
wngudwls000 0:a35213e1e14e 68 {
wngudwls000 0:a35213e1e14e 69 this->m_set_point = target;
wngudwls000 0:a35213e1e14e 70 }
wngudwls000 0:a35213e1e14e 71
wngudwls000 0:a35213e1e14e 72 void PIDController::setKp(float kp)
wngudwls000 0:a35213e1e14e 73 {
wngudwls000 0:a35213e1e14e 74 this->kp = kp;
wngudwls000 0:a35213e1e14e 75 }
wngudwls000 0:a35213e1e14e 76
wngudwls000 0:a35213e1e14e 77 void PIDController::setKi(float ki)
wngudwls000 0:a35213e1e14e 78 {
wngudwls000 0:a35213e1e14e 79 this->ki = ki;
wngudwls000 0:a35213e1e14e 80 }
wngudwls000 0:a35213e1e14e 81
wngudwls000 0:a35213e1e14e 82 void PIDController::setKd(float kd)
wngudwls000 0:a35213e1e14e 83 {
wngudwls000 0:a35213e1e14e 84 this->kd = kd;
wngudwls000 0:a35213e1e14e 85 }
wngudwls000 0:a35213e1e14e 86
wngudwls000 0:a35213e1e14e 87 void PIDController::setMaxWindup(float max_windup)
wngudwls000 0:a35213e1e14e 88 {
wngudwls000 0:a35213e1e14e 89 this->max_windup = max_windup;
wngudwls000 0:a35213e1e14e 90 }
wngudwls000 0:a35213e1e14e 91
wngudwls000 0:a35213e1e14e 92 float PIDController::getLastTimeStamp()
wngudwls000 0:a35213e1e14e 93 {
wngudwls000 0:a35213e1e14e 94 return this->m_last_timestamp;
wngudwls000 0:a35213e1e14e 95 }