Clark Lin / Mbed 2 deprecated 1407102

AVGG

Dependencies:   mbed

Fork of 7_7Boboobooo by 譯文 張

controller.cpp@7:f04bde0ca846, 2014-07-07 (annotated)

Committer:
even
Date:
Mon Jul 07 15:23:50 2014 +0000
Revision:
7:f04bde0ca846
half circle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
even 7:f04bde0ca846 1 #include "mbed.h"
even 7:f04bde0ca846 2 #include "controller.h"
even 7:f04bde0ca846 3
even 7:f04bde0ca846 4
even 7:f04bde0ca846 5
even 7:f04bde0ca846 6
even 7:f04bde0ca846 7 PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
even 7:f04bde0ca846 8
even 7:f04bde0ca846 9 //BX tune
even 7:f04bde0ca846 10 Kp = 0.0004;
even 7:f04bde0ca846 11 Ki = 0.0;
even 7:f04bde0ca846 12 Kd = 0.0;
even 7:f04bde0ca846 13 setInputLimits(in_min,in_max);
even 7:f04bde0ca846 14 setOutputLimits(out_min,out_max);
even 7:f04bde0ca846 15
even 7:f04bde0ca846 16 tSample_ = interval;
even 7:f04bde0ca846 17
even 7:f04bde0ca846 18 setTunings(Kc, tauI, tauD);
even 7:f04bde0ca846 19
even 7:f04bde0ca846 20 setPoint_ = 0.0;
even 7:f04bde0ca846 21 processVariable_ = 0.0;
even 7:f04bde0ca846 22 prevProcessVariable_ = 0.0;
even 7:f04bde0ca846 23 controllerOutput_ = 0.0;
even 7:f04bde0ca846 24 prevControllerOutput_ = 0.0;
even 7:f04bde0ca846 25
even 7:f04bde0ca846 26
even 7:f04bde0ca846 27
even 7:f04bde0ca846 28 }
even 7:f04bde0ca846 29
even 7:f04bde0ca846 30 void PID::setInputLimits(float inMin, float inMax) {
even 7:f04bde0ca846 31
even 7:f04bde0ca846 32 //Make sure we haven't been given impossible values.
even 7:f04bde0ca846 33 if (inMin >= inMax) {
even 7:f04bde0ca846 34 return;
even 7:f04bde0ca846 35 }
even 7:f04bde0ca846 36
even 7:f04bde0ca846 37
even 7:f04bde0ca846 38
even 7:f04bde0ca846 39
even 7:f04bde0ca846 40
even 7:f04bde0ca846 41 inMin_ = inMin;
even 7:f04bde0ca846 42 inMax_ = inMax;
even 7:f04bde0ca846 43 inSpan_ = (inMax - inMin);
even 7:f04bde0ca846 44
even 7:f04bde0ca846 45
even 7:f04bde0ca846 46
even 7:f04bde0ca846 47
even 7:f04bde0ca846 48 }
even 7:f04bde0ca846 49
even 7:f04bde0ca846 50 void PID::setOutputLimits(float outMin, float outMax) {
even 7:f04bde0ca846 51
even 7:f04bde0ca846 52 //Make sure we haven't been given impossible values.
even 7:f04bde0ca846 53 if (outMin >= outMax) {
even 7:f04bde0ca846 54 return;
even 7:f04bde0ca846 55 }
even 7:f04bde0ca846 56
even 7:f04bde0ca846 57
even 7:f04bde0ca846 58
even 7:f04bde0ca846 59 //ppp
even 7:f04bde0ca846 60
even 7:f04bde0ca846 61 outMin_ = outMin;
even 7:f04bde0ca846 62 outMax_ = outMax;
even 7:f04bde0ca846 63 outMid_ = (outMin+outMax)/2;
even 7:f04bde0ca846 64
even 7:f04bde0ca846 65 outSpan_ = (outMax - outMin);
even 7:f04bde0ca846 66
even 7:f04bde0ca846 67
even 7:f04bde0ca846 68
even 7:f04bde0ca846 69
even 7:f04bde0ca846 70
even 7:f04bde0ca846 71
even 7:f04bde0ca846 72 }
even 7:f04bde0ca846 73
even 7:f04bde0ca846 74
even 7:f04bde0ca846 75
even 7:f04bde0ca846 76 //--------------------------------------------------
even 7:f04bde0ca846 77
even 7:f04bde0ca846 78
even 7:f04bde0ca846 79
even 7:f04bde0ca846 80 void PID::setTunings(float Kc, float tauI, float tauD) {
even 7:f04bde0ca846 81
even 7:f04bde0ca846 82 //Verify that the tunings make sense.
even 7:f04bde0ca846 83 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
even 7:f04bde0ca846 84 return;
even 7:f04bde0ca846 85 }
even 7:f04bde0ca846 86
even 7:f04bde0ca846 87 //Store raw values to hand back to user on request.
even 7:f04bde0ca846 88 pParam_ = Kc;
even 7:f04bde0ca846 89 iParam_ = tauI;
even 7:f04bde0ca846 90 dParam_ = tauD;
even 7:f04bde0ca846 91
even 7:f04bde0ca846 92
even 7:f04bde0ca846 93
even 7:f04bde0ca846 94
even 7:f04bde0ca846 95
even 7:f04bde0ca846 96 Kc_ = Kc;
even 7:f04bde0ca846 97 tauI_=tauI;
even 7:f04bde0ca846 98 tauD_=tauD;
even 7:f04bde0ca846 99
even 7:f04bde0ca846 100 }
even 7:f04bde0ca846 101
even 7:f04bde0ca846 102
even 7:f04bde0ca846 103
even 7:f04bde0ca846 104
even 7:f04bde0ca846 105
even 7:f04bde0ca846 106
even 7:f04bde0ca846 107
even 7:f04bde0ca846 108
even 7:f04bde0ca846 109
even 7:f04bde0ca846 110
even 7:f04bde0ca846 111
even 7:f04bde0ca846 112
even 7:f04bde0ca846 113
even 7:f04bde0ca846 114
even 7:f04bde0ca846 115
even 7:f04bde0ca846 116
even 7:f04bde0ca846 117
even 7:f04bde0ca846 118
even 7:f04bde0ca846 119
even 7:f04bde0ca846 120
even 7:f04bde0ca846 121
even 7:f04bde0ca846 122
even 7:f04bde0ca846 123 float PID::compute(float center , float sp) {
even 7:f04bde0ca846 124 //turn right 122~64 122
even 7:f04bde0ca846 125 //turn left 64~6 8
even 7:f04bde0ca846 126 float C = center;
even 7:f04bde0ca846 127 float goal = sp; // center of black
even 7:f04bde0ca846 128 float error = goal - C;//
even 7:f04bde0ca846 129 return 0.073+Kp*error;
even 7:f04bde0ca846 130
even 7:f04bde0ca846 131 }
even 7:f04bde0ca846 132
even 7:f04bde0ca846 133 float PID::getInMin() {
even 7:f04bde0ca846 134
even 7:f04bde0ca846 135 return inMin_;
even 7:f04bde0ca846 136
even 7:f04bde0ca846 137 }
even 7:f04bde0ca846 138
even 7:f04bde0ca846 139 float PID::getInMax() {
even 7:f04bde0ca846 140
even 7:f04bde0ca846 141 return inMax_;
even 7:f04bde0ca846 142
even 7:f04bde0ca846 143 }
even 7:f04bde0ca846 144
even 7:f04bde0ca846 145 float PID::getOutMin() {
even 7:f04bde0ca846 146
even 7:f04bde0ca846 147 return outMin_;
even 7:f04bde0ca846 148
even 7:f04bde0ca846 149 }
even 7:f04bde0ca846 150
even 7:f04bde0ca846 151 float PID::getOutMax() {
even 7:f04bde0ca846 152
even 7:f04bde0ca846 153 return outMax_;
even 7:f04bde0ca846 154
even 7:f04bde0ca846 155 }
even 7:f04bde0ca846 156
even 7:f04bde0ca846 157 float PID::getInterval() {
even 7:f04bde0ca846 158
even 7:f04bde0ca846 159 return tSample_;
even 7:f04bde0ca846 160
even 7:f04bde0ca846 161 }
even 7:f04bde0ca846 162
even 7:f04bde0ca846 163 float PID::getPParam() {
even 7:f04bde0ca846 164
even 7:f04bde0ca846 165 return pParam_;
even 7:f04bde0ca846 166
even 7:f04bde0ca846 167 }
even 7:f04bde0ca846 168
even 7:f04bde0ca846 169 float PID::getIParam() {
even 7:f04bde0ca846 170
even 7:f04bde0ca846 171 return iParam_;
even 7:f04bde0ca846 172
even 7:f04bde0ca846 173 }
even 7:f04bde0ca846 174
even 7:f04bde0ca846 175 float PID::getDParam() {
even 7:f04bde0ca846 176
even 7:f04bde0ca846 177 return dParam_;
even 7:f04bde0ca846 178
even 7:f04bde0ca846 179 }