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@2:8a7b4b3cb4c1, 2020-11-10 (annotated)

Committer:
yeongsookim
Date:
Tue Nov 10 13:48:28 2020 +0000
Revision:
2:8a7b4b3cb4c1
Parent:
1:d7a26e14bc4d
Child:
5:81e5cc52b4fe
test

Who changed what in which revision?

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