A simple OO PID library

git-svn-id: https://arducopter.googlecode.com/svn/trunk@742 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/PID/PID.cpp

@@ -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));
+	*/
+}

libraries/PID/PID.h

@@ -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

libraries/PID/examples/pid/pid.pde

@@ -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);
+}

libraries/PID/keywords.txt

@@ -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