diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp new file mode 100644 index 0000000000..c4195eb316 --- /dev/null +++ b/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)); + */ +} \ No newline at end of file diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h new file mode 100644 index 0000000000..4e304283fb --- /dev/null +++ b/libraries/PID/PID.h @@ -0,0 +1,37 @@ +#ifndef PID_h +#define PID_h + +#include +#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 \ No newline at end of file diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde new file mode 100644 index 0000000000..ed1d1fe3a4 --- /dev/null +++ b/libraries/PID/examples/pid/pid.pde @@ -0,0 +1,38 @@ +/* + Example of PID library. + 2010 Code by Jason Short. DIYDrones.com +*/ + +#include // ArduPilot Mega RC Library +#include // 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); +} \ No newline at end of file diff --git a/libraries/PID/keywords.txt b/libraries/PID/keywords.txt new file mode 100644 index 0000000000..3efcc0fa52 --- /dev/null +++ b/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