From 4370f1d4e483da3d14ae2779cf89eec72f64ef96 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Mon, 3 Dec 2012 13:25:21 -0800 Subject: [PATCH] remove ArduBoat sketch, deprecated since removal of APO --- ArduBoat/ArduBoat.pde | 29 --------- ArduBoat/BoatGeneric.h | 65 ------------------- ArduBoat/ControllerBoat.h | 117 ---------------------------------- ArduBoat/ControllerSailboat.h | 104 ------------------------------ ArduBoat/Makefile | 1 - ArduBoat/SailboatLaser.h | 61 ------------------ 6 files changed, 377 deletions(-) delete mode 100644 ArduBoat/ArduBoat.pde delete mode 100644 ArduBoat/BoatGeneric.h delete mode 100644 ArduBoat/ControllerBoat.h delete mode 100644 ArduBoat/ControllerSailboat.h delete mode 100644 ArduBoat/Makefile delete mode 100644 ArduBoat/SailboatLaser.h diff --git a/ArduBoat/ArduBoat.pde b/ArduBoat/ArduBoat.pde deleted file mode 100644 index 9df8a1f8b9..0000000000 --- a/ArduBoat/ArduBoat.pde +++ /dev/null @@ -1,29 +0,0 @@ -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Vehicle Configuration -//#include "BoatGeneric.h" -#include "SailboatLaser.h" - -// ArduPilotOne Default Setup -#include "APO_DefaultSetup.h" diff --git a/ArduBoat/BoatGeneric.h b/ArduBoat/BoatGeneric.h deleted file mode 100644 index 8292bae789..0000000000 --- a/ArduBoat/BoatGeneric.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - * BoatGeneric.h - * - * Created on: May 1, 2011 - * Author: jgoppert - * - */ - -#ifndef BOATGENERIC_H_ -#define BOATGENERIC_H_ - -using namespace apo; - -// vehicle options -static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; -static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; -static const apo::halMode_t halMode = apo::MODE_LIVE; -//static const apo::AP_Board::mode_e = apo::AP_Board::MODE_HIL_CNTL; -static const apo::AP_Board::mode_e = apo::AP_Board::MODE_LIVE; -static const uint8_t heartBeatTimeout = 0; - -// algorithm selection -#define CONTROLLER_CLASS ControllerBoat -#define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS Navigator_Dcm - -// hardware selection -//#define BOARD_TYPE Board_APM1 -//#define BOARD_TYPE Board_APM1_2560 -#define BOARD_TYPE Board_APM2 - -// loop rates -static const float loopRate = 150; // attitude nav -static const float loop0Rate = 50; // controller -static const float loop1Rate = 5; // pos nav/ gcs fast -static const float loop2Rate = 1; // gcs slow -static const float loop3Rate = 0.1; - -// controller -static const bool useForwardReverseSwitch = true; - -// gains -static const float steeringP = 0.1; -static const float steeringI = 0.0; -static const float steeringD = 0.1; -static const float steeringIMax = 0.0; -static const float steeringYMax = 1; -static const float steeringDFCut = 25.0; - -static const float throttleP = 0.1; -static const float throttleI = 0.0; -static const float throttleD = 0.0; -static const float throttleIMax = 0.0; -static const float throttleYMax = 1; -static const float throttleDFCut = 0.0; - -// guidance -static const float velCmd = 5; -static const float xt = 10; -static const float xtLim = 90; - -#include "ControllerBoat.h" - -#endif /* BOATGENERIC_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h deleted file mode 100644 index ee45be824c..0000000000 --- a/ArduBoat/ControllerBoat.h +++ /dev/null @@ -1,117 +0,0 @@ -/* - * ControllerBoat.h - * - * Created on: Jun 30, 2011 - * Author: jgoppert - */ - -#ifndef CONTROLLERBOAT_H_ -#define CONTROLLERBOAT_H_ - -#include "../APO/AP_Controller.h" - -namespace apo { - -class ControllerBoat: public AP_Controller { -public: - ControllerBoat(AP_Navigator * nav, AP_Guide * guide, - AP_Board * board) : - AP_Controller(nav, guide, board,new AP_ArmingMechanism(board,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), - pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, - steeringI, steeringD, steeringIMax, steeringYMax), - pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP, - throttleI, throttleD, throttleIMax, throttleYMax, - throttleDFCut), _strCmd(0), _thrustCmd(0), - _rangeFinderFront() - { - _board->debug->println_P(PSTR("initializing boat controller")); - - _board->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100, - 1500, 1900, RC_MODE_IN, false)); - _board->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chThrust, PSTR("THR_"), board->radio, 2, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chFwdRev, PSTR("FWDREV_"), board->radio, 4, 1100, 1500, - 1900, RC_MODE_IN, false)); - - for (uint8_t i = 0; i < _board->rangeFinders.getSize(); i++) { - RangeFinder * rF = _board->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) { - _strCmd = _board->rc[ch_str]->getRadioPosition(); - _thrustCmd = _board->rc[ch_thrust]->getRadioPosition(); - if (useForwardReverseSwitch && _board->rc[ch_FwdRev]->getRadioPosition() < 0.0) - _thrustCmd = -_thrustCmd; - } - void autoLoop(const float dt) { - //_board->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_board->rc[ch_thrust]->getRadioPosition(), _board->rc[ch_str]->getRadioPosition()); - // neglects heading command derivative - 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 setMotors() { - _board->rc[ch_str]->setPosition(_strCmd); - _board->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd); - } - void handleFailsafe() { - // turn off - setMode(MAV_MODE_LOCKED); - } - - // attributes - enum { - ch_mode = 0, ch_str, ch_thrust, ch_FwdRev - }; - enum { - k_chMode = k_radioChannelsStart, k_chStr, k_chThrust, k_chFwdRev - }; - enum { - k_pidStr = k_controllersStart, k_pidThrust - }; - BlockPIDDfb pidStr; - BlockPID pidThrust; - float _strCmd, _thrustCmd; - - RangeFinder * _rangeFinderFront; -}; - -} // namespace apo - -#endif /* CONTROLLERBOAT_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/ArduBoat/ControllerSailboat.h b/ArduBoat/ControllerSailboat.h deleted file mode 100644 index b0bb7cd8d5..0000000000 --- a/ArduBoat/ControllerSailboat.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - * ControllerSailboat.h - * - * Created on: Jun 30, 2011 - * Author: jgoppert - */ - -#ifndef CONTROLLERSAILBOAT_H_ -#define CONTROLLERSAILBOAT_H_ - -#include "../APO/AP_Controller.h" - -namespace apo { - -class ControllerSailboat: public AP_Controller { -public: - ControllerSailboat(AP_Navigator * nav, AP_Guide * guide, - AP_Board * board) : - AP_Controller(nav, guide, board,new AP_ArmingMechanism(board,this,ch_sail,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), - pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, - steeringI, steeringD, steeringIMax, steeringYMax), - pidSail(new AP_Var_group(k_pidSail, PSTR("SAIL_")), 1, throttleP, - throttleI, throttleD, throttleIMax, throttleYMax, - throttleDFCut), _strCmd(0), _sailCmd(0) - { - _board->debug->println_P(PSTR("initializing sailboat controller")); - - _board->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100, - 1500, 1900, RC_MODE_IN, false)); - _board->rc.push_back( - new AP_RcChannel(k_chStr, PSTR("STR_"), board->radio, 3, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chSail, PSTR("SAIL_"), board->radio, 2, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - } - -private: - // methods - void manualLoop(const float dt) { - _strCmd = -_board->rc[ch_str]->getRadioPosition(); - _sailCmd = _board->rc[ch_sail]->getRadioPosition(); - _board->debug->printf_P(PSTR("sail: %f, steering: %f\n"),_sailCmd,_strCmd); - } - void autoLoop(const float dt) { - //_board->debug->printf_P(PSTR("cont: ch1: %f\tch2: %f\n"),_board->rc[ch_sail]->getRadioPosition(), _board->rc[ch_str]->getRadioPosition()); - static AP_AnalogSource_Arduino wind_pin(1); - float windDir = -.339373*wind_pin.read_average()+175.999; - - // neglects heading command derivative - float steering = -pidStr.update(_guide->getHeadingError(), -_nav->getYawRate(), dt); - float sail = 0.00587302*fabs(windDir) - 0.05; - if (sail < 0.0) sail = 0.0; - - //_board->debug->printf_P(PSTR("heading: %f\n"),heading); //Print Heading - - //if(fabs(psi)<45) //Tacking Logic - //{ - //if(psi<-10) - //alpha = -45; - //else if(psi>10) - //alpha = 45; - //else - //{ - //if(psi==10) - //alpha = 45; - //else if(psi==-10) - //alpha = -45; - //else - //alpha = alpha; - //} - //}*/ - _strCmd = steering; - _sailCmd = sail; - } - void setMotors() { - _board->rc[ch_str]->setPosition(_strCmd); - _board->rc[ch_sail]->setPosition(_sailCmd); - } - void handleFailsafe() { - // turn off - setMode(MAV_MODE_LOCKED); - } - - // attributes - enum { - ch_mode = 0, ch_str, ch_sail - }; - enum { - k_chMode = k_radioChannelsStart, k_chStr, k_chSail - }; - enum { - k_pidStr = k_controllersStart, k_pidSail - }; - BlockPIDDfb pidStr; - BlockPID pidSail; - float _strCmd, _sailCmd; -}; - -} // namespace apo - -#endif /* CONTROLLERSAILBOAT_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/ArduBoat/Makefile b/ArduBoat/Makefile deleted file mode 100644 index 1d9b6a022d..0000000000 --- a/ArduBoat/Makefile +++ /dev/null @@ -1 +0,0 @@ -include ../libraries/AP_Common/Arduino.mk diff --git a/ArduBoat/SailboatLaser.h b/ArduBoat/SailboatLaser.h deleted file mode 100644 index 74b4194438..0000000000 --- a/ArduBoat/SailboatLaser.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * SailboatLaser.h - * - * Created on: May 1, 2011 - * Author: jgoppert - * - */ - -#ifndef SAILBOATLASER_H_ -#define SAILBOATLASER_H_ - -using namespace apo; - -// vehicle options -static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; -static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; -static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_HIL_CNTL; -//static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_LIVE; -static const uint8_t heartBeatTimeout = 0; - -// algorithm selection -#define CONTROLLER_CLASS ControllerSailboat -#define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS Navigator_Dcm - -// hardware selection -#define BOARD_TYPE Board_APM1 -//#define BOARD_TYPE Board_APM1_2560 -//#define BOARD_TYPE Board_APM2 - -// loop rates -static const float loopRate = 150; // attitude nav -static const float loop0Rate = 50; // controller -static const float loop1Rate = 5; // pos nav/ gcs fast -static const float loop2Rate = 1; // gcs slow -static const float loop3Rate = 0.1; - -// gains -static const float steeringP = 0.1; -static const float steeringI = 0.0; -static const float steeringD = 0.1; -static const float steeringIMax = 0.0; -static const float steeringYMax = 1; -static const float steeringDFCut = 25.0; - -static const float throttleP = 0.1; -static const float throttleI = 0.0; -static const float throttleD = 0.2; -static const float throttleIMax = 0.0; -static const float throttleYMax = 1; -static const float throttleDFCut = 0.0; - -// guidance -static const float velCmd = 2; -static const float xt = 10; -static const float xtLim = 90; - -#include "ControllerSailboat.h" - -#endif /* SAILBOATLASER_H_ */ -// vim:ts=4:sw=4:expandtab