Go to the documentation of this file.00001
00002
00005
00006 #include <math.h>
00007 #include <avr/eeprom.h>
00008
00009 #include "PID.h"
00010
00011
00012 #define _kp _gain_array[0]
00013 #define _ki _gain_array[1]
00014 #define _kd _gain_array[2]
00015 #define _imax _gain_array[3]
00016
00017 long
00018 PID::get_pid(int32_t error, uint16_t dt, float scaler)
00019 {
00020 float output = 0;
00021 float delta_time = (float)dt / 1000.0;
00022
00023
00024 output += error * _kp;
00025
00026
00027 if (_kd && (dt > 0)) {
00028 float derivative = (error - _last_error) / delta_time;
00029
00030
00031
00032 derivative = _last_derivative +
00033 ((delta_time / (_RC + delta_time)) * (derivative - _last_derivative));
00034
00035
00036 _last_error = error;
00037 _last_derivative = derivative;
00038
00039
00040 output += _kd * derivative;
00041 }
00042
00043
00044 output *= scaler;
00045
00046
00047 if (_ki && (dt > 0)) {
00048 _integrator += (error * _ki) * scaler * delta_time;
00049 if (_integrator < -_imax) {
00050 _integrator = -_imax;
00051 } else if (_integrator > _imax) {
00052 _integrator = _imax;
00053 }
00054 output += _integrator;
00055 }
00056
00057 return output;
00058 }
00059
00060 void
00061 PID::imax(float v)
00062 {
00063 _imax = fabs(v);
00064 }
00065
00066 void
00067 PID::load_gains()
00068 {
00069 switch(_storage)
00070 {
00071 case STORE_OFF:
00072
00073 break;
00074 case STORE_EEPROM_UINT16:
00075
00076
00077
00078
00079
00080 _kp = (float)_ee.read_int(_address) / 1000.0;
00081 _ki = (float)_ee.read_int(_address + 2) / 1000.0;
00082 _kd = (float)_ee.read_int(_address + 4) / 1000.0;
00083 _imax = (float)_ee.read_int(_address + 6) * 100;
00084
00085 break;
00086
00087 case STORE_EEPROM_FLOAT:
00088
00089
00090
00091
00092 break;
00093 }
00094 }
00095
00096 void
00097 PID::save_gains()
00098 {
00099 switch(_storage)
00100 {
00101 case STORE_OFF:
00102
00103 break;
00104 case STORE_EEPROM_UINT16:
00105
00106
00107
00108
00109
00110
00111 _ee.write_int(_address, (int)(_kp * 1000));
00112 _ee.write_int(_address + 2, (int)(_ki * 1000));
00113 _ee.write_int(_address + 4, (int)(_kd * 1000));
00114 _ee.write_int(_address + 6, (int)(_imax /100));
00115 break;
00116 case STORE_EEPROM_FLOAT:
00117
00118
00119
00120
00121 break;
00122 }
00123 }
00124