A simple OO PID library

git-svn-id: https://arducopter.googlecode.com/svn/trunk@742 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-10-28 04:59:24 +00:00
parent 610609f98d
commit e7b26da27d
4 changed files with 269 additions and 0 deletions

186
libraries/PID/PID.cpp Normal file
View File

@ -0,0 +1,186 @@
#include "PID.h"
PID::PID()
{
}
long
PID::get_pid(long err, long dt, float scaler)
{
// Positive error produces positive output
float output;
float error = (float)err;
float delta_time = (float)dt/1000.0;
// Compute proportional component
if(_kp != 0){
output += (_kp * error);
}
if(_kd != 0){
// Compute derivative component
//derivative = (error - previous_error)/dt
float derivative = (error - _last_error) / dt;
//Serial.print("d: ");
//Serial.println(derivative,DEC);
_last_error = error;
output += _kd * derivative; // Sum the derivative component
}
output *= scaler;
// Compute integral component
if(_ki != 0){
_integrator += (error * _ki) * scaler * dt *.001;
_integrator = constrain(_integrator, -_imax, _imax);
output += _integrator;
}
return output;
}
void
PID::reset_I(void)
{
_integrator = 0;
_last_error = 0;
}
/// setters
void
PID::set_P(float p)
{
_kp = p;
}
void
PID::set_I(float i)
{
_ki = i;
}
void
PID::set_D(float d)
{
_kd = d;
}
void
PID::set_imax(int imax)
{
_imax = imax;
Serial.print("set imax ");
Serial.println(_imax, DEC);
}
/// getters
float
PID::get_P(void)
{
return _kp;
}
float
PID::get_I(void)
{
return _ki;
}
float
PID::get_D(void)
{
return _kd;
}
int
PID::get_imax(void)
{
return _imax;
}
void
PID::load_gains(int address)
{
//Serial.println("load gains ");
//Serial.println(address, DEC);
_kp = (float)(eeprom_read_word((uint16_t *) address)) / 1000.0;
_ki = (float)(eeprom_read_word((uint16_t *) (address + 2))) / 1000.0;
_kd = (float)(eeprom_read_word((uint16_t *) (address + 4))) / 1000.0;
_imax = eeprom_read_word((uint16_t *) (address + 6)) * 100;
}
void
PID::save_gains(int address)
{
eeprom_write_word((uint16_t *) address, (int)(_kp * 1000));
eeprom_write_word((uint16_t *) (address + 2), (int)(_ki * 1000));
eeprom_write_word((uint16_t *) (address + 4), (int)(_kd * 1000));
eeprom_write_word((uint16_t *) (address + 6), (int)_imax/100);
}
/*
float
read_EE_compressed_float(int address, byte places)
{
float scale = places * 10;
int temp = eeprom_read_word((uint16_t *) address);
return ((float)temp / scale);
}
void write_EE_compressed_float(float value, int address, byte places)
{
float scale = places * 10;
int temp = value * scale;
eeprom_write_word((uint16_t *) address, temp);
}
*/
void
PID::test(void)
{
/*
int address = 0x46;
Serial.print("imax= ");
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.print(eeprom_read_word((uint16_t *) address));
Serial.print(" ");
address += 8;
Serial.println(eeprom_read_word((uint16_t *) address));
*/
}

37
libraries/PID/PID.h Normal file
View File

@ -0,0 +1,37 @@
#ifndef PID_h
#define PID_h
#include <avr/eeprom.h>
#include "WProgram.h"
//#define PID_SIZE 8
class PID {
public:
PID();
long get_pid(long err, long dt, float scaler);
void reset_I();
void load_gains(int address);
void save_gains(int address);
void set_P(float p);
void set_I(float i);
void set_D(float d);
void set_imax(int imax);
void test(void);
float get_P(void);
float get_I(void);
float get_D(void);
int get_imax(void);
float _imax; // integrator max
float _integrator; // integrator value
float _last_error; // last error for derivative
float _kp;
float _ki;
float _kd;
private:
};
#endif

View File

@ -0,0 +1,38 @@
/*
Example of PID library.
2010 Code by Jason Short. DIYDrones.com
*/
#include <PID.h> // ArduPilot Mega RC Library
#include <APM_RC.h> // ArduPilot RC Library
PID pid;
long radio_in;
long radio_trim;
void setup()
{
Serial.begin(38400);
Serial.println("ArduPilot Mega PID library test");
APM_RC.Init(); // APM Radio initialization
delay(1000);
//rc.trim();
radio_trim = APM_RC.InputCh(0);
pid.set_P(1);
pid.set_I(0);
pid.set_D(.5);
pid.set_imax(50);
}
void loop()
{
delay(20);
//rc.read_pwm();
long error = APM_RC.InputCh(0) - radio_trim;
long control = pid.get_pid(error, 20, 1);
Serial.print("control: ");
Serial.println(control,DEC);
}

View File

@ -0,0 +1,8 @@
APM_RC KEYWORD1
begin KEYWORD2
InputCh KEYWORD2
OutputCh KEYWORD2
GetState KEYWORD2
Force_Out0_Out1 KEYWORD2
Force_Out2_Out3 KEYWORD2
Force_Out6_Out7 KEYWORD2