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

57
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
@ -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<Block * > _blocks;
Vector<AP_RcChannel * > _rc;
Vector<Block * > _blocks;
};
#endif // AP_Controller_H

84
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;
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<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

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

@ -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 <FastSerial.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 <AP_Vector.h>
@ -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<AP_RcChannel *> ch;
float testPosition;
int8_t testSign;
enum
{
version,
rollKey,
pitchKey,
thrKey,
yawKey,
ch5Key,
ch6Key,
ch7Key,
ch8Key
};
Vector<AP_RcChannel *> 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;i<ch.getSize();i++) ch[i]->setNormalized(testPosition);
// set channel positions
for (int i=0;i<ch.getSize();i++) ch[i]->setNormalized(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;i<ch.getSize();i++)
{
ch[i]->copy_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;i<ch.getSize();i++)
{
ch[i]->copy_name(name,7);
Serial.printf("%7s\t",name);
}
Serial.println();
// print pwm
Serial.printf("pwm :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7d\t",ch[i]->getPwm());
Serial.println();
// print pwm
Serial.printf("pwm :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7d\t",ch[i]->getPwm());
Serial.println();
// print position
Serial.printf("position :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7.2f\t",ch[i]->getPosition());
Serial.println();
// print position
Serial.printf("position :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7.2f\t",ch[i]->getPosition());
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