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@10:332a4ad5911d, 2015-12-05 (annotated)

Committer:
jasonliujc
Date:
Sat Dec 05 00:41:23 2015 +0000
Revision:
10:332a4ad5911d
Parent:
8:1ce513eff545
Child:
11:8faa068940f1
changes in motor, pid and sensors

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 10:332a4ad5911d 31
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 10:332a4ad5911d 71 float time=timer.read_us();
jasonliujc 10:332a4ad5911d 72 float dt=time-prevTime;
jasonliujc 10:332a4ad5911d 73 prevTime=time;
jasonliujc 10:332a4ad5911d 74
jasonliujc 10:332a4ad5911d 75 //pc.printf("time: %f \n",time);
jasonliujc 10:332a4ad5911d 76
jasonliujc 10:332a4ad5911d 77
jasonliujc 4:d59328f14363 78 timer.reset();
jasonliujc 4:d59328f14363 79 setprevErr(error);
jasonliujc 4:d59328f14363 80 correction=kd*dErr/dt;
jasonliujc 4:d59328f14363 81
jasonliujc 4:d59328f14363 82
jasonliujc 4:d59328f14363 83 return correction;
jasonliujc 4:d59328f14363 84
ahmedallibhoy 1:45f1f67eab62 85 }
jasonliujc 4:d59328f14363 86
jasonliujc 4:d59328f14363 87
jasonliujc 5:0975b797bf54 88 float ctrl::ictrl(){
jasonliujc 5:0975b797bf54 89 floategrater+=error;
jasonliujc 4:d59328f14363 90
jasonliujc 5:0975b797bf54 91 correction=ki*floategrater;
jasonliujc 5:0975b797bf54 92 floategrater/=decay;
jasonliujc 4:d59328f14363 93 return correction;
jasonliujc 4:d59328f14363 94
jasonliujc 4:d59328f14363 95 }
jasonliujc 4:d59328f14363 96
jasonliujc 5:0975b797bf54 97 float ctrl::total(){
jasonliujc 10:332a4ad5911d 98
jasonliujc 10:332a4ad5911d 99
jasonliujc 10:332a4ad5911d 100 pc.printf("p: %f \t d: %f \t i: %f \t \n ", pctrl(),dctrl(),ictrl());
jasonliujc 10:332a4ad5911d 101
jasonliujc 10:332a4ad5911d 102
jasonliujc 4:d59328f14363 103 return pctrl()+dctrl()+ictrl();
jasonliujc 4:d59328f14363 104
jasonliujc 4:d59328f14363 105
jasonliujc 4:d59328f14363 106
jasonliujc 4:d59328f14363 107
jasonliujc 4:d59328f14363 108 }
jasonliujc 4:d59328f14363 109
jasonliujc 8:1ce513eff545 110 void ctrl::adjust(float xSpeed,float L, float R){
jasonliujc 4:d59328f14363 111
jasonliujc 4:d59328f14363 112
jasonliujc 5:0975b797bf54 113 TErr=xSpeed-(L+R);
jasonliujc 5:0975b797bf54 114 RErr=rotationError();
jasonliujc 5:0975b797bf54 115
jasonliujc 5:0975b797bf54 116 }
jasonliujc 6:95722ada4706 117
jasonliujc 6:95722ada4706 118
jasonliujc 8:1ce513eff545 119 void ctrl::updateErr(float e){
jasonliujc 6:95722ada4706 120
jasonliujc 6:95722ada4706 121 setprevErr(error);
jasonliujc 6:95722ada4706 122 error=e;
jasonliujc 6:95722ada4706 123
jasonliujc 6:95722ada4706 124 }
jasonliujc 6:95722ada4706 125
jasonliujc 5:0975b797bf54 126
jasonliujc 5:0975b797bf54 127 float ctrl::getT(){
jasonliujc 5:0975b797bf54 128
jasonliujc 5:0975b797bf54 129 return TErr;
jasonliujc 5:0975b797bf54 130 }
jasonliujc 4:d59328f14363 131
jasonliujc 4:d59328f14363 132
jasonliujc 5:0975b797bf54 133 float ctrl::getR(){
jasonliujc 5:0975b797bf54 134 return RErr;
jasonliujc 5:0975b797bf54 135
jasonliujc 5:0975b797bf54 136 }