From 0cfa2dbea1a3e76b2a7c122f72e04f2298269f00 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Tue, 15 Feb 2011 00:57:23 +0000 Subject: [PATCH] AP_Controller working. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1659 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Controller/AP_Controller.h | 108 ++++++++++--------- libraries/AP_Controller/examples/Car/Car.pde | 57 ++++++---- libraries/AP_RcChannel/AP_RcChannel.cpp | 12 ++- libraries/AP_RcChannel/AP_RcChannel.h | 2 +- 4 files changed, 106 insertions(+), 73 deletions(-) diff --git a/libraries/AP_Controller/AP_Controller.h b/libraries/AP_Controller/AP_Controller.h index 8260286b24..87917456fa 100644 --- a/libraries/AP_Controller/AP_Controller.h +++ b/libraries/AP_Controller/AP_Controller.h @@ -26,8 +26,6 @@ #include -#define APVarPtr2Float(var) (*(AP_Meta_class::meta_cast((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++) { diff --git a/libraries/AP_Controller/examples/Car/Car.pde b/libraries/AP_Controller/examples/Car/Car.pde index a1d1841a95..f86cfaca6c 100644 --- a/libraries/AP_Controller/examples/Car/Car.pde +++ b/libraries/AP_Controller/examples/Car/Car.pde @@ -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 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 diff --git a/libraries/AP_RcChannel/AP_RcChannel.cpp b/libraries/AP_RcChannel/AP_RcChannel.cpp index 536605434f..19c84735e8 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.cpp +++ b/libraries/AP_RcChannel/AP_RcChannel.cpp @@ -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) { diff --git a/libraries/AP_RcChannel/AP_RcChannel.h b/libraries/AP_RcChannel/AP_RcChannel.h index de8dfe3cab..e9271bb212 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.h +++ b/libraries/AP_RcChannel/AP_RcChannel.h @@ -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