From 6788def6798e224bd3887b2868a78fe7b8f3ad8d Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Mon, 14 Feb 2011 21:02:35 +0000 Subject: [PATCH] Debugging controller. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1658 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Controller/AP_Controller.h | 57 ++++--- libraries/AP_Controller/examples/Car/Car.pde | 84 ++++++---- .../examples/ServoSweep/ServoSweep.pde | 154 +++++++++--------- 3 files changed, 165 insertions(+), 130 deletions(-) diff --git a/libraries/AP_Controller/AP_Controller.h b/libraries/AP_Controller/AP_Controller.h index 23dba88fc3..8260286b24 100644 --- a/libraries/AP_Controller/AP_Controller.h +++ b/libraries/AP_Controller/AP_Controller.h @@ -52,7 +52,7 @@ protected: class ToServo: public Block { public: - ToServo(AP_RcChannel & ch) : _ch(ch) + ToServo(AP_RcChannel * ch) : _ch(ch) { } virtual void connect(Block * block) @@ -63,11 +63,11 @@ public: virtual void update(const float & dt = 0) { if (_input.getSize() > 0) - _ch.setPosition(APVarPtr2Float(_output[0])); + _ch->setNormalized(APVarPtr2Float(_input[0])); } private: float _position; - AP_RcChannel & _ch; + AP_RcChannel * _ch; }; /// SumGain @@ -94,17 +94,15 @@ public: 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) { _input.push_back(&var); _gain.push_back(&gain); } - virtual void connect(Block * block) - { - if (block->getOutput().getSize() > 0) - _input.push_back(block->getOutput()[0]); - } virtual void update(const float & dt = 0) { if (_output.getSize() < 1) return; @@ -140,12 +138,13 @@ public: } virtual void connect(Block * block) { + if (!block) return; if (block->getOutput().getSize() > 0) _input.push_back(block->getOutput()[0]); } virtual void update(const float & dt = 0) { - if (_output.getSize() < 1) return; + if (_output.getSize() < 1 || !_input[0] || !_output[0]) return; // derivative float RC = 1/(2*M_PI*_fCut); // low pass filter @@ -160,6 +159,18 @@ public: // 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])); + + } private: float _e; /// input @@ -176,28 +187,22 @@ private: class AP_Controller { public: - void addBlock(Block * block) - { + void addBlock(Block * block) + { if (_blocks.getSize() > 0) - _blocks[_blocks.getSize()]->connect(block); - _blocks.push_back(block); - } - void addCh(AP_RcChannel * ch) - { - _rc.push_back(ch); - } - AP_RcChannel & getRc(int i) - { - return *(_rc[i]); - } - void update() + _blocks[_blocks.getSize()-1]->connect(block); + _blocks.push_back(block); + } + void update(const double dt=0) { for (int i=0;i<_blocks.getSize();i++) - _blocks[i]->update(); + { + if (!_blocks[i]) continue; + _blocks[i]->update(dt); + } } private: - Vector _blocks; - Vector _rc; + Vector _blocks; }; #endif // AP_Controller_H diff --git a/libraries/AP_Controller/examples/Car/Car.pde b/libraries/AP_Controller/examples/Car/Car.pde index dff4bb2749..a1d1841a95 100644 --- a/libraries/AP_Controller/examples/Car/Car.pde +++ b/libraries/AP_Controller/examples/Car/Car.pde @@ -1,7 +1,6 @@ #include #include #include -#include #include #include @@ -11,53 +10,82 @@ class CarController : public AP_Controller { private: // state - AP_Float * roll; - AP_Float * airspeed; - AP_Float * velocity; - AP_Float * heading; - - // servo positions - AP_Float * steering; - AP_Float * throttle; + AP_Var & velocity; + AP_Var & heading; // control variables - AP_Float * headingCommand; - AP_Float * velocityCommand; + AP_Var & headingCommand; + AP_Var & velocityCommand; // channels - uint8_t chStr; - uint8_t chThr; -public: - CarController() : - chStr(0), chThr(1) - { - // rc channels - addCh(new AP_RcChannel("STR",APM_RC,chStr,45)); - addCh(new AP_RcChannel("THR",APM_RC,chThr,100)); + AP_RcChannel * steeringCh; + AP_RcChannel * throttleCh; +public: + CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey, + AP_Var & heading, AP_Var & velocity, + AP_Var & headingCommand, AP_Var & 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_unity,heading,&AP_negativeUnity)); - addBlock(new Pid("HDNG",0.1,0,0,1,20)); - addBlock(new ToServo(getRc(chStr))); + 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 ToServo(steeringCh)); // throttle control loop - addBlock(new SumGain(velocityCommand,&AP_unity,velocity,&AP_negativeUnity)); - addBlock(new Pid("THR",0.1,0,0,1,20)); - addBlock(new ToServo(getRc(chThr))); + 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 ToServo(throttleCh)); } }; AP_Controller * controller; +Vector ch; + +AP_Float heading, velocity; // measurements +AP_Float headingCommand, velocityCommand; // guidance data + +enum keys +{ + config, + chStrKey, + chThrKey, + pidStrKey, + pidThrKey +}; void setup() { Serial.begin(115200); - //controller = new CarController; + + // variables + heading = 0; + velocity = 0; + headingCommand = 1; + velocityCommand = 1; + + // 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)); + + // controller + controller = new CarController(pidStrKey,pidThrKey,heading,velocity,headingCommand,velocityCommand,ch[0],ch[1]); } void loop() { Serial.println("Looping"); + heading += 0.1; + velocity += 0.1; delay(1000); - //controller->update(); + 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()); } + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde index 2d5adef894..2d5de8215d 100644 --- a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde +++ b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde @@ -1,13 +1,13 @@ /* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com + Example of RC_Channel library. + Code by James Goppert/ Jason Short. 2010 + DIYDrones.com */ #include #include -#include // ArduPilot Mega RC Library +#include // ArduPilot Mega RC Library #include #include @@ -18,94 +18,96 @@ FastSerialPort0(Serial); // make sure this proceeds variable declarations class RadioTest { private: - float testPosition; - int8_t testSign; - enum - { - version, - rollKey, - pitchKey, - thrKey, - yawKey, - ch5Key, - ch6Key, - ch7Key, - ch8Key - }; - Vector ch; + float testPosition; + int8_t testSign; + enum + { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; public: - RadioTest() : testPosition(2), testSign(1) - { - ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45)); - ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45)); - ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100)); - ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45)); - ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1)); - ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1)); - ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1)); - ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1)); + RadioTest() : testPosition(2), testSign(1) + { + ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45)); + ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45)); + ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100)); + ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45)); + ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1)); + ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1)); + ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1)); + ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1)); - Serial.begin(115200); - delay(2000); - Serial.println("ArduPilot RC Channel test"); - APM_RC.Init(); // APM Radio initialization - delay(2000); - } + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } - void update() - { - // update test value - testPosition += testSign*.1; - if (testPosition > 1) - { - //eepromRegistry.print(Serial); // show eeprom map - testPosition = 1; - testSign = -1; - } - else if (testPosition < -1) - { - testPosition = -1; - testSign = 1; - } + void update() + { + // update test value + testPosition += testSign*.1; + if (testPosition > 1) + { + //eepromRegistry.print(Serial); // show eeprom map + testPosition = 1; + testSign = -1; + } + else if (testPosition < -1) + { + testPosition = -1; + testSign = 1; + } - // set channel positions - for (int i=0;isetNormalized(testPosition); + // set channel positions + for (int i=0;isetNormalized(testPosition); - // print test position - Serial.printf("\nnormalized position (%f)\n",testPosition); + // print test position + Serial.printf("\nnormalized position (%f)\n",testPosition); - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (int i=0;icopy_name(name,7); - Serial.printf("%7s\t",name); - } - Serial.println(); + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (int i=0;icopy_name(name,7); + Serial.printf("%7s\t",name); + } + Serial.println(); - // print pwm - Serial.printf("pwm :\t"); - for (int i=0;igetPwm()); - Serial.println(); + // print pwm + Serial.printf("pwm :\t"); + for (int i=0;igetPwm()); + Serial.println(); - // print position - Serial.printf("position :\t"); - for (int i=0;igetPosition()); - Serial.println(); + // print position + Serial.printf("position :\t"); + for (int i=0;igetPosition()); + Serial.println(); - delay(500); - } + delay(500); + } }; RadioTest * test; void setup() { - test = new RadioTest; + test = new RadioTest; } -void loop() +void loop() { - test->update(); + test->update(); } + +// vim:ts=4:sw=4:expandtab