DECS_YoungCHA / Mbed 2 deprecated BTS_dc_motor

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

PID.cpp@1:fa0730bf53ef, 2022-08-23 (annotated)

Committer:
ohdoyoel
Date:
Tue Aug 23 08:55:01 2022 +0000
Revision:
1:fa0730bf53ef
hello~

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ohdoyoel 1:fa0730bf53ef 1 #include "PID.h"
ohdoyoel 1:fa0730bf53ef 2
ohdoyoel 1:fa0730bf53ef 3 extern Serial bt;
ohdoyoel 1:fa0730bf53ef 4
ohdoyoel 1:fa0730bf53ef 5 float PID::getP(){
ohdoyoel 1:fa0730bf53ef 6 return Kp_;
ohdoyoel 1:fa0730bf53ef 7 }
ohdoyoel 1:fa0730bf53ef 8 void PID::setP(float Kp){
ohdoyoel 1:fa0730bf53ef 9 Kp_ = Kp;
ohdoyoel 1:fa0730bf53ef 10 }
ohdoyoel 1:fa0730bf53ef 11 float PID::getI(){
ohdoyoel 1:fa0730bf53ef 12 return Ki_;
ohdoyoel 1:fa0730bf53ef 13 }
ohdoyoel 1:fa0730bf53ef 14 void PID::setI(float Ki){
ohdoyoel 1:fa0730bf53ef 15 Ki_ = Ki;
ohdoyoel 1:fa0730bf53ef 16 }
ohdoyoel 1:fa0730bf53ef 17 float PID::getD(){
ohdoyoel 1:fa0730bf53ef 18 return Kd_;
ohdoyoel 1:fa0730bf53ef 19 }
ohdoyoel 1:fa0730bf53ef 20 void PID::setD(float Kd){
ohdoyoel 1:fa0730bf53ef 21 Kd_ = Kd;
ohdoyoel 1:fa0730bf53ef 22 }
ohdoyoel 1:fa0730bf53ef 23
ohdoyoel 1:fa0730bf53ef 24 PID::PID(float Kp, float Ki, float Kd, float interval)
ohdoyoel 1:fa0730bf53ef 25 {
ohdoyoel 1:fa0730bf53ef 26 Kp_=Kp;
ohdoyoel 1:fa0730bf53ef 27 Ki_=Ki;
ohdoyoel 1:fa0730bf53ef 28 Kd_=Kd;
ohdoyoel 1:fa0730bf53ef 29 interval_=interval;
ohdoyoel 1:fa0730bf53ef 30
ohdoyoel 1:fa0730bf53ef 31 err_ = 0;
ohdoyoel 1:fa0730bf53ef 32 accErr_ = 0;
ohdoyoel 1:fa0730bf53ef 33 prevCV_ = 0;
ohdoyoel 1:fa0730bf53ef 34
ohdoyoel 1:fa0730bf53ef 35 prevVelocity_=0; // speed control
ohdoyoel 1:fa0730bf53ef 36
ohdoyoel 1:fa0730bf53ef 37 TV_ = 0;
ohdoyoel 1:fa0730bf53ef 38
ohdoyoel 1:fa0730bf53ef 39 Scale_ = PID_SCALE; // pulses / rotation
ohdoyoel 1:fa0730bf53ef 40 }
ohdoyoel 1:fa0730bf53ef 41
ohdoyoel 1:fa0730bf53ef 42 void PID::ReadCurrentValue(int CurrentValue) // current Encoder Value
ohdoyoel 1:fa0730bf53ef 43 {
ohdoyoel 1:fa0730bf53ef 44 CV_ = CurrentValue;
ohdoyoel 1:fa0730bf53ef 45 }
ohdoyoel 1:fa0730bf53ef 46
ohdoyoel 1:fa0730bf53ef 47 void PID::SetTargetValue(int targetValue)
ohdoyoel 1:fa0730bf53ef 48 {
ohdoyoel 1:fa0730bf53ef 49 TV_ = targetValue;
ohdoyoel 1:fa0730bf53ef 50 }
ohdoyoel 1:fa0730bf53ef 51
ohdoyoel 1:fa0730bf53ef 52 float PID::ReadCurrentVelocity(void)
ohdoyoel 1:fa0730bf53ef 53 {
ohdoyoel 1:fa0730bf53ef 54 float CurrentVelocity_ = (CV_ - prevCV_) / interval_ / PID_SCALE;
ohdoyoel 1:fa0730bf53ef 55
ohdoyoel 1:fa0730bf53ef 56 return CurrentVelocity_;
ohdoyoel 1:fa0730bf53ef 57 }
ohdoyoel 1:fa0730bf53ef 58
ohdoyoel 1:fa0730bf53ef 59 float PID::Performance_Speed(void)
ohdoyoel 1:fa0730bf53ef 60 {
ohdoyoel 1:fa0730bf53ef 61 float CurrentVelocity_ = (CV_ - prevCV_) / interval_ / PID_SCALE;
ohdoyoel 1:fa0730bf53ef 62
ohdoyoel 1:fa0730bf53ef 63 err_= TV_ - CurrentVelocity_;
ohdoyoel 1:fa0730bf53ef 64 accErr_ += err_;
ohdoyoel 1:fa0730bf53ef 65
ohdoyoel 1:fa0730bf53ef 66 /* // wrong code
ohdoyoel 1:fa0730bf53ef 67 float slope = (CurrentVelocity_ - prevVelocity_) / interval_;
ohdoyoel 1:fa0730bf53ef 68 controllerOutput_ = (Kp_ * err_) + (Ki_ * accErr_) + (Kd_ * slope);
ohdoyoel 1:fa0730bf53ef 69 */
ohdoyoel 1:fa0730bf53ef 70
ohdoyoel 1:fa0730bf53ef 71 controllerOutput_ = (Kp_ * err_) + (Ki_ * interval_ * accErr_) + (Kd_ * (ScaledCV_ - ScaledPrevCV_) / interval_);
ohdoyoel 1:fa0730bf53ef 72
ohdoyoel 1:fa0730bf53ef 73
ohdoyoel 1:fa0730bf53ef 74 prevCV_ = CV_;
ohdoyoel 1:fa0730bf53ef 75
ohdoyoel 1:fa0730bf53ef 76 prevVelocity_ = CurrentVelocity_;
ohdoyoel 1:fa0730bf53ef 77
ohdoyoel 1:fa0730bf53ef 78 return controllerOutput_;
ohdoyoel 1:fa0730bf53ef 79 }
ohdoyoel 1:fa0730bf53ef 80
ohdoyoel 1:fa0730bf53ef 81 float PID::Performance_Location(void)
ohdoyoel 1:fa0730bf53ef 82 {
ohdoyoel 1:fa0730bf53ef 83 ScaledCV_ = CV_ / Scale_;
ohdoyoel 1:fa0730bf53ef 84 ScaledTV_ = TV_ / Scale_;
ohdoyoel 1:fa0730bf53ef 85 ScaledPrevCV_ = prevCV_ / Scale_;
ohdoyoel 1:fa0730bf53ef 86
ohdoyoel 1:fa0730bf53ef 87
ohdoyoel 1:fa0730bf53ef 88 err_ = ScaledTV_ - ScaledCV_;
ohdoyoel 1:fa0730bf53ef 89 accErr_ += err_;
ohdoyoel 1:fa0730bf53ef 90
ohdoyoel 1:fa0730bf53ef 91 /* // wrong code
ohdoyoel 1:fa0730bf53ef 92 float slope = (ScaledCV_ - ScaledPrevCV_) / interval_;
ohdoyoel 1:fa0730bf53ef 93 controllerOutput_ = (Kp_ * err_) + (Ki_ * accErr_) + (Kd_ * slope); // missed the sampling time in Ki terms
ohdoyoel 1:fa0730bf53ef 94 */
ohdoyoel 1:fa0730bf53ef 95
ohdoyoel 1:fa0730bf53ef 96 controllerOutput_ = (Kp_ * err_) + (Ki_ * interval_ * accErr_) + (Kd_ * (ScaledCV_ - ScaledPrevCV_) / interval_);
ohdoyoel 1:fa0730bf53ef 97
ohdoyoel 1:fa0730bf53ef 98 prevCV_ = CV_;
ohdoyoel 1:fa0730bf53ef 99
ohdoyoel 1:fa0730bf53ef 100 return controllerOutput_;
ohdoyoel 1:fa0730bf53ef 101 }
ohdoyoel 1:fa0730bf53ef 102
ohdoyoel 1:fa0730bf53ef 103 void PID::Reset(void)
ohdoyoel 1:fa0730bf53ef 104 {
ohdoyoel 1:fa0730bf53ef 105 err_ = 0;
ohdoyoel 1:fa0730bf53ef 106 accErr_ = 0;
ohdoyoel 1:fa0730bf53ef 107 prevCV_ = 0;
ohdoyoel 1:fa0730bf53ef 108
ohdoyoel 1:fa0730bf53ef 109 prevVelocity_=0; // speed control
ohdoyoel 1:fa0730bf53ef 110
ohdoyoel 1:fa0730bf53ef 111 TV_ = 0;
ohdoyoel 1:fa0730bf53ef 112 }
ohdoyoel 1:fa0730bf53ef 113
ohdoyoel 1:fa0730bf53ef 114 void PID::ResetError(void)
ohdoyoel 1:fa0730bf53ef 115 {
ohdoyoel 1:fa0730bf53ef 116 accErr_ = 0;
ohdoyoel 1:fa0730bf53ef 117 }
ohdoyoel 1:fa0730bf53ef 118