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 float RC = 1/(2*M_PI*_fCut);
00033 derivative = _last_derivative +
00034 (delta_time / (RC + delta_time)) * (derivative - _last_derivative);
00035
00036
00037 _last_error = error;
00038 _last_derivative = derivative;
00039
00040
00041 output += _kd * derivative;
00042 }
00043
00044
00045 output *= scaler;
00046
00047
00048 if (_ki && (dt > 0)) {
00049 _integrator += (error * _ki) * scaler * delta_time;
00050 if (_integrator < -_imax) {
00051 _integrator = -_imax;
00052 } else if (_integrator > _imax) {
00053 _integrator = _imax;
00054 }
00055 output += _integrator;
00056 }
00057
00058 return output;
00059 }
00060
00061 void
00062 PID::imax(float v)
00063 {
00064 _imax = fabs(v);
00065 }
00066
00067 void
00068 PID::load_gains()
00069 {
00070 switch(_storage)
00071 {
00072 case STORE_OFF:
00073
00074 break;
00075 case STORE_EEPROM_UINT16:
00076 _kp = (float)(eeprom_read_word((uint16_t *) _address)) / 1000.0;
00077 _ki = (float)(eeprom_read_word((uint16_t *) (_address + 2))) / 1000.0;
00078 _kd = (float)(eeprom_read_word((uint16_t *) (_address + 4))) / 1000.0;
00079 _imax = eeprom_read_word((uint16_t *) (_address + 6)) * 100;
00080
00081
00082
00083
00084
00085
00086 break;
00087
00088 case STORE_EEPROM_FLOAT:
00089
00090
00091
00092
00093 break;
00094 }
00095 }
00096
00097 void
00098 PID::save_gains()
00099 {
00100 switch(_storage)
00101 {
00102 case STORE_OFF:
00103
00104 break;
00105 case STORE_EEPROM_UINT16:
00106 eeprom_write_word((uint16_t *) _address, (int)(_kp * 1000));
00107 eeprom_write_word((uint16_t *) (_address + 2), (int)(_ki * 1000));
00108 eeprom_write_word((uint16_t *) (_address + 4), (int)(_kd * 1000));
00109 eeprom_write_word((uint16_t *) (_address + 6), (int)_imax/100);
00110
00111
00112
00113
00114
00115
00116 break;
00117 case STORE_EEPROM_FLOAT:
00118
00119
00120
00121
00122 break;
00123 }
00124 }
00125