Secret_Betago / Mbed 2 deprecated Low_Level

first

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of BEAR_Motion by BE@R lab

pidcontrol.cpp@37:8719223998d8, 2016-06-07 (annotated)

Committer:
icyzkungz
Date:
Tue Jun 07 07:00:11 2016 +0000
Revision:
37:8719223998d8
Parent:
34:0cf04acfe422 
update protocol.h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 34:0cf04acfe422 1 #include "pidcontrol.h"
soulx 34:0cf04acfe422 2
soulx 34:0cf04acfe422 3 PID::PID()
soulx 34:0cf04acfe422 4 {
soulx 34:0cf04acfe422 5 Kp=1.0f;
soulx 34:0cf04acfe422 6 Ki=0.0f;
soulx 34:0cf04acfe422 7 Kd=0.0f;
soulx 34:0cf04acfe422 8 il=65535.0;
soulx 34:0cf04acfe422 9 margin = 0.0f;
soulx 34:0cf04acfe422 10
soulx 34:0cf04acfe422 11 }
soulx 34:0cf04acfe422 12
soulx 34:0cf04acfe422 13 PID::PID(float p,float i,float d)
soulx 34:0cf04acfe422 14 {
soulx 34:0cf04acfe422 15 Kp=p;
soulx 34:0cf04acfe422 16 Ki=i;
soulx 34:0cf04acfe422 17 Kd=d;
soulx 34:0cf04acfe422 18 il=65535.0;
soulx 34:0cf04acfe422 19 margin =0.0f;
soulx 34:0cf04acfe422 20 }
soulx 34:0cf04acfe422 21
soulx 34:0cf04acfe422 22 void PID::setGoal(float ref)
soulx 34:0cf04acfe422 23 {
soulx 34:0cf04acfe422 24 setpoint = ref;
soulx 34:0cf04acfe422 25 }
soulx 34:0cf04acfe422 26
soulx 34:0cf04acfe422 27 void PID::setCurrent(float sensor)
soulx 34:0cf04acfe422 28 {
soulx 34:0cf04acfe422 29 input = sensor;
soulx 34:0cf04acfe422 30 }
soulx 34:0cf04acfe422 31
soulx 34:0cf04acfe422 32 float PID::compute()
soulx 34:0cf04acfe422 33 {
soulx 34:0cf04acfe422 34
soulx 34:0cf04acfe422 35 e_n = setpoint - input;
soulx 34:0cf04acfe422 36
soulx 34:0cf04acfe422 37 if((e_i < il) && (e_i > -il))
soulx 34:0cf04acfe422 38 {
soulx 34:0cf04acfe422 39 e_i += e_n;
soulx 34:0cf04acfe422 40 }
soulx 34:0cf04acfe422 41 else
soulx 34:0cf04acfe422 42 {
soulx 34:0cf04acfe422 43 #ifdef PID_DEBUG
soulx 34:0cf04acfe422 44 printf("il overflow\n\r");
soulx 34:0cf04acfe422 45 #endif
soulx 34:0cf04acfe422 46 e_i =il;
soulx 34:0cf04acfe422 47 }
soulx 34:0cf04acfe422 48
soulx 34:0cf04acfe422 49
soulx 34:0cf04acfe422 50 output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1));
soulx 34:0cf04acfe422 51
soulx 34:0cf04acfe422 52 if(output > 0)
soulx 34:0cf04acfe422 53 {
soulx 34:0cf04acfe422 54 if(output < margin)
soulx 34:0cf04acfe422 55 {
soulx 34:0cf04acfe422 56 output = 0.0;
soulx 34:0cf04acfe422 57 }
soulx 34:0cf04acfe422 58 }
soulx 34:0cf04acfe422 59 else
soulx 34:0cf04acfe422 60 {
soulx 34:0cf04acfe422 61 if(output > -margin)
soulx 34:0cf04acfe422 62 {
soulx 34:0cf04acfe422 63 output = 0.0;
soulx 34:0cf04acfe422 64 }
soulx 34:0cf04acfe422 65 }
soulx 34:0cf04acfe422 66
soulx 34:0cf04acfe422 67 return output;
soulx 34:0cf04acfe422 68 }
soulx 34:0cf04acfe422 69
soulx 34:0cf04acfe422 70 void PID::setMargin(float gap)
soulx 34:0cf04acfe422 71 {
soulx 34:0cf04acfe422 72 margin =gap;
soulx 34:0cf04acfe422 73 }
soulx 34:0cf04acfe422 74
soulx 34:0cf04acfe422 75 float PID::getMargin()
soulx 34:0cf04acfe422 76 {
soulx 34:0cf04acfe422 77 return margin;
soulx 34:0cf04acfe422 78 }
soulx 34:0cf04acfe422 79
soulx 34:0cf04acfe422 80
soulx 34:0cf04acfe422 81 void PID::setIntegalLimit(float limit)
soulx 34:0cf04acfe422 82 {
soulx 34:0cf04acfe422 83 il = limit;
soulx 34:0cf04acfe422 84 }
soulx 34:0cf04acfe422 85 float PID::getIntegalLimit()
soulx 34:0cf04acfe422 86 {
soulx 34:0cf04acfe422 87 return il;
soulx 34:0cf04acfe422 88 }
soulx 34:0cf04acfe422 89
soulx 34:0cf04acfe422 90
soulx 34:0cf04acfe422 91 float PID::getErrorNow()
soulx 34:0cf04acfe422 92 {
soulx 34:0cf04acfe422 93 return e_n;
soulx 34:0cf04acfe422 94 }
soulx 34:0cf04acfe422 95
soulx 34:0cf04acfe422 96 float PID::getErrorLast()
soulx 34:0cf04acfe422 97 {
soulx 34:0cf04acfe422 98 return e_n_1;
soulx 34:0cf04acfe422 99 }
soulx 34:0cf04acfe422 100
soulx 34:0cf04acfe422 101 float PID::getErrorDiff()
soulx 34:0cf04acfe422 102 {
soulx 34:0cf04acfe422 103 return e_n - e_n_1;
soulx 34:0cf04acfe422 104 }
soulx 34:0cf04acfe422 105
soulx 34:0cf04acfe422 106 float PID::getErrorIntegal()
soulx 34:0cf04acfe422 107 {
soulx 34:0cf04acfe422 108 return e_i;
soulx 34:0cf04acfe422 109 }
soulx 34:0cf04acfe422 110
soulx 34:0cf04acfe422 111 void PID::setKp(float p)
soulx 34:0cf04acfe422 112 {
soulx 34:0cf04acfe422 113 if(p < 0.0f)
soulx 34:0cf04acfe422 114 {
soulx 34:0cf04acfe422 115 #ifdef PID_DEBUG
soulx 34:0cf04acfe422 116 printf("Kp = 0.0\n\r");
soulx 34:0cf04acfe422 117 #endif
soulx 34:0cf04acfe422 118 Kp=0.0;
soulx 34:0cf04acfe422 119 }
soulx 34:0cf04acfe422 120 else
soulx 34:0cf04acfe422 121 {
soulx 34:0cf04acfe422 122 Kp=p;
soulx 34:0cf04acfe422 123 }
soulx 34:0cf04acfe422 124 }
soulx 34:0cf04acfe422 125
soulx 34:0cf04acfe422 126 void PID::setKi(float i)
soulx 34:0cf04acfe422 127 {
soulx 34:0cf04acfe422 128 if(i < 0.0f)
soulx 34:0cf04acfe422 129 {
soulx 34:0cf04acfe422 130 #ifdef PID_DEBUG
soulx 34:0cf04acfe422 131 printf("Ki = 0.0\n\r");
soulx 34:0cf04acfe422 132 #endif
soulx 34:0cf04acfe422 133 Ki=0.0;
soulx 34:0cf04acfe422 134 }
soulx 34:0cf04acfe422 135 else
soulx 34:0cf04acfe422 136 {
soulx 34:0cf04acfe422 137 Ki=i;
soulx 34:0cf04acfe422 138 }
soulx 34:0cf04acfe422 139 }
soulx 34:0cf04acfe422 140 void PID::setKd(float d)
soulx 34:0cf04acfe422 141 {
soulx 34:0cf04acfe422 142 if(d < 0.0f)
soulx 34:0cf04acfe422 143 {
soulx 34:0cf04acfe422 144 #ifdef PID_DEBUG
soulx 34:0cf04acfe422 145 printf("Kd = 0.0\n\r");
soulx 34:0cf04acfe422 146 #endif
soulx 34:0cf04acfe422 147 Kd=0.0;
soulx 34:0cf04acfe422 148 }
soulx 34:0cf04acfe422 149 else
soulx 34:0cf04acfe422 150 {
soulx 34:0cf04acfe422 151 Kd=d;
soulx 34:0cf04acfe422 152 }
soulx 34:0cf04acfe422 153 }
soulx 34:0cf04acfe422 154
soulx 34:0cf04acfe422 155 float PID::getKp()
soulx 34:0cf04acfe422 156 {
soulx 34:0cf04acfe422 157 return Kp;
soulx 34:0cf04acfe422 158 }
soulx 34:0cf04acfe422 159
soulx 34:0cf04acfe422 160 float PID::getKi()
soulx 34:0cf04acfe422 161 {
soulx 34:0cf04acfe422 162 return Ki;
soulx 34:0cf04acfe422 163 }
soulx 34:0cf04acfe422 164
soulx 34:0cf04acfe422 165 float PID::getKd()
soulx 34:0cf04acfe422 166 {
soulx 34:0cf04acfe422 167 return Kd;
soulx 34:0cf04acfe422 168 }