Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 02:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Debugging controller.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1658 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-02-14 21:02:35 +00:00
parent e2d4c9e901
commit 6788def679
3 changed files with 165 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_Controller/AP_Controller.h
View File

@ -52,7 +52,7 @@ protected:
class ToServo: public Block class ToServo: public Block
{ {
public: public:
ToServo(AP_RcChannel & ch) : _ch(ch) ToServo(AP_RcChannel * ch) : _ch(ch)
{ {
} }
virtual void connect(Block * block) virtual void connect(Block * block)
@ -63,11 +63,11 @@ public:
virtual void update(const float & dt = 0) virtual void update(const float & dt = 0)
{ {
if (_input.getSize() > 0) if (_input.getSize() > 0)
_ch.setPosition(APVarPtr2Float(_output[0])); _ch->setNormalized(APVarPtr2Float(_input[0]));
} }
private: private:
float _position; float _position;
AP_RcChannel & _ch; AP_RcChannel * _ch;
}; };
/// SumGain /// SumGain
@ -94,17 +94,15 @@ public:
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
_output.push_back(new AP_Float(0));
} }
void add(AP_Var & var, AP_Var & gain) void add(AP_Var & var, AP_Var & gain)
{ {
_input.push_back(&var); _input.push_back(&var);
_gain.push_back(&gain); _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) virtual void update(const float & dt = 0)
{ {
if (_output.getSize() < 1) return; if (_output.getSize() < 1) return;
@ -140,12 +138,13 @@ public:
} }
virtual void connect(Block * block) virtual void connect(Block * block)
{ {
if (!block) return;
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 = 0)
{ {
if (_output.getSize() < 1) return; if (_output.getSize() < 1 || !_input[0] || !_output[0]) return;
// derivative // derivative
float RC = 1/(2*M_PI*_fCut); // low pass filter float RC = 1/(2*M_PI*_fCut); // low pass filter
@ -160,6 +159,18 @@ public:
// pid sum // pid sum
*_output[0] = _kP*_e + _kI*_eI + _kD*_eD; *_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: private:
float _e; /// input float _e; /// input
@ -176,28 +187,22 @@ private:
class AP_Controller class AP_Controller
{ {
public: public:
void addBlock(Block * block) void addBlock(Block * block)
{ {
if (_blocks.getSize() > 0) if (_blocks.getSize() > 0)
_blocks[_blocks.getSize()]->connect(block); _blocks[_blocks.getSize()-1]->connect(block);
_blocks.push_back(block); _blocks.push_back(block);
} }
void addCh(AP_RcChannel * ch) void update(const double dt=0)
{
_rc.push_back(ch);
}
AP_RcChannel & getRc(int i)
{
return *(_rc[i]);
}
void update()
{ {
for (int i=0;i<_blocks.getSize();i++) for (int i=0;i<_blocks.getSize();i++)
_blocks[i]->update(); {
if (!_blocks[i]) continue;
_blocks[i]->update(dt);
}
} }
private: private:
Vector<Block * > _blocks; Vector<Block * > _blocks;
Vector<AP_RcChannel * > _rc;
}; };
#endif // AP_Controller_H #endif // AP_Controller_H

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

@ -1,7 +1,6 @@
#include <FastSerial.h> #include <FastSerial.h>
#include <AP_Controller.h> #include <AP_Controller.h>
#include <AP_Var.h> #include <AP_Var.h>
#include <AP_EEProm.h>
#include <AP_RcChannel.h> #include <AP_RcChannel.h>
#include <APM_RC.h> #include <APM_RC.h>
@ -11,53 +10,82 @@ class CarController : public AP_Controller
{ {
private: private:
// state // state
AP_Float * roll; AP_Var & velocity;
AP_Float * airspeed; AP_Var & heading;
AP_Float * velocity;
AP_Float * heading;
// servo positions
AP_Float * steering;
AP_Float * throttle;
// control variables // control variables
AP_Float * headingCommand; AP_Var & headingCommand;
AP_Float * velocityCommand; AP_Var & velocityCommand;
// channels // channels
uint8_t chStr; AP_RcChannel * steeringCh;
uint8_t chThr; AP_RcChannel * throttleCh;
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));
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 // steering control loop
addBlock(new SumGain(headingCommand,&AP_unity,heading,&AP_negativeUnity)); addBlock(new SumGain(headingCommand,AP_Float_unity,heading,AP_Float_negative_unity));
addBlock(new Pid("HDNG",0.1,0,0,1,20)); addBlock(new Pid(pidStrKey,PSTR("STR"),0.1,0,0,1,20));
addBlock(new ToServo(getRc(chStr))); addBlock(new ToServo(steeringCh));
// throttle control loop // throttle control loop
addBlock(new SumGain(velocityCommand,&AP_unity,velocity,&AP_negativeUnity)); addBlock(new SumGain(velocityCommand,AP_Float_unity,velocity,AP_Float_negative_unity));
addBlock(new Pid("THR",0.1,0,0,1,20)); addBlock(new Pid(pidThrKey,PSTR("THR"),0.1,0,0,1,20));
addBlock(new ToServo(getRc(chThr))); addBlock(new ToServo(throttleCh));
} }
}; };
AP_Controller * controller; AP_Controller * controller;
Vector<AP_RcChannel * > ch;
AP_Float heading, velocity; // measurements
AP_Float headingCommand, velocityCommand; // guidance data
enum keys
{
config,
chStrKey,
chThrKey,
pidStrKey,
pidThrKey
};
void setup() void setup()
{ {
Serial.begin(115200); 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() void loop()
{ {
Serial.println("Looping"); Serial.println("Looping");
heading += 0.1;
velocity += 0.1;
delay(1000); 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

154
libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde
View File

@ -1,13 +1,13 @@
/* /*
Example of RC_Channel library. Example of RC_Channel library.
Code by James Goppert/ Jason Short. 2010 Code by James Goppert/ Jason Short. 2010
DIYDrones.com DIYDrones.com
*/ */
#include <FastSerial.h> #include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_RcChannel.h> // ArduPilot Mega RC Library #include <AP_RcChannel.h> // ArduPilot Mega RC Library
#include <APM_RC.h> #include <APM_RC.h>
#include <AP_Vector.h> #include <AP_Vector.h>
@ -18,94 +18,96 @@ FastSerialPort0(Serial); // make sure this proceeds variable declarations
class RadioTest class RadioTest
{ {
private: private:
float testPosition; float testPosition;
int8_t testSign; int8_t testSign;
enum enum
{ {
version, version,
rollKey, rollKey,
pitchKey, pitchKey,
thrKey, thrKey,
yawKey, yawKey,
ch5Key, ch5Key,
ch6Key, ch6Key,
ch7Key, ch7Key,
ch8Key ch8Key
}; };
Vector<AP_RcChannel *> ch; Vector<AP_RcChannel *> ch;
public: public:
RadioTest() : testPosition(2), testSign(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(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(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(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(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(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(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(ch7Key,PSTR("CH7"),APM_RC,7,1));
ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1)); ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1));
Serial.begin(115200); Serial.begin(115200);
delay(2000); delay(2000);
Serial.println("ArduPilot RC Channel test"); Serial.println("ArduPilot RC Channel test");
APM_RC.Init(); // APM Radio initialization APM_RC.Init(); // APM Radio initialization
delay(2000); delay(2000);
} }
void update() void update()
{ {
// update test value // update test value
testPosition += testSign*.1; testPosition += testSign*.1;
if (testPosition > 1) if (testPosition > 1)
{ {
//eepromRegistry.print(Serial); // show eeprom map //eepromRegistry.print(Serial); // show eeprom map
testPosition = 1; testPosition = 1;
testSign = -1; testSign = -1;
} }
else if (testPosition < -1) else if (testPosition < -1)
{ {
testPosition = -1; testPosition = -1;
testSign = 1; testSign = 1;
} }
// set channel positions // set channel positions
for (int i=0;i<ch.getSize();i++) ch[i]->setNormalized(testPosition); for (int i=0;i<ch.getSize();i++) ch[i]->setNormalized(testPosition);
// print test position // print test position
Serial.printf("\nnormalized position (%f)\n",testPosition); Serial.printf("\nnormalized position (%f)\n",testPosition);
// print channel names // print channel names
Serial.print("\t\t"); Serial.print("\t\t");
static char name[7]; static char name[7];
for (int i=0;i<ch.getSize();i++) for (int i=0;i<ch.getSize();i++)
{ {
ch[i]->copy_name(name,7); ch[i]->copy_name(name,7);
Serial.printf("%7s\t",name); Serial.printf("%7s\t",name);
} }
Serial.println(); Serial.println();
// print pwm // print pwm
Serial.printf("pwm :\t"); Serial.printf("pwm :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7d\t",ch[i]->getPwm()); for (int i=0;i<ch.getSize();i++) Serial.printf("%7d\t",ch[i]->getPwm());
Serial.println(); Serial.println();
// print position // print position
Serial.printf("position :\t"); Serial.printf("position :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7.2f\t",ch[i]->getPosition()); for (int i=0;i<ch.getSize();i++) Serial.printf("%7.2f\t",ch[i]->getPosition());
Serial.println(); Serial.println();
delay(500); delay(500);
} }
}; };
RadioTest * test; RadioTest * test;
void setup() void setup()
{ {
test = new RadioTest; test = new RadioTest;
} }
void loop() void loop()
{ {
test->update(); test->update();
} }
// vim:ts=4:sw=4:expandtab