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@4:d59328f14363, 2015-12-04 (annotated)

Committer:
jasonliujc
Date:
Fri Dec 04 05:12:57 2015 +0000
Revision:
4:d59328f14363
Parent:
1:45f1f67eab62
Child:
5:0975b797bf54
pid update;

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 4:d59328f14363 32 void ctrl::setKp(double num){
jasonliujc 4:d59328f14363 33
jasonliujc 4:d59328f14363 34 kp=num;
jasonliujc 4:d59328f14363 35 }
jasonliujc 4:d59328f14363 36
jasonliujc 4:d59328f14363 37
jasonliujc 4:d59328f14363 38 void ctrl::setKi(double num){
jasonliujc 4:d59328f14363 39
jasonliujc 4:d59328f14363 40 ki=num;
jasonliujc 4:d59328f14363 41 }
jasonliujc 4:d59328f14363 42 void ctrl::setKd(double num){
jasonliujc 4:d59328f14363 43
jasonliujc 4:d59328f14363 44 kd=num;
ahmedallibhoy 1:45f1f67eab62 45 }
jasonliujc 4:d59328f14363 46
jasonliujc 4:d59328f14363 47 void ctrl::setErr(double num){
jasonliujc 4:d59328f14363 48
jasonliujc 4:d59328f14363 49 error=num;
jasonliujc 4:d59328f14363 50 }
jasonliujc 4:d59328f14363 51
jasonliujc 4:d59328f14363 52 void ctrl::setprevErr(double num){
jasonliujc 4:d59328f14363 53
jasonliujc 4:d59328f14363 54 prevErr=num;
jasonliujc 4:d59328f14363 55 }
jasonliujc 4:d59328f14363 56
jasonliujc 4:d59328f14363 57 void ctrl::setCorrection(double num){
jasonliujc 4:d59328f14363 58
jasonliujc 4:d59328f14363 59 correction=num;
jasonliujc 4:d59328f14363 60 }
ahmedallibhoy 1:45f1f67eab62 61
jasonliujc 4:d59328f14363 62 double 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 4:d59328f14363 68 double ctrl::dctrl(){
jasonliujc 4:d59328f14363 69 double dErr=error-prevErr;
jasonliujc 4:d59328f14363 70 double 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 4:d59328f14363 81 double ctrl::ictrl(){
jasonliujc 4:d59328f14363 82 doubleegrater+=error;
jasonliujc 4:d59328f14363 83
jasonliujc 4:d59328f14363 84 correction=ki*doubleegrater;
jasonliujc 4:d59328f14363 85 doubleegrater/=decay;
jasonliujc 4:d59328f14363 86 return correction;
jasonliujc 4:d59328f14363 87
jasonliujc 4:d59328f14363 88 }
jasonliujc 4:d59328f14363 89
jasonliujc 4:d59328f14363 90 double 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 4:d59328f14363 98 void ctrl::adjust(double xSpeed,double ySpeed,double L, double R){
jasonliujc 4:d59328f14363 99
jasonliujc 4:d59328f14363 100
jasonliujc 4:d59328f14363 101 int EncoderErr=xSpeed-(L+R);
jasonliujc 4:d59328f14363 102
jasonliujc 4:d59328f14363 103
jasonliujc 4:d59328f14363 104 }