Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Controller working.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1659 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-02-15 00:57:23 +00:00
parent 6788def679
commit 0cfa2dbea1
4 changed files with 106 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
libraries/AP_Controller/AP_Controller.h
View File

@ -26,8 +26,6 @@
#include <APM_RC.h> #include <APM_RC.h>
#define APVarPtr2Float(var) (*(AP_Meta_class::meta_cast<AP_Float>((AP_Var *)var)))
/// Block /// Block
class Block class Block
{ {
@ -36,16 +34,16 @@ public:
_input(), _output() _input(), _output()
{ {
} }
virtual void update(const float & dt = 0) = 0; virtual void update(const float & dt) = 0;
virtual void connect( Block * block) virtual void connect( Block * block)
{ {
} }
const char * getName() { return _name; } const char * getName() { return _name; }
const Vector < AP_Var * > & getOutput() const { return _output; } const Vector < AP_Float * > & getOutput() const { return _output; }
protected: protected:
const char * _name; const char * _name;
Vector< const AP_Var * > _input; Vector< const AP_Float * > _input;
Vector< AP_Var * > _output; Vector< AP_Float * > _output;
}; };
/// Servo Block /// Servo Block
@ -63,7 +61,9 @@ public:
virtual void update(const float & dt = 0) virtual void update(const float & dt = 0)
{ {
if (_input.getSize() > 0) if (_input.getSize() > 0)
_ch->setNormalized(APVarPtr2Float(_input[0])); {
_ch->setNormalized(_input[0]->get());
}
} }
private: private:
float _position; float _position;
@ -77,28 +77,28 @@ public:
/// Constructor that allows 1-8 sum gain pairs, more /// Constructor that allows 1-8 sum gain pairs, more
/// can be added if necessary /// can be added if necessary
SumGain( SumGain(
AP_Var & var1 = AP_Float_zero, AP_Var & gain1 = AP_Float_zero, AP_Float & var1 = AP_Float_zero, AP_Float & gain1 = AP_Float_zero,
AP_Var & var2 = AP_Float_zero, AP_Var & gain2 = AP_Float_zero, AP_Float & var2 = AP_Float_zero, AP_Float & gain2 = AP_Float_zero,
AP_Var & var3 = AP_Float_zero, AP_Var & gain3 = AP_Float_zero, AP_Float & var3 = AP_Float_zero, AP_Float & gain3 = AP_Float_zero,
AP_Var & var4 = AP_Float_zero, AP_Var & gain4 = AP_Float_zero, AP_Float & var4 = AP_Float_zero, AP_Float & gain4 = AP_Float_zero,
AP_Var & var5 = AP_Float_zero, AP_Var & gain5 = AP_Float_zero, AP_Float & var5 = AP_Float_zero, AP_Float & gain5 = AP_Float_zero,
AP_Var & var6 = AP_Float_zero, AP_Var & gain6 = AP_Float_zero, AP_Float & var6 = AP_Float_zero, AP_Float & gain6 = AP_Float_zero,
AP_Var & var7 = AP_Float_zero, AP_Var & gain7 = AP_Float_zero, AP_Float & var7 = AP_Float_zero, AP_Float & gain7 = AP_Float_zero,
AP_Var & var8 = AP_Float_zero, AP_Var & gain8 = AP_Float_zero) AP_Float & var8 = AP_Float_zero, AP_Float & gain8 = AP_Float_zero)
{ {
if ( (&var1 == &AP_Float_zero) || (&gain1 == &AP_Float_zero) ) add(var1,gain1); if ( (&var1 != &AP_Float_zero) && (&gain1 != &AP_Float_zero) ) add(var1,gain1);
if ( (&var2 == &AP_Float_zero) || (&gain2 == &AP_Float_zero) ) add(var2,gain2); if ( (&var2 != &AP_Float_zero) && (&gain2 != &AP_Float_zero) ) add(var2,gain2);
if ( (&var3 == &AP_Float_zero) || (&gain3 == &AP_Float_zero) ) add(var3,gain3); if ( (&var3 != &AP_Float_zero) && (&gain3 != &AP_Float_zero) ) add(var3,gain3);
if ( (&var4 == &AP_Float_zero) || (&gain4 == &AP_Float_zero) ) add(var4,gain4); if ( (&var4 != &AP_Float_zero) && (&gain4 != &AP_Float_zero) ) add(var4,gain4);
if ( (&var5 == &AP_Float_zero) || (&gain5 == &AP_Float_zero) ) add(var5,gain5); if ( (&var5 != &AP_Float_zero) && (&gain5 != &AP_Float_zero) ) add(var5,gain5);
if ( (&var6 == &AP_Float_zero) || (&gain6 == &AP_Float_zero) ) add(var6,gain6); if ( (&var6 != &AP_Float_zero) && (&gain6 != &AP_Float_zero) ) add(var6,gain6);
if ( (&var7 == &AP_Float_zero) || (&gain7 == &AP_Float_zero) ) add(var7,gain7); if ( (&var7 != &AP_Float_zero) && (&gain7 != &AP_Float_zero) ) add(var7,gain7);
if ( (&var8 == &AP_Float_zero) || (&gain8 == &AP_Float_zero) ) add(var8,gain8); if ( (&var8 != &AP_Float_zero) && (&gain8 != &AP_Float_zero) ) add(var8,gain8);
// create output // create output
_output.push_back(new AP_Float(0)); _output.push_back(new AP_Float(0));
} }
void add(AP_Var & var, AP_Var & gain) void add(AP_Float & var, AP_Float & gain)
{ {
_input.push_back(&var); _input.push_back(&var);
_gain.push_back(&gain); _gain.push_back(&gain);
@ -106,14 +106,12 @@ public:
virtual void update(const float & dt = 0) virtual void update(const float & dt = 0)
{ {
if (_output.getSize() < 1) return; if (_output.getSize() < 1) return;
_output[0]=0; float sum =0;
for (int i=0;i<_input.getSize();i++) for (int i=0;i<_input.getSize();i++) sum += _input[i]->get() * _gain[i]->get() ;
{ _output[0]->set(sum);
*_output[0] = APVarPtr2Float(_output[i]) * APVarPtr2Float(_gain[i]) ;
}
} }
private: private:
Vector< AP_Var * > _gain; Vector< AP_Float * > _gain;
}; };
/// PID block /// PID block
@ -142,38 +140,44 @@ public:
if (block->getOutput().getSize() > 0) if (block->getOutput().getSize() > 0)
_input.push_back(block->getOutput()[0]); _input.push_back(block->getOutput()[0]);
} }
virtual void update(const float & dt = 0) virtual void update(const float & dt)
{ {
if (_output.getSize() < 1 || !_input[0] || !_output[0]) return; if (_output.getSize() < 1 || (!_input[0]) || (!_output[0]) ) return;
// derivative // derivative with low pass
float RC = 1/(2*M_PI*_fCut); // low pass filter float RC = 1/(2*M_PI*_fCut); // low pass filter
_eD = _eD + ( ((_e - APVarPtr2Float(_input[0])))/dt - _eD ) * (dt / (dt + RC)); _eD = _eD + ( ( _eP - _input[0]->get() )/dt - _eD ) * (dt / (dt + RC));
// proportional, note must come after derivative // proportional, note must come after derivative
// because derivatve uses _e as previous value // because derivatve uses _eP as previous value
_e = APVarPtr2Float(_input[0]); _eP = _input[0]->get();
// integral // integral
_eI += _e*dt; _eI += _eP*dt;
// wind up guard
if (_eI > _iMax) _eI = _iMax;
else if (_eI < -_iMax) _eI = -_iMax;
// pid sum // pid sum
*_output[0] = _kP*_e + _kI*_eI + _kD*_eD; _output[0]->set(_kP*_eP + _kI*_eI + _kD*_eD);
Serial.println("debug");
Serial.println(_kP);
Serial.println(_kI);
Serial.println(_kD);
Serial.println(_e);
Serial.println(_eI);
Serial.println(_eD);
Serial.println(APVarPtr2Float(_output[0]));
// debug output
/*Serial.println("kP, kI, kD: ");
Serial.print(_kP,5); Serial.print(" ");
Serial.print(_kI,5); Serial.print(" ");
Serial.println(_kD,5);
Serial.print("eP, eI, eD: ");
Serial.print(_eP,5); Serial.print(" ");
Serial.print(_eI,5); Serial.print(" ");
Serial.println(_eD,5);
Serial.print("input: ");
Serial.println(_input[0]->get(),5);
Serial.print("output: ");
Serial.println(_output[0]->get(),5);*/
} }
private: private:
float _e; /// input float _eP; /// input
float _eI; /// integral of input float _eI; /// integral of input
float _eD; /// derivative of input float _eD; /// derivative of input
AP_Float _kP; /// proportional gain AP_Float _kP; /// proportional gain
@ -190,10 +194,10 @@ public:
void addBlock(Block * block) void addBlock(Block * block)
{ {
if (_blocks.getSize() > 0) if (_blocks.getSize() > 0)
_blocks[_blocks.getSize()-1]->connect(block); block->connect(_blocks[_blocks.getSize()-1]);
_blocks.push_back(block); _blocks.push_back(block);
} }
void update(const double dt=0) void update(const double dt)
{ {
for (int i=0;i<_blocks.getSize();i++) for (int i=0;i<_blocks.getSize();i++)
{ {

57
libraries/AP_Controller/examples/Car/Car.pde
View File

@ -10,12 +10,12 @@ class CarController : public AP_Controller
{ {
private: private:
// state // state
AP_Var & velocity; AP_Float & velocity;
AP_Var & heading; AP_Float & heading;
// control variables // control variables
AP_Var & headingCommand; AP_Float & headingCommand;
AP_Var & velocityCommand; AP_Float & velocityCommand;
// channels // channels
AP_RcChannel * steeringCh; AP_RcChannel * steeringCh;
@ -23,22 +23,22 @@ private:
public: public:
CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey, CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey,
AP_Var & heading, AP_Var & velocity, AP_Float & heading, AP_Float & velocity,
AP_Var & headingCommand, AP_Var & velocityCommand, AP_Float & headingCommand, AP_Float & velocityCommand,
AP_RcChannel * steeringCh, AP_RcChannel * throttleCh) : AP_RcChannel * steeringCh, AP_RcChannel * throttleCh) :
heading(heading), velocity(velocity), heading(heading), velocity(velocity),
headingCommand(headingCommand), velocityCommand(velocityCommand), headingCommand(headingCommand), velocityCommand(velocityCommand),
steeringCh(steeringCh), throttleCh(throttleCh) steeringCh(steeringCh), throttleCh(throttleCh)
{ {
// steering control loop // steering control loop
addBlock(new SumGain(headingCommand,AP_Float_unity,heading,AP_Float_negative_unity)); addBlock(new SumGain(headingCommand,AP_Float_unity,heading,AP_Float_negative_unity));
addBlock(new Pid(pidStrKey,PSTR("STR"),0.1,0,0,1,20)); addBlock(new Pid(pidStrKey,PSTR("STR"),1,1,1,1,20));
addBlock(new ToServo(steeringCh)); addBlock(new ToServo(steeringCh));
// throttle control loop // throttle control loop
addBlock(new SumGain(velocityCommand,AP_Float_unity,velocity,AP_Float_negative_unity)); addBlock(new SumGain(velocityCommand,AP_Float_unity,velocity,AP_Float_negative_unity));
addBlock(new Pid(pidThrKey,PSTR("THR"),0.1,0,0,1,20)); addBlock(new Pid(pidThrKey,PSTR("THR"),1,1,1,1,20));
addBlock(new ToServo(throttleCh)); addBlock(new ToServo(throttleCh));
} }
}; };
@ -48,6 +48,8 @@ Vector<AP_RcChannel * > ch;
AP_Float heading, velocity; // measurements AP_Float heading, velocity; // measurements
AP_Float headingCommand, velocityCommand; // guidance data AP_Float headingCommand, velocityCommand; // guidance data
int8_t sign = 1;
float value = 0;
enum keys enum keys
{ {
@ -65,13 +67,13 @@ void setup()
// variables // variables
heading = 0; heading = 0;
velocity = 0; velocity = 0;
headingCommand = 1; headingCommand = 0;
velocityCommand = 1; velocityCommand = 0;
// rc channels // rc channels
APM_RC.Init(); APM_RC.Init();
ch.push_back(new AP_RcChannel(chStrKey,PSTR("STR"),APM_RC,1,45)); ch.push_back(new AP_RcChannel(chStrKey,PSTR("STR"),APM_RC,1,45));
ch.push_back(new AP_RcChannel(chThrKey,PSTR("THR"),APM_RC,2,100)); ch.push_back(new AP_RcChannel(chThrKey,PSTR("THR"),APM_RC,2,45));
// controller // controller
controller = new CarController(pidStrKey,pidThrKey,heading,velocity,headingCommand,velocityCommand,ch[0],ch[1]); controller = new CarController(pidStrKey,pidThrKey,heading,velocity,headingCommand,velocityCommand,ch[0],ch[1]);
@ -79,13 +81,30 @@ void setup()
void loop() void loop()
{ {
Serial.println("Looping"); // loop rate 20 Hz
heading += 0.1; delay(1000.0/20);
velocity += 0.1;
delay(1000); // feedback signal, 1/3 Hz sawtooth
controller->update(.1); if (value > 1)
Serial.printf("heading:\t%f\tcommand(%f)\tservo(%f)\n", (float)heading,(float) headingCommand, ch[0]->getNormalized()); {
Serial.printf("velocity:\t%f\tcommand(%f)\tservo(%f)\n", (float)velocity, (float) velocityCommand, ch[1]->getNormalized()); value = 1;
sign = -1;
}
else if (value < -1)
{
value = -1;
sign = 1;
}
value += sign/20.0/3.0;
velocity = value;
heading = value;
// update controller
controller->update(1./20);
// output
Serial.printf("hdng:%f\tcmd:(%f)\tservo:%f\t", heading.get(),headingCommand.get(), ch[0]->getNormalized());
Serial.printf("| thr:%f\tcmd:(%f)\tservo:%f\n", velocity.get(), velocityCommand.get(), ch[1]->getNormalized());
} }
// vim:ts=4:sw=4:expandtab // vim:ts=4:sw=4:expandtab

12
libraries/AP_RcChannel/AP_RcChannel.cpp
View File

@ -32,7 +32,9 @@ AP_RcChannel::AP_RcChannel(AP_Var::Key key, const prog_char * name, APM_RC_Class
filter(this,7,filter,PSTR("FLTR")), filter(this,7,filter,PSTR("FLTR")),
reverse(this,8,reverse,PSTR("REV")), reverse(this,8,reverse,PSTR("REV")),
_pwm(0) _pwm(0)
{ } {
setNormalized(0.0);
}
void AP_RcChannel::readRadio() { void AP_RcChannel::readRadio() {
@ -74,9 +76,17 @@ AP_RcChannel::setPwm(uint16_t pwm)
void void
AP_RcChannel::setPosition(float position) AP_RcChannel::setPosition(float position)
{ {
if (position > scale) position = scale;
else if (position < -scale) position = -scale;
setPwm(_positionToPwm(position)); setPwm(_positionToPwm(position));
} }
void
AP_RcChannel::setNormalized(float normPosition)
{
setPosition(normPosition*scale);
}
void void
AP_RcChannel::mixRadio(uint16_t infStart) AP_RcChannel::mixRadio(uint16_t infStart)
{ {

2
libraries/AP_RcChannel/AP_RcChannel.h
View File

@ -40,7 +40,7 @@ public:
void readRadio(); void readRadio();
void setPwm(uint16_t pwm); void setPwm(uint16_t pwm);
void setPosition(float position); void setPosition(float position);
void setNormalized(float normPosition) { setPosition(normPosition*scale); } void setNormalized(float normPosition);
void mixRadio(uint16_t infStart); void mixRadio(uint16_t infStart);
// get // get