From 389b89056abdfc1e16fdec34bc89ed854301168a Mon Sep 17 00:00:00 2001 From: Simon Salykov Date: Tue, 1 Nov 2011 19:09:57 +0100 Subject: [PATCH] Obstacle avoidance moved from AP_Guide to ControllerCar (works only for cars for now), algorithm changed New feature Forward/Reverse in Controller Car Misc bug fixes in ControllerCar --- ArduRover/CarStampede.h | 2 ++ ArduRover/ControllerCar.h | 57 ++++++++++++++++++++++++-------- libraries/APO/APO_DefaultSetup.h | 2 +- libraries/APO/AP_Guide.cpp | 57 +------------------------------- libraries/APO/AP_Guide.h | 4 --- 5 files changed, 48 insertions(+), 74 deletions(-) diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h index 57de768a4b..82dc03b9cd 100644 --- a/ArduRover/CarStampede.h +++ b/ArduRover/CarStampede.h @@ -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; diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index c1951cf786..75a2b76a73 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -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 diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index 82ee0250b8..b6e842f746 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -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); diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 3437415967..49eaf263a8 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -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() { diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index f01258a42d..033e27ed45 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -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;