Yeongsoo Kim / Mbed 2 deprecated Mecha_Speed_control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Controller/SpeedController.cpp@5:81e5cc52b4fe, 2020-11-10 (annotated)

Committer:
yeongsookim
Date:
Tue Nov 10 14:47:43 2020 +0000
Revision:
5:81e5cc52b4fe
Parent:
2:8a7b4b3cb4c1 
test

Who changed what in which revision?

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