Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -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 57892082b2
commit dd51139b36
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>
#define APVarPtr2Float(var) (*(AP_Meta_class::meta_cast<AP_Float>((AP_Var *)var)))
/// Block
class Block
{
@ -36,16 +34,16 @@ public:
_input(), _output()
{
}
virtual void update(const float & dt = 0) = 0;
virtual void update(const float & dt) = 0;
virtual void connect( Block * block)
{
}
const char * getName() { return _name; }
const Vector < AP_Var * > & getOutput() const { return _output; }
const Vector < AP_Float * > & getOutput() const { return _output; }
protected:
const char * _name;
Vector< const AP_Var * > _input;
Vector< AP_Var * > _output;
Vector< const AP_Float * > _input;
Vector< AP_Float * > _output;
};
/// Servo Block
@ -63,7 +61,9 @@ public:
virtual void update(const float & dt = 0)
{
if (_input.getSize() > 0)
_ch->setNormalized(APVarPtr2Float(_input[0]));
{
_ch->setNormalized(_input[0]->get());
}
}
private:
float _position;
@ -77,28 +77,28 @@ public:
/// Constructor that allows 1-8 sum gain pairs, more
/// can be added if necessary
SumGain(
AP_Var & var1 = AP_Float_zero, AP_Var & gain1 = AP_Float_zero,
AP_Var & var2 = AP_Float_zero, AP_Var & gain2 = AP_Float_zero,
AP_Var & var3 = AP_Float_zero, AP_Var & gain3 = AP_Float_zero,
AP_Var & var4 = AP_Float_zero, AP_Var & gain4 = AP_Float_zero,
AP_Var & var5 = AP_Float_zero, AP_Var & gain5 = AP_Float_zero,
AP_Var & var6 = AP_Float_zero, AP_Var & gain6 = AP_Float_zero,
AP_Var & var7 = AP_Float_zero, AP_Var & gain7 = AP_Float_zero,
AP_Var & var8 = AP_Float_zero, AP_Var & gain8 = AP_Float_zero)
AP_Float & var1 = AP_Float_zero, AP_Float & gain1 = AP_Float_zero,
AP_Float & var2 = AP_Float_zero, AP_Float & gain2 = AP_Float_zero,
AP_Float & var3 = AP_Float_zero, AP_Float & gain3 = AP_Float_zero,
AP_Float & var4 = AP_Float_zero, AP_Float & gain4 = AP_Float_zero,
AP_Float & var5 = AP_Float_zero, AP_Float & gain5 = AP_Float_zero,
AP_Float & var6 = AP_Float_zero, AP_Float & gain6 = AP_Float_zero,
AP_Float & var7 = AP_Float_zero, AP_Float & gain7 = 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 ( (&var2 == &AP_Float_zero) || (&gain2 == &AP_Float_zero) ) add(var2,gain2);
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 ( (&var5 == &AP_Float_zero) || (&gain5 == &AP_Float_zero) ) add(var5,gain5);
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 ( (&var8 == &AP_Float_zero) || (&gain8 == &AP_Float_zero) ) add(var8,gain8);
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 ( (&var3 != &AP_Float_zero) && (&gain3 != &AP_Float_zero) ) add(var3,gain3);
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 ( (&var6 != &AP_Float_zero) && (&gain6 != &AP_Float_zero) ) add(var6,gain6);
if ( (&var7 != &AP_Float_zero) && (&gain7 != &AP_Float_zero) ) add(var7,gain7);
if ( (&var8 != &AP_Float_zero) && (&gain8 != &AP_Float_zero) ) add(var8,gain8);
// create output
_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);
_gain.push_back(&gain);
@ -106,14 +106,12 @@ public:
virtual void update(const float & dt = 0)
{
if (_output.getSize() < 1) return;
_output[0]=0;
for (int i=0;i<_input.getSize();i++)
{
*_output[0] = APVarPtr2Float(_output[i]) * APVarPtr2Float(_gain[i]) ;
}
float sum =0;
for (int i=0;i<_input.getSize();i++) sum += _input[i]->get() * _gain[i]->get() ;
_output[0]->set(sum);
}
private:
Vector< AP_Var * > _gain;
Vector< AP_Float * > _gain;
};
/// PID block
@ -142,38 +140,44 @@ public:
if (block->getOutput().getSize() > 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
_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
// because derivatve uses _e as previous value
_e = APVarPtr2Float(_input[0]);
// because derivatve uses _eP as previous value
_eP = _input[0]->get();
// integral
_eI += _e*dt;
_eI += _eP*dt;
// wind up guard
if (_eI > _iMax) _eI = _iMax;
else if (_eI < -_iMax) _eI = -_iMax;
// pid sum
*_output[0] = _kP*_e + _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]));
_output[0]->set(_kP*_eP + _kI*_eI + _kD*_eD);
// 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:
float _e; /// input
float _eP; /// input
float _eI; /// integral of input
float _eD; /// derivative of input
AP_Float _kP; /// proportional gain
@ -190,10 +194,10 @@ public:
void addBlock(Block * block)
{
if (_blocks.getSize() > 0)
_blocks[_blocks.getSize()-1]->connect(block);
block->connect(_blocks[_blocks.getSize()-1]);
_blocks.push_back(block);
}
void update(const double dt=0)
void update(const double dt)
{
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:
// state
AP_Var & velocity;
AP_Var & heading;
AP_Float & velocity;
AP_Float & heading;
// control variables
AP_Var & headingCommand;
AP_Var & velocityCommand;
AP_Float & headingCommand;
AP_Float & velocityCommand;
// channels
AP_RcChannel * steeringCh;
@ -23,22 +23,22 @@ private:
public:
CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey,
AP_Var & heading, AP_Var & velocity,
AP_Var & headingCommand, AP_Var & velocityCommand,
AP_Float & heading, AP_Float & velocity,
AP_Float & headingCommand, AP_Float & velocityCommand,
AP_RcChannel * steeringCh, AP_RcChannel * throttleCh) :
heading(heading), velocity(velocity),
headingCommand(headingCommand), velocityCommand(velocityCommand),
steeringCh(steeringCh), throttleCh(throttleCh)
{
// steering control loop
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));
// throttle control loop
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));
}
};
@ -48,6 +48,8 @@ Vector<AP_RcChannel * > ch;
AP_Float heading, velocity; // measurements
AP_Float headingCommand, velocityCommand; // guidance data
int8_t sign = 1;
float value = 0;
enum keys
{
@ -65,13 +67,13 @@ void setup()
// variables
heading = 0;
velocity = 0;
headingCommand = 1;
velocityCommand = 1;
headingCommand = 0;
velocityCommand = 0;
// rc channels
APM_RC.Init();
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 = new CarController(pidStrKey,pidThrKey,heading,velocity,headingCommand,velocityCommand,ch[0],ch[1]);
@ -79,13 +81,30 @@ void setup()
void loop()
{
Serial.println("Looping");
heading += 0.1;
velocity += 0.1;
delay(1000);
controller->update(.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());
// loop rate 20 Hz
delay(1000.0/20);
// feedback signal, 1/3 Hz sawtooth
if (value > 1)
{
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

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")),
reverse(this,8,reverse,PSTR("REV")),
_pwm(0)
{ }
{
setNormalized(0.0);
}
void AP_RcChannel::readRadio() {
@ -74,9 +76,17 @@ AP_RcChannel::setPwm(uint16_t pwm)
void
AP_RcChannel::setPosition(float position)
{
if (position > scale) position = scale;
else if (position < -scale) position = -scale;
setPwm(_positionToPwm(position));
}
void
AP_RcChannel::setNormalized(float normPosition)
{
setPosition(normPosition*scale);
}
void
AP_RcChannel::mixRadio(uint16_t infStart)
{

2
libraries/AP_RcChannel/AP_RcChannel.h
View File

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