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

Obstacle avoidance moved from AP_Guide to ControllerCar (works only for cars for now), algorithm changed

Browse Source 
New feature Forward/Reverse in Controller Car
Misc bug fixes in ControllerCar
This commit is contained in:
Simon Salykov 2011-11-01 19:09:57 +01:00
parent 1b0328e889
commit 04d4aa4310
5 changed files with 48 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduRover/CarStampede.h
View File

@ -48,6 +48,8 @@ static const float batteryVoltageDivRatio = 6;
static const float batteryMinVolt = 10.0;
static const float batteryMaxVolt = 12.4;
static const bool useForwardReverseSwitch = false;
static const bool rangeFinderFrontEnabled = false;
static const bool rangeFinderBackEnabled = false;
static const bool rangeFinderLeftEnabled = false;

57
ArduRover/ControllerCar.h
View File

@ -21,7 +21,8 @@ public:
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _strCmd(0), _thrustCmd(0)
throttleDFCut), _strCmd(0), _thrustCmd(0),
_rangeFinderFront()
{
_hal->debug->println_P(PSTR("initializing car controller"));
@ -34,38 +35,66 @@ public:
_hal->rc.push_back(
new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), APM_RC, 4, 1100, 1500,
1900, RC_MODE_IN, false));
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
RangeFinder * rF = _hal->rangeFinders[i];
if (rF == NULL)
continue;
if (rF->orientation_x == 1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderFront = rF;
}
}
private:
// methods
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_strCmd = _hal->rc[ch_str]->getRadioPosition();
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
if (useForwardReverseSwitch && _hal->rc[ch_FwdRev]->getRadioPosition() < 0.0)
_thrustCmd = -_thrustCmd;
}
void autoLoop(const float dt) {
//_hal->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_hal->rc[ch_thrust]->getRadioPosition(), _hal->rc[ch_str]->getRadioPosition());
_strCmd = pidStr.update(_guide->getHeadingError(), _nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
float steering = pidStr.update(_guide->getHeadingError(), _nav->getYawRate(), dt);
float thrust = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
// obstacle avoidance overrides
// try to drive around the obstacle in front. if this fails, stop
if (_rangeFinderFront) {
_rangeFinderFront->read();
int distanceToObstacle = _rangeFinderFront->distance;
if (distanceToObstacle < 100) {
thrust = 0; // Avoidance didn't work out. Stopping
}
else if (distanceToObstacle < 650) {
// Deviating from the course. Deviation angle is inverse proportional
// to the distance to the obstacle, with 15 degrees min angle, 180 - max
steering += (15 + 165 *
(1 - ((float)(distanceToObstacle - 100)) / 550)) * deg2Rad;
}
}
_strCmd = steering;
_thrustCmd = thrust;
}
void setMotorsActive() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_thrust]->setPosition(_thrustCmd);
_hal->rc[ch_str]->setPosition(_strCmd);
}
_hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
}
// attributes
enum {
ch_mode = 0, ch_str, ch_thrust
ch_mode = 0, ch_str, ch_thrust, ch_FwdRev
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust, k_chFwdRev
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
@ -73,6 +102,8 @@ private:
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
RangeFinder * _rangeFinderFront;
};
} // namespace apo

2
libraries/APO/APO_DefaultSetup.h
View File

@ -86,7 +86,7 @@ void setup() {
if (rangeFinderFrontEnabled) {
hal->debug->println_P(PSTR("initializing front range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(NULL,new ModeFilter);
rangeFinder->set_analog_port(1);
rangeFinder->set_orientation(1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);

57
libraries/APO/AP_Guide.cpp
View File

@ -44,71 +44,16 @@ float AP_Guide::getHeadingError() {
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
_rangeFinderLeft(), _rangeFinderRight(),
AP_Guide(navigator, hal),
_group(k_guide, PSTR("guide_")),
_velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
_crossTrackGain(&_group, 2, xt, PSTR("xt")),
_crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
RangeFinder * rF = _hal->rangeFinders[i];
if (rF == NULL)
continue;
if (rF->orientation_x == 1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderFront = rF;
else if (rF->orientation_x == -1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderBack = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == 1
&& rF->orientation_z == 0)
_rangeFinderRight = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == -1
&& rF->orientation_z == 0)
_rangeFinderLeft = rF;
}
}
void MavlinkGuide::update() {
// process mavlink commands
handleCommand();
// obstacle avoidance overrides
// stop if your going to drive into something in front of you
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++)
_hal->rangeFinders[i]->read();
float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc
if (_rangeFinderFront && frontDistance < 2) {
_mode = MAV_NAV_VECTOR;
//airSpeedCommand = 0;
//groundSpeedCommand = 0;
// _headingCommand -= 45 * deg2Rad;
// _hal->debug->print("Obstacle Distance (m): ");
// _hal->debug->println(frontDistance);
// _hal->debug->print("Obstacle avoidance Heading Command: ");
// _hal->debug->println(headingCommand);
// _hal->debug->printf_P(
// PSTR("Front Distance, %f\n"),
// frontDistance);
}
if (_rangeFinderBack && _rangeFinderBack->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderRight && _rangeFinderRight->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
}
void MavlinkGuide::nextCommand() {

4
libraries/APO/AP_Guide.h
View File

@ -136,10 +136,6 @@ public:
void updateCommand();
private:
RangeFinder * _rangeFinderFront;
RangeFinder * _rangeFinderBack;
RangeFinder * _rangeFinderLeft;
RangeFinder * _rangeFinderRight;
AP_Var_group _group;
AP_Float _velocityCommand;
AP_Float _crossTrackGain;