Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Debugging controller. 

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

47
libraries/AP_Controller/AP_Controller.h
View File

@ -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
@ -179,25 +190,19 @@ public:
void addBlock(Block * block)
{
if (_blocks.getSize() > 0)
_blocks[_blocks.getSize()]->connect(block);
_blocks[_blocks.getSize()-1]->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()
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<Block * > _blocks;
Vector<AP_RcChannel * > _rc;
};
#endif // AP_Controller_H

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

@ -1,7 +1,6 @@
#include <FastSerial.h>
#include <AP_Controller.h>
#include <AP_Var.h>
#include <AP_EEProm.h>
#include <AP_RcChannel.h>
#include <APM_RC.h>
@ -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;
AP_RcChannel * steeringCh;
AP_RcChannel * throttleCh;
public:
CarController() :
chStr(0), chThr(1)
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)
{
// rc channels
addCh(new AP_RcChannel("STR",APM_RC,chStr,45));
addCh(new AP_RcChannel("THR",APM_RC,chThr,100));
// 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<AP_RcChannel * > 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

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

@ -109,3 +109,5 @@ void loop()
{
test->update();
}
// vim:ts=4:sw=4:expandtab