Ahmed Allibhoy / Mbed 2 deprecated MM_Competition

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:   QEI mbed

pid.cpp@5:0975b797bf54, 2015-12-04 (annotated)

Committer:
jasonliujc
Date:
Fri Dec 04 05:19:10 2015 +0000
Revision:
5:0975b797bf54
Parent:
4:d59328f14363
Child:
6:95722ada4706
pid changes 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jasonliujc 4:d59328f14363 1 //#include "pid.h"
jasonliujc 4:d59328f14363 2 //
jasonliujc 4:d59328f14363 3 //void PIDController::onLoop()
jasonliujc 4:d59328f14363 4 //{
jasonliujc 4:d59328f14363 5 // float error = (m_error)();
jasonliujc 4:d59328f14363 6 // (m_out)(m_Kp * error + m_Ki * iController(error) + m_Kd * dController(error));
jasonliujc 4:d59328f14363 7 // wait(m_dt);
jasonliujc 4:d59328f14363 8 //}
jasonliujc 4:d59328f14363 9 //
jasonliujc 4:d59328f14363 10 //float PIDController::iController(float error)
jasonliujc 4:d59328f14363 11 //{
jasonliujc 4:d59328f14363 12 // m_integral += error;
jasonliujc 4:d59328f14363 13 // return m_integral;
jasonliujc 4:d59328f14363 14 //}
jasonliujc 4:d59328f14363 15 //
jasonliujc 4:d59328f14363 16 //float PIDController::dController(float error)
jasonliujc 4:d59328f14363 17 //{
jasonliujc 4:d59328f14363 18 // float derivative = error - m_pError;
jasonliujc 4:d59328f14363 19 // m_pError = error;
jasonliujc 4:d59328f14363 20 // return derivative;
jasonliujc 4:d59328f14363 21 //}
jasonliujc 4:d59328f14363 22 //
jasonliujc 4:d59328f14363 23 //void PIDController::reset()
jasonliujc 4:d59328f14363 24 //{
jasonliujc 4:d59328f14363 25 // m_integral = 0;
jasonliujc 4:d59328f14363 26 //}
ahmedallibhoy 1:45f1f67eab62 27
jasonliujc 4:d59328f14363 28
jasonliujc 4:d59328f14363 29 #include "pid.h"
jasonliujc 4:d59328f14363 30 Timer timer;
ahmedallibhoy 1:45f1f67eab62 31
jasonliujc 5:0975b797bf54 32 void ctrl::setKp(float num){
jasonliujc 4:d59328f14363 33
jasonliujc 4:d59328f14363 34 kp=num;
jasonliujc 4:d59328f14363 35 }
jasonliujc 4:d59328f14363 36
jasonliujc 4:d59328f14363 37
jasonliujc 5:0975b797bf54 38 void ctrl::setKi(float num){
jasonliujc 4:d59328f14363 39
jasonliujc 4:d59328f14363 40 ki=num;
jasonliujc 4:d59328f14363 41 }
jasonliujc 5:0975b797bf54 42 void ctrl::setKd(float num){
jasonliujc 4:d59328f14363 43
jasonliujc 4:d59328f14363 44 kd=num;
ahmedallibhoy 1:45f1f67eab62 45 }
jasonliujc 4:d59328f14363 46
jasonliujc 5:0975b797bf54 47 void ctrl::setErr(float num){
jasonliujc 4:d59328f14363 48
jasonliujc 4:d59328f14363 49 error=num;
jasonliujc 4:d59328f14363 50 }
jasonliujc 4:d59328f14363 51
jasonliujc 5:0975b797bf54 52 void ctrl::setprevErr(float num){
jasonliujc 4:d59328f14363 53
jasonliujc 4:d59328f14363 54 prevErr=num;
jasonliujc 4:d59328f14363 55 }
jasonliujc 4:d59328f14363 56
jasonliujc 5:0975b797bf54 57 void ctrl::setCorrection(float num){
jasonliujc 4:d59328f14363 58
jasonliujc 4:d59328f14363 59 correction=num;
jasonliujc 4:d59328f14363 60 }
ahmedallibhoy 1:45f1f67eab62 61
jasonliujc 5:0975b797bf54 62 float ctrl::pctrl(){
jasonliujc 4:d59328f14363 63 correction=kp*error;
jasonliujc 4:d59328f14363 64 return correction;
jasonliujc 4:d59328f14363 65
jasonliujc 4:d59328f14363 66 }
jasonliujc 4:d59328f14363 67
jasonliujc 5:0975b797bf54 68 float ctrl::dctrl(){
jasonliujc 5:0975b797bf54 69 float dErr=error-prevErr;
jasonliujc 5:0975b797bf54 70 float dt=timer.read_us();
jasonliujc 4:d59328f14363 71 timer.reset();
jasonliujc 4:d59328f14363 72 setprevErr(error);
jasonliujc 4:d59328f14363 73 correction=kd*dErr/dt;
jasonliujc 4:d59328f14363 74
jasonliujc 4:d59328f14363 75
jasonliujc 4:d59328f14363 76 return correction;
jasonliujc 4:d59328f14363 77
ahmedallibhoy 1:45f1f67eab62 78 }
jasonliujc 4:d59328f14363 79
jasonliujc 4:d59328f14363 80
jasonliujc 5:0975b797bf54 81 float ctrl::ictrl(){
jasonliujc 5:0975b797bf54 82 floategrater+=error;
jasonliujc 4:d59328f14363 83
jasonliujc 5:0975b797bf54 84 correction=ki*floategrater;
jasonliujc 5:0975b797bf54 85 floategrater/=decay;
jasonliujc 4:d59328f14363 86 return correction;
jasonliujc 4:d59328f14363 87
jasonliujc 4:d59328f14363 88 }
jasonliujc 4:d59328f14363 89
jasonliujc 5:0975b797bf54 90 float ctrl::total(){
jasonliujc 4:d59328f14363 91 return pctrl()+dctrl()+ictrl();
jasonliujc 4:d59328f14363 92
jasonliujc 4:d59328f14363 93
jasonliujc 4:d59328f14363 94
jasonliujc 4:d59328f14363 95
jasonliujc 4:d59328f14363 96 }
jasonliujc 4:d59328f14363 97
jasonliujc 5:0975b797bf54 98 void ctrl::adjust(float xSpeed,float ySpeed,float L, float R){
jasonliujc 4:d59328f14363 99
jasonliujc 4:d59328f14363 100
jasonliujc 5:0975b797bf54 101 TErr=xSpeed-(L+R);
jasonliujc 5:0975b797bf54 102 RErr=rotationError();
jasonliujc 5:0975b797bf54 103
jasonliujc 5:0975b797bf54 104 }
jasonliujc 5:0975b797bf54 105
jasonliujc 5:0975b797bf54 106 float ctrl::getT(){
jasonliujc 5:0975b797bf54 107
jasonliujc 5:0975b797bf54 108 return TErr;
jasonliujc 5:0975b797bf54 109 }
jasonliujc 4:d59328f14363 110
jasonliujc 4:d59328f14363 111
jasonliujc 5:0975b797bf54 112 float ctrl::getR(){
jasonliujc 5:0975b797bf54 113 return RErr;
jasonliujc 5:0975b797bf54 114
jasonliujc 5:0975b797bf54 115 }