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@8:1ce513eff545, 2015-12-04 (annotated)

Committer:
jasonliujc
Date:
Fri Dec 04 05:56:00 2015 +0000
Revision:
8:1ce513eff545
Parent:
6:95722ada4706
Child:
10:332a4ad5911d
Rotation Err implemented

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 8:1ce513eff545 30
jasonliujc 4:d59328f14363 31 Timer timer;
ahmedallibhoy 1:45f1f67eab62 32
jasonliujc 5:0975b797bf54 33 void ctrl::setKp(float num){
jasonliujc 4:d59328f14363 34
jasonliujc 4:d59328f14363 35 kp=num;
jasonliujc 4:d59328f14363 36 }
jasonliujc 4:d59328f14363 37
jasonliujc 4:d59328f14363 38
jasonliujc 5:0975b797bf54 39 void ctrl::setKi(float num){
jasonliujc 4:d59328f14363 40
jasonliujc 4:d59328f14363 41 ki=num;
jasonliujc 4:d59328f14363 42 }
jasonliujc 5:0975b797bf54 43 void ctrl::setKd(float num){
jasonliujc 4:d59328f14363 44
jasonliujc 4:d59328f14363 45 kd=num;
ahmedallibhoy 1:45f1f67eab62 46 }
jasonliujc 4:d59328f14363 47
jasonliujc 5:0975b797bf54 48 void ctrl::setErr(float num){
jasonliujc 4:d59328f14363 49
jasonliujc 4:d59328f14363 50 error=num;
jasonliujc 4:d59328f14363 51 }
jasonliujc 4:d59328f14363 52
jasonliujc 5:0975b797bf54 53 void ctrl::setprevErr(float num){
jasonliujc 4:d59328f14363 54
jasonliujc 4:d59328f14363 55 prevErr=num;
jasonliujc 4:d59328f14363 56 }
jasonliujc 4:d59328f14363 57
jasonliujc 5:0975b797bf54 58 void ctrl::setCorrection(float num){
jasonliujc 4:d59328f14363 59
jasonliujc 4:d59328f14363 60 correction=num;
jasonliujc 4:d59328f14363 61 }
ahmedallibhoy 1:45f1f67eab62 62
jasonliujc 5:0975b797bf54 63 float ctrl::pctrl(){
jasonliujc 4:d59328f14363 64 correction=kp*error;
jasonliujc 4:d59328f14363 65 return correction;
jasonliujc 4:d59328f14363 66
jasonliujc 4:d59328f14363 67 }
jasonliujc 4:d59328f14363 68
jasonliujc 5:0975b797bf54 69 float ctrl::dctrl(){
jasonliujc 5:0975b797bf54 70 float dErr=error-prevErr;
jasonliujc 5:0975b797bf54 71 float dt=timer.read_us();
jasonliujc 4:d59328f14363 72 timer.reset();
jasonliujc 4:d59328f14363 73 setprevErr(error);
jasonliujc 4:d59328f14363 74 correction=kd*dErr/dt;
jasonliujc 4:d59328f14363 75
jasonliujc 4:d59328f14363 76
jasonliujc 4:d59328f14363 77 return correction;
jasonliujc 4:d59328f14363 78
ahmedallibhoy 1:45f1f67eab62 79 }
jasonliujc 4:d59328f14363 80
jasonliujc 4:d59328f14363 81
jasonliujc 5:0975b797bf54 82 float ctrl::ictrl(){
jasonliujc 5:0975b797bf54 83 floategrater+=error;
jasonliujc 4:d59328f14363 84
jasonliujc 5:0975b797bf54 85 correction=ki*floategrater;
jasonliujc 5:0975b797bf54 86 floategrater/=decay;
jasonliujc 4:d59328f14363 87 return correction;
jasonliujc 4:d59328f14363 88
jasonliujc 4:d59328f14363 89 }
jasonliujc 4:d59328f14363 90
jasonliujc 5:0975b797bf54 91 float ctrl::total(){
jasonliujc 4:d59328f14363 92 return pctrl()+dctrl()+ictrl();
jasonliujc 4:d59328f14363 93
jasonliujc 4:d59328f14363 94
jasonliujc 4:d59328f14363 95
jasonliujc 4:d59328f14363 96
jasonliujc 4:d59328f14363 97 }
jasonliujc 4:d59328f14363 98
jasonliujc 8:1ce513eff545 99 void ctrl::adjust(float xSpeed,float L, float R){
jasonliujc 4:d59328f14363 100
jasonliujc 4:d59328f14363 101
jasonliujc 5:0975b797bf54 102 TErr=xSpeed-(L+R);
jasonliujc 5:0975b797bf54 103 RErr=rotationError();
jasonliujc 5:0975b797bf54 104
jasonliujc 5:0975b797bf54 105 }
jasonliujc 6:95722ada4706 106
jasonliujc 6:95722ada4706 107
jasonliujc 8:1ce513eff545 108 void ctrl::updateErr(float e){
jasonliujc 6:95722ada4706 109
jasonliujc 6:95722ada4706 110 setprevErr(error);
jasonliujc 6:95722ada4706 111 error=e;
jasonliujc 6:95722ada4706 112
jasonliujc 6:95722ada4706 113
jasonliujc 6:95722ada4706 114
jasonliujc 6:95722ada4706 115 }
jasonliujc 6:95722ada4706 116
jasonliujc 5:0975b797bf54 117
jasonliujc 5:0975b797bf54 118 float ctrl::getT(){
jasonliujc 5:0975b797bf54 119
jasonliujc 5:0975b797bf54 120 return TErr;
jasonliujc 5:0975b797bf54 121 }
jasonliujc 4:d59328f14363 122
jasonliujc 4:d59328f14363 123
jasonliujc 5:0975b797bf54 124 float ctrl::getR(){
jasonliujc 5:0975b797bf54 125 return RErr;
jasonliujc 5:0975b797bf54 126
jasonliujc 5:0975b797bf54 127 }