diff --git a/.gitignore b/.gitignore index 00ab58d..1d0d7fe 100644 --- a/.gitignore +++ b/.gitignore @@ -112,3 +112,4 @@ Temporary Items .apdisk sysProps.xml *.rej +docs/ \ No newline at end of file diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index e69de29..0000000 diff --git a/README.md b/README.md deleted file mode 100644 index 999cddb..0000000 --- a/README.md +++ /dev/null @@ -1,18 +0,0 @@ -Dreadbot V -============ - -Code for the 2015 robot. - -Project Files -============= - -We are *not* putting the following Eclipse files into version control -because they're specific to individuals or operating systems: - - .project - .cproject - build.xml - build.properties - -Please do not commit them to github! It screws up other people when they -pull updates. diff --git a/iomap.txt b/iomap.txt deleted file mode 100644 index ccc33c1..0000000 --- a/iomap.txt +++ /dev/null @@ -1,103 +0,0 @@ -################################################################################ -## INPUT -################################################################################ - -Control Joystick -------- ------------------- -LX Axis X Translation -LY Axis -RX Axis Rotation -RY Axis Y Translation -A Lift down -B Arms in? -X Arms out? -Y Lift up -LB Transit outtake -RB Transit intake -LT Intake Outtake -RT Intake Intake -Back -Start Toggle camera -Up -Down -Left -Right - - Analog In ---- ------------------- -0 Gyro Rate -1 Gyro Temp -2 -3 - - Digital In ---- ------------------- -0 Lift Down switch -1 Left transit switch -2 Right transit switch -3 -4 -5 Practice Bot -6 -7 Auton Mode LSB -8 Auton Mode -9 Auton Mode MSB - -################################################################################ -## OUTPUT -################################################################################ - -CAN Motor ---- ------------------- -1 LF Drive -2 RF Drive -3 LB Drive -4 RB Drive - -PWM Motor ---- ------------------- -0 L Intake (inverted) -1 R Intake -2 L Transit (inverted) -3 R Transit -4 Compressor Relay -5 -6 -7 -8 -9 - - Pneumatic ---- ------------------- -0 Lift Forward -1 Lift Reverse -2 Intake Forward -3 Intake Reverse -4 -5 -6 -7 - - Relay ---- ------------------- -0 -1 -2 -3 - - Digital Out ---- ------------------- -0 -1 -2 -3 -4 -5 -6 -7 -8 -9 - - I2C ---------- ------------- -SDA & SCL MPU-9150 diff --git a/lib/Logger.cpp b/lib/Logger.cpp index bb809b9..64606d9 100644 --- a/lib/Logger.cpp +++ b/lib/Logger.cpp @@ -37,7 +37,7 @@ namespace Hydra } stringstream logEntry; - logEntry << "[" << timeInfo->tm_hour << ":" << timeInfo->tm_min << ":" << timeInfo->tm_sec << "] "; + logEntry << "[" << timeInfo->tm_hour << ":" << timeInfo->tm_min << ":" << timeInfo->tm_sec << "]\t"; logEntry << flagText; logEntry << message; logBuffer.push_back(logEntry.str()); diff --git a/lib/Logger.h b/lib/Logger.h index 6899cf1..d558545 100644 --- a/lib/Logger.h +++ b/lib/Logger.h @@ -13,31 +13,35 @@ using std::vector; using std::ofstream; using std::stringstream; +//Logger files were ripped out of a game dev library. Some things here (such as flags) are customized more for game dev than for robotics. +//Note that as the roboRIO is Linux-based (or something really close) a "/" needs to be at the beginning of the filename in order to log anything. namespace Hydra { - #define MAX_LOGBUFFER_ENTRIES 10 + #define MAX_LOGBUFFER_ENTRIES 10 //How many entries the log buffer stores before outputting all entries to a file. Change this to 0 for unstable (i.e. can crash) environments. - enum logFlag {error, hydsys, info, resource}; - class Log + enum logFlag {error, hydsys, info, resource}; //All possible flags that could be used. Default is hydsys. + class Log { - public: - Log(string newName, string newFilename); - void log(string message, logFlag flag = hydsys); //!< Writes a message (with timestamp) to the log with the specified flag - void flushBuffer(); + public: + Log(string newName, string newFilename); //!< Creates a new log with this name at this filename. Do NOT append .txt to the filename, it does it automatically + void log(string message, logFlag flag = hydsys); //!< Writes a message (with timestamp) to the log with the specified flag + void flushBuffer(); //!< Forces the contents of the buffer to output to file. private: - vector logBuffer; - string filename; + vector logBuffer; + string filename; string name; friend class Logger; }; - class Logger + + //Logger class automatically creates its own sysLog log upon creation. + class Logger { public: friend class Log; - void log(string message = "Default log output", logFlag flag = hydsys, string name = "sysLog"); //!< Logs something in the logger of the given name - void newLog(string name = "sysLog", string filename = "/sysLog"); //!< Creates a new log with the specified name and filename - Log* getLog(string name); //!< Returns a pointer to the log with the specified name. - void flushLogBuffers(); //!< Manually flushes all log buffers. + void log(string message = "Default log output", logFlag flag = hydsys, string name = "sysLog"); //!< Logs something in the logger of the given name + void newLog(string name = "sysLog", string filename = "/sysLog"); //!< Creates a new log with the specified name and filename + Log* getLog(string name); //!< Returns a pointer to the log with the specified name. If no such log exists, returns nullptr. + void flushLogBuffers(); //!< Manually flushes all log buffers. static Logger* getInstance(); private: static Logger* instance; diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp deleted file mode 100644 index 411a320..0000000 --- a/src/Autonomous.cpp +++ /dev/null @@ -1,475 +0,0 @@ -#include "Autonomous.h" - -Log* sysLog; - -namespace dreadbot -{ - //States - GettingTote::GettingTote() - { - drivebase = nullptr; - intake = nullptr; - timerActive = false; - } - void GettingTote::setHardware(MecanumDrive* newDrivebase, MotorGrouping* newIntake) - { - drivebase = newDrivebase; - intake = newIntake; - } - void GettingTote::enter() - { - XMLInput::getInstance()->getPGroup("lift")->Set(1); //Raise the lift - eStopTimer.Start(); //estop timer - if limit is passed, automatically estops the robot. - sysLog->log("State: GettingTote"); - } - int GettingTote::update() - { - if (isToteInTransit() && !timerActive) //Run once, upon tote contact with transit wheels. - { - //Stay still while a tote is loaded - timerActive = true; //This doesn't really control a timer anymore... - drivebase->Drive_v(0, 0, 0); - } - if (!isToteInTransit() && timerActive) - { - //Tote successfully collected - it should have just left contact with the transit wheels. - intake->Set(0); - timerActive = false; - eStopTimer.Stop(); - eStopTimer.Reset(); - return HALBot::timerExpired; - } - if (timerActive) - { - //Keep sucking a tote in - intake->Set(-0.5); - return HALBot::no_update; - } - if (HALBot::getToteCount() != 0) //Open the intake arms for grabbing totes after the first tote is collected - XMLInput::getInstance()->getPGroup("intakeArms")->Set(-1); - drivebase->Drive_v(0, -0.75, 0); - intake->Set(-0.6); - - //E-stop in case the tote is missed. Zeros the velocity and intake/transit motors, and puts the robot into Stopped - if (eStopTimer.Get() >= ESTOP_TIME) - { - eStopTimer.Stop(); - eStopTimer.Reset(); - if (drivebase != nullptr) drivebase->Drive_v(0, 0, 0); - if (intake != nullptr) intake->Set(0); - sysLog->log("E-stopped in GettingTote", Hydra::error); - return HALBot::eStop; - } - - return HALBot::no_update; - } - - DriveToZone::DriveToZone() - { - drivebase = nullptr; - timerActive = false; - strafe = false; - dir = 1; //Multiplier that changes the direction the robot moves. - } - void DriveToZone::setHardware(MecanumDrive* newDrivebase) - { - drivebase = newDrivebase; - } - void DriveToZone::enter() - { - driveTimer.Reset(); - driveTimer.Start(); - timerActive = true; - if (HALBot::getToteCount() < 3) - XMLInput::getInstance()->getPGroup("lift")->Set(1); //Raise the lift for tote transit - it's more stable that way. - sysLog->log("State: DriveToZone"); - } - int DriveToZone::update() - { - float drvZoneTime = DRIVE_TO_ZONE_TIME; - if (HALBot::getToteCount() == 0) - drvZoneTime += 0.4f; //The robot starts farther back when no tote is collected. This adds on additional time to make sure the robot still drives far enough. - - //Break once the robot has moved far enough - timing based. - if ((driveTimer.Get() >= drvZoneTime && !strafe) || (driveTimer.Get() >= STRAFE_TO_ZONE_TIME && strafe)) - { - driveTimer.Stop(); - driveTimer.Reset(); - timerActive = false; - drivebase->Drive_v(0, 0, 0); - return HALBot::timerExpired; - } - - //Apply actual velocity changes - if (drivebase != nullptr) - { - if (strafe) - drivebase->Drive_v(1, 0, 0); //Right - else - drivebase->Drive_v(0, 0.8 * dir, 0); // Do a short dance - } - - return HALBot::no_update; - } - - ForkGrab::ForkGrab() - { - timerActive = false; - drivebase = nullptr; - lift = nullptr; - } - void ForkGrab::enter() - { - sysLog->log("State: ForkGrab"); - } - int ForkGrab::update() - { - if (isLiftDown()) - { - HALBot::incrTote(); //Once the lift is down, it is assumed that the tote is actually collected, i.e. in the fork. - - //special 3-tote auton condition. Really kludgy. Causes the robot to NOT lift before rotating/driving - if (HALBot::getToteCount() >= 3 && HALBot::enoughTotes()) - { - lift->Set(1); //Raise lift - Wait(0.25); //Totes must engage first - lift->Set(0); - return HALBot::finish; - } - - //Raise the lift and cheat to alight the tote - drivebase->GoFast(); - drivebase->Drive_v(0, 1, 0); // @todo Calibrate - Wait(STACK_CORRECTION_TIME); - drivebase->Drive_v(0, 0, 0); - drivebase->GoSlow(); - lift->Set(1); - Wait(0.3); - if (HALBot::enoughTotes()) - return HALBot::finish; - else - return HALBot::nextTote; - } - drivebase->Drive_v(0, 0, 0); - if (lift != nullptr) - lift->Set(-1); //Lower the lift for grabbing - return HALBot::no_update; - } - - void Stopped::enter() - { - sysLog->log("State: Stopped"); - if (HALBot::getToteCount() == 3) - XMLInput::getInstance()->getPGroup("liftArms")->Set(0); //Only lower the forks when the robot is in 3TA - it's not needed elsewhere - } - int Stopped::update() - { - return HALBot::no_update; //Does nothing of significance. - } - - Rotate::Rotate() - { - timerActive = false; - rotateConstant = 1; //Changes the direction that the robot turns. - } - void Rotate::enter() - { - driveTimer.Reset(); - driveTimer.Start(); - timerActive = true; - sysLog->log("State: Rotate"); - } - int Rotate::update() - { - if (driveTimer.Get() >= ROTATE_TIME) - { //Rotated far enough; break - timerActive = false; - drivebase->Drive_v(0, 0, 0); - if (HALBot::getToteCount() == 3) - XMLInput::getInstance()->getPGroup("lift")->Set(-1); //Lower lift - return HALBot::timerExpired; - } - if (drivebase != nullptr) - drivebase->Drive_v(0, 0, 0.5 * rotateConstant); - return HALBot::no_update; - } - - void BackAway::enter() - { - sysLog->log("State: BackAway"); - drivebase->Drive_v(0, 0, 0); - timerActive = false; //Cheat way of figuring out if the lift is down. Used elsewhere - } - int BackAway::update() - { - //While the lift isn't down - if (!timerActive) - { - lift->Set(-1); - - //On first lift down - if (isLiftDown()) - { - timerActive = true; - XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); - Wait(0.1); //Uber cheap way of getting the totes to disengage - grabTimer.Reset(); - grabTimer.Start(); - } - return HALBot::no_update; - } - - if (grabTimer.Get() >= BACK_AWAY_TIME) - { - drivebase->Drive_v(0, 0, 0); - return HALBot::timerExpired; - } - - if (drivebase != nullptr) - drivebase->Drive_v(0, -1, 0); - XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); - return HALBot::no_update; - } - - PushContainer::PushContainer() - { - enableScaling = false; - } - void PushContainer::enter() - { - pushConstant *= -1; //Not used, but it can change the direction the robot pushes containers - sysLog->log("State: PushContainer"); - } - int PushContainer::update() - { - float pushTime = PUSH_TIME; - if (enableScaling) //I refuse comment on this bit. Let's just say that it makes the robot push less. - pushTime += ((float)HALBot::getToteCount() - 1) / 3; //Scaling for three-tote autonomous, since the second container is farther away than the first - XMLInput::getInstance()->getPGroup("intakeArms")->Set(1); //Intake arms in - if (!timerActive) - { - driveTimer.Reset(); - driveTimer.Start(); - timerActive = true; - } - - if (driveTimer.Get() >= pushTime) - { - timerActive = false; - drivebase->Drive_v(0, 0, 0); - return HALBot::timerExpired; - } - - if (drivebase != nullptr) - drivebase->Drive_v(0, -PUSH_SPEED, 0); //Straight forward - if (pusher1 != nullptr) - pusher1->Set(1); //Push the container? - if (pusher2 != nullptr) - pusher2->Set(1); - return HALBot::no_update; - } - - RotateDrive::RotateDrive() - { - timerActive = false; - rotateConstant = 1; - } - void RotateDrive::enter() - { - sysLog->log("State: RotateDrive"); - drivebase->GoFast(); //Gotta go faaaaaaaasssst. - driveTimer.Start(); - } - int RotateDrive::update() - { - if (driveTimer.Get() >= (ROTATE_TIME - 1.2f)) - { //Rotated far enough; break - timerActive = false; - drivebase->GoSlow(); - drivebase->Drive_v(0, 1, 0); - Wait(ROTATE_DRIVE_STRAIGHT); - if (HALBot::getToteCount() == 3) - XMLInput::getInstance()->getPGroup("lift")->Set(-1); //Lower lift - return HALBot::timerExpired; - } - if (drivebase != nullptr) - drivebase->Drive_v(0, 1.0 * dir, 0.5 * rotateConstant); - return HALBot::no_update; - } - - - int HALBot::toteCount = 0; - AutonMode HALBot::mode = AUTON_MODE_STOP; - int HALBot::getToteCount() - { - return toteCount; - } - bool HALBot::enoughTotes() - { - switch (mode) - { - case AUTON_MODE_TOTE: - return toteCount >= 1; - break; - case AUTON_MODE_STACK3: - return toteCount >= 3; - break; - case AUTON_MODE_STACK2: - return toteCount >= 2; - break; - default: - return true; - } - } - void HALBot::incrTote() - { - toteCount++; - } - HALBot::HALBot() - { - stopped = new Stopped; - gettingTote = new GettingTote; - driveToZone = new DriveToZone; - rotate = new Rotate; - rotate2 = new Rotate; - forkGrab = new ForkGrab; - pushContainer = new PushContainer; - backAway = new BackAway; - fsm = new FiniteStateMachine; - rotateDrive = new RotateDrive; - mode = AUTON_MODE_DRIVE; //Just drive straight forward. Assumes a spherical cow. - } - HALBot::~HALBot() - { - delete stopped; - delete gettingTote; - delete driveToZone; - delete rotate; - delete rotateDrive; - delete forkGrab; - delete pushContainer; - delete backAway; - delete fsm; - toteCount = 0; - } - void HALBot::init(MecanumDrive* drivebase, MotorGrouping* intake, PneumaticGrouping* lift) - { - int i; - sysLog = Logger::getInstance()->getLog("sysLog"); - gettingTote->setHardware(drivebase, intake); - driveToZone->setHardware(drivebase); - rotate->setHardware(drivebase); - rotateDrive->setHardware(drivebase); - rotate2->setHardware(drivebase); - pushContainer->setHardware(drivebase); - pushContainer->pusher1 = XMLInput::getInstance()->getPWMMotor(0); - pushContainer->pusher2 = XMLInput::getInstance()->getPWMMotor(1); - pushContainer->pushConstant = 1; - stopped->lift = lift; //Don't know if I like these... - forkGrab->lift = lift; - forkGrab->drivebase = drivebase; - backAway->lift = lift; - backAway->drivebase = drivebase; - - - //Apply state tables and set the starting state - FSMState* defState = nullptr; - if (mode == AUTON_MODE_STOP) - { - i = 0; - transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; - transitionTable[i++] = END_STATE_TABLE; - defState = stopped; - } - if (mode == AUTON_MODE_DRIVE) - { - i = 0; - driveToZone->strafe = false; - driveToZone->dir = -1; - transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, rotate}; - transitionTable[i++] = {rotate, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; - transitionTable[i++] = END_STATE_TABLE; - defState = driveToZone; - } - if (mode == AUTON_MODE_TOTE) - { - i = 0; - rotate->rotateConstant = 1; - driveToZone->strafe = false; - transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; - transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, rotate}; - transitionTable[i++] = {rotate, HALBot::timerExpired, nullptr, driveToZone}; - transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, rotate2}; - transitionTable[i++] = {rotate2, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; - transitionTable[i++] = END_STATE_TABLE; - defState = gettingTote; - } - if (mode == AUTON_MODE_CONTAINER) - { - driveToZone->strafe = true; - i = 0; - rotate->rotateConstant = -1; - transitionTable[i++] = {rotate, HALBot::timerExpired, nullptr, driveToZone}; - transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = END_STATE_TABLE; - } - if (mode == AUTON_MODE_BOTH) - { - i = 0; - transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; - transitionTable[i++] = END_STATE_TABLE; - defState = stopped; - } - if (mode == AUTON_MODE_STACK2) - { - i = 0; - rotateDrive->rotateConstant = -1; - pushContainer->pushConstant = -1; - pushContainer->enableScaling = true; - driveToZone->strafe = false; - incrTote(); //We already have a tote - - transitionTable[i++] = {pushContainer, HALBot::timerExpired, nullptr, gettingTote}; - transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; - transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; - transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, rotateDrive}; - transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; - defState = pushContainer; - } - if (mode == AUTON_MODE_STACK3) - { - i = 0; - rotateDrive->rotateConstant = -1; - pushContainer->pushConstant = -1; - pushContainer->enableScaling = true; - driveToZone->strafe = false; - incrTote(); //We already have a tote - - transitionTable[i++] = {pushContainer, HALBot::timerExpired, nullptr, gettingTote}; - transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; - transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; - transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, rotateDrive}; - transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, backAway}; - transitionTable[i++] = {backAway, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; - defState = pushContainer; - } - - fsm->init(transitionTable, defState); - } - void HALBot::update() - { - fsm->update(); - } - void HALBot::setMode(AutonMode newMode) - { - mode = newMode; - } - AutonMode HALBot::getMode() - { - return mode; //Used exactly once, in AutonInit(). This must be killed with fire when possible. - } -} diff --git a/src/Autonomous.h b/src/Autonomous.h deleted file mode 100644 index 7877727..0000000 --- a/src/Autonomous.h +++ /dev/null @@ -1,144 +0,0 @@ -#pragma once - -#include -#include "WPILib.h" -#include "Timer.h" -#include "MecanumDrive.h" -#include "XMLInput.h" -#include "FSM.h" -#include "DreadbotDIO.h" -#include "../lib/Logger.h" -using namespace Hydra; - -//All timings -#define STRAFE_TO_ZONE_TIME 3.1f //Used in some auton modes (!2TA !3TA) - how long does the robot strafe? -#define DRIVE_TO_ZONE_TIME 2.0f //How long the robot normally drives forward - -#define PUSH_TIME 0.9 //How long the robot pushes containers out of the way -#define PUSH_SPEED 0.75 //How fast the robot drives forward when pushing containers - -#define BACK_AWAY_TIME 0.75f //How long the robot backs up after releasing totes in 3TA -#define ROTATE_TIME 2.5f //Also, timing is modified in RotateDrive::update - 1.0 s is subtracted -#define ROTATE_DRIVE_STRAIGHT 1.0f //How long to drive straight in RotateDrive AFTER rotating -#define ESTOP_TIME 5.0f //How long the robot waits without getting a tote until it e-stops (drops dead in its place) -#define STACK_CORRECTION_TIME 0.35f //How long the robot jerks backward in order to fix tote alignment - -namespace dreadbot -{ - //States - class GettingTote : public FSMState - { - public: - GettingTote(); - virtual void enter(); - virtual int update(); - void setHardware(MecanumDrive* newDrivebase, MotorGrouping* newIntake); - private: - MecanumDrive* drivebase; - MotorGrouping* intake; - bool timerActive; - Timer getTimer; - Timer eStopTimer; - }; - class DriveToZone : public FSMState - { - public: - DriveToZone(); - virtual void enter(); - virtual int update(); - void setHardware(MecanumDrive* newDrivebase); - Timer driveTimer; - int dir; - bool strafe; - protected: - MecanumDrive* drivebase; - bool timerActive; - }; - class ForkGrab : public FSMState - { - public: - ForkGrab(); - virtual void enter(); - virtual int update(); - Timer grabTimer; - PneumaticGrouping* lift; - MecanumDrive* drivebase; - protected: - bool timerActive; - }; - class Rotate : public DriveToZone - { - public: - Rotate(); - virtual void enter(); - virtual int update(); - - int rotateConstant; - }; - class Stopped : public FSMState - { - public: - virtual void enter(); - virtual int update(); - PneumaticGrouping* lift; - }; - class PushContainer : public DriveToZone - { - public: - PushContainer(); - virtual void enter(); - virtual int update(); - Talon* pusher1; - Talon* pusher2; - int pushConstant; - bool enableScaling; - }; - class BackAway : public ForkGrab - { - public: - virtual void enter(); - virtual int update(); - MecanumDrive* drivebase; - }; - class RotateDrive : public Rotate - { - public: - RotateDrive(); - int update(); - void enter(); - }; - - //Needs to appear in https://en.wikipedia.org/wiki/Kludge#Computer_science - class HALBot - { - public: - enum fsmInputs {no_update, finish, timerExpired, nextTote, eStop}; - - HALBot(); - ~HALBot(); - static bool enoughTotes(); //Stupid global way of determining if the robot has acquired enough totes. Kludgish, really. - static void incrTote(); //Increment the collected tote cout. - static int getToteCount(); //Gets the tote count. Used in a few kludgish areas. Also stupid. - void setMode(AutonMode newMode); //Called during AutonomousInit. Determines what autonomous mode to run. - AutonMode getMode(); //Gets the mode. Used in AutonomousInit. Also really, really dumb/stupid. - void init(MecanumDrive* drivebase, MotorGrouping* intake, PneumaticGrouping* lift); //Sets hardware, intializes stuff, and prepares the transition tables. Assumes that the setMode thing has been used already. - void update(); //Basically just a cheap call to FiniteStateMachine::update. - private: - static int toteCount; //How many totes have been acquired. - FiniteStateMachine* fsm; - static AutonMode mode; //The mode that the robot is in. Also dumb. - - FSMTransition transitionTable[15]; //The transition table used for transitioning. Changes based on the setMode thingy. - - //State objects. These should be self-explanatory. - GettingTote* gettingTote; - DriveToZone* driveToZone; - ForkGrab* forkGrab; - Rotate* rotate; - Rotate* rotate2; - Stopped* stopped; - PushContainer* pushContainer; - BackAway* backAway; - RotateDrive* rotateDrive; - }; -} diff --git a/src/Autonomous/BackAway.cpp b/src/Autonomous/BackAway.cpp new file mode 100644 index 0000000..59c1c35 --- /dev/null +++ b/src/Autonomous/BackAway.cpp @@ -0,0 +1,44 @@ +#include "BackAway.h" + +namespace dreadbot +{ + void BackAway::enter() + { + sysLog->log("State: BackAway"); + drivebase->Drive_v(0, 0, 0); + timerActive = false; //Cheat way of figuring out if the lift is down. Used elsewhere + } + int BackAway::update() + { + //While the lift isn't down + if (!timerActive) + { + lift->Set(-1); + + //On first lift down + if (isLiftDown()) + { + timerActive = true; + liftArms->Set(-1); + Wait(0.13); //Uber cheap way of getting the totes to disengage + grabTimer.Reset(); + grabTimer.Start(); + } + return RoboState::no_update; + } + + if (grabTimer.Get() >= BACK_AWAY_TIME) + { + drivebase->Drive_v(0, 0, 0); + return RoboState::timerExpired; + } + + if (drivebase != nullptr) + { + drivebase->GoFast(); + drivebase->Drive_v(0, -1, 0); + } + //liftArms->Set(-1); + return RoboState::no_update; + } +} diff --git a/src/Autonomous/BackAway.h b/src/Autonomous/BackAway.h new file mode 100644 index 0000000..ca5608d --- /dev/null +++ b/src/Autonomous/BackAway.h @@ -0,0 +1,13 @@ +#pragma once + +#include "ForkGrab.h" + +namespace dreadbot +{ + class BackAway : public ForkGrab + { + public: + virtual void enter(); + virtual int update(); + }; +} diff --git a/src/Autonomous/DriveToZone.cpp b/src/Autonomous/DriveToZone.cpp new file mode 100644 index 0000000..930ce99 --- /dev/null +++ b/src/Autonomous/DriveToZone.cpp @@ -0,0 +1,47 @@ +#include "DriveToZone.h" + +namespace dreadbot +{ + DriveToZone::DriveToZone() + { + timerActive = false; + strafe = false; + dir = 1; //Multiplier that changes the direction the robot moves. + } + void DriveToZone::enter() + { + driveTimer.Reset(); + driveTimer.Start(); + timerActive = true; + if (RoboState::toteCount < 3) + lift->Set(1); //Raise the lift for tote transit - it's more stable that way. + sysLog->log("State: DriveToZone"); + } + int DriveToZone::update() + { + float drvZoneTime = DRIVE_TO_ZONE_TIME; + if (RoboState::toteCount == 0) + drvZoneTime += 0.4f; //The robot starts farther back when no tote is collected. This adds on additional time to make sure the robot still drives far enough. + + //Break once the robot has moved far enough - timing based. + if ((driveTimer.Get() >= drvZoneTime && !strafe) || (driveTimer.Get() >= STRAFE_TO_ZONE_TIME && strafe)) + { + driveTimer.Stop(); + driveTimer.Reset(); + timerActive = false; + drivebase->Drive_v(0, 0, 0); + return RoboState::timerExpired; + } + + //Apply actual velocity changes + if (drivebase != nullptr) + { + if (strafe) + drivebase->Drive_v(1, 0, 0); //Right + else + drivebase->Drive_v(0, 0.8 * dir, 0); // Do a short dance + } + + return RoboState::no_update; + } +} diff --git a/src/Autonomous/DriveToZone.h b/src/Autonomous/DriveToZone.h new file mode 100644 index 0000000..1520bfd --- /dev/null +++ b/src/Autonomous/DriveToZone.h @@ -0,0 +1,19 @@ +#pragma once + +#include "RoboState.h" + +namespace dreadbot +{ + class DriveToZone : public RoboState + { + public: + DriveToZone(); + virtual void enter(); + virtual int update(); + Timer driveTimer; + int dir; + bool strafe; + protected: + bool timerActive; + }; +} diff --git a/src/FSM.cpp b/src/Autonomous/FSM.cpp similarity index 100% rename from src/FSM.cpp rename to src/Autonomous/FSM.cpp diff --git a/src/FSM.h b/src/Autonomous/FSM.h similarity index 96% rename from src/FSM.h rename to src/Autonomous/FSM.h index 916341d..cc3c567 100644 --- a/src/FSM.h +++ b/src/Autonomous/FSM.h @@ -1,7 +1,5 @@ #pragma once -#include "WPILib.h" - #define END_STATE_TABLE {nullptr, 0, nullptr, nullptr} namespace dreadbot diff --git a/src/Autonomous/ForkGrab.cpp b/src/Autonomous/ForkGrab.cpp new file mode 100644 index 0000000..59a2161 --- /dev/null +++ b/src/Autonomous/ForkGrab.cpp @@ -0,0 +1,69 @@ +#include "ForkGrab.h" + +namespace dreadbot +{ + ForkGrab::ForkGrab() + { + timerActive = false; + } + void ForkGrab::enter() + { + sysLog->log("State: ForkGrab"); + grabTimer.Reset(); + grabTimer.Start(); + } + bool thing = true; //PARKER, NO! + int ForkGrab::update() + { + if (RoboState::toteCount >= 2 && grabTimer.Get() >= LIFT_ENGAGEMENT_DELAY) + { + intakeArms->Set(1); + drivebase->GoFast(); + drivebase->Drive_v(0, RD_DRIVE_SPEED, RD_ROTATE_SPEED); + if (isLiftDown()) + { + //XMLInput::getInstance()->getPGroup("liftArms")->Set(0); + lift->Set(1); //Raise lift + Wait(0.3); //Totes must engage first + lift->Set(0); + return RoboState::finish; + } else { + return RoboState::no_update; + } + } else if (RoboState::toteCount >= 2 && grabTimer.Get() >= 0.2 && thing) { + //XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); + thing = false; + } + + if (isLiftDown()) + { + //XMLInput::getInstance()->getPGroup("liftArms")->Set(0); + RoboState::toteCount++; //Once the lift is down, it is assumed that the tote is actually collected, i.e. in the fork. + + //special 3-tote auton condition. Really kludgy. Causes the robot to NOT lift before rotating/driving + if (RoboState::toteCount >= 3 && RoboState::toteCount >= RoboState::neededTCount) + { + lift->Set(1); //Raise lift + Wait(0.25); //Totes must engage first + lift->Set(0); + return RoboState::finish; + } + //Raise the lift and cheat to alight the tote + drivebase->GoFast(); + drivebase->Drive_v(0, STACK_CORRECTION_SPEED, 0); // @todo Calibrate + Wait(STACK_CORRECTION_TIME); + drivebase->Drive_v(0, 0, 0); + drivebase->GoSlow(); + lift->Set(1); + Wait(0.3); + if (RoboState::toteCount >= RoboState::neededTCount) + return RoboState::finish; + else + return RoboState::nextTote; + } + drivebase->Drive_v(0, 0, 0); + if (lift != nullptr) + lift->Set(-1); //Lower the lift for grabbing + return RoboState::no_update; + } +} diff --git a/src/Autonomous/ForkGrab.h b/src/Autonomous/ForkGrab.h new file mode 100644 index 0000000..425d362 --- /dev/null +++ b/src/Autonomous/ForkGrab.h @@ -0,0 +1,17 @@ +#pragma once + +#include "RoboState.h" + +namespace dreadbot +{ + class ForkGrab : public RoboState + { + public: + ForkGrab(); + virtual void enter(); + virtual int update(); + Timer grabTimer; + protected: + bool timerActive; + }; +} diff --git a/src/Autonomous/GettingTote.cpp b/src/Autonomous/GettingTote.cpp new file mode 100644 index 0000000..2a483f2 --- /dev/null +++ b/src/Autonomous/GettingTote.cpp @@ -0,0 +1,70 @@ +#include "GettingTote.h" + +namespace dreadbot +{ + GettingTote::GettingTote() + { + timerActive = false; + } + void GettingTote::enter() + { + lift->Set(1); //Raise the lift + eStopTimer.Start(); //estop timer - if limit is passed, automatically estops the robot. + sysLog->log("State: GettingTote"); + } + int GettingTote::update() + { + if (isToteInTransit() && !timerActive) //Run once, upon tote contact with transit wheels. + { + //Stay still while a tote is loaded + timerActive = true; //This doesn't really control a timer anymore... + drivebase->Drive_v(0, 0, 0); + } + if (!isToteInTransit() && timerActive) + { + //Tote successfully collected - it should have just left contact with the transit wheels. + intake->Set(0); + timerActive = false; + eStopTimer.Stop(); + eStopTimer.Reset(); + return RoboState::timerExpired; + } + if (timerActive) + { + //Keep sucking a tote in + if (RoboState::toteCount >= 2) + intake->Set(-1); + else + intake->Set(-0.5); + + return RoboState::no_update; + } + if (RoboState::toteCount != 0) //Open the intake arms for grabbing totes after the first tote is collected + intakeArms->Set(-1); + + drivebase->Drive_v(0, -0.65, 0); + if (RoboState::toteCount >= 2) + intake->Set(-1); + else + intake->Set(-0.6); + + if (eStopTimer.Get() >= 3.5) + intakeArms->Set(1); + else if (eStopTimer.Get() >= 2.5 && eStopTimer.Get() < 3.5) + intakeArms->Set(0); + + // Emergeny stop in case the tote is missed. + // Stop the drivebase, raise the lift, passify the intake arms, and stop the transit wheels. + if (eStopTimer.Get() >= ESTOP_TIME) + { + eStopTimer.Stop(); + eStopTimer.Reset(); + drivebase->Drive_v(0, 0, 0);intake->Set(0); + lift->Set(1); // Raise the lift + sysLog->log("Emergency-stopped in GettingTote", Hydra::error); + return RoboState::eStop; + } + + return RoboState::no_update; + } +} diff --git a/src/Autonomous/GettingTote.h b/src/Autonomous/GettingTote.h new file mode 100644 index 0000000..29e82da --- /dev/null +++ b/src/Autonomous/GettingTote.h @@ -0,0 +1,18 @@ +#pragma once + +#include "RoboState.h" + +namespace dreadbot +{ + class GettingTote : public RoboState + { + public: + GettingTote(); + virtual void enter(); + virtual int update(); + private: + bool timerActive; + Timer getTimer; + Timer eStopTimer; + }; +} diff --git a/src/Autonomous/HALBot.cpp b/src/Autonomous/HALBot.cpp new file mode 100644 index 0000000..4850349 --- /dev/null +++ b/src/Autonomous/HALBot.cpp @@ -0,0 +1,150 @@ +#include "HALBot.h" + +namespace dreadbot +{ + HALBot::HALBot() + { + sysLog = nullptr; + stopped = new Stopped; + gettingTote = new GettingTote; + driveToZone = new DriveToZone; + rotate = new Rotate; + rotate2 = new Rotate; + forkGrab = new ForkGrab; + pushContainer = new PushContainer; + backAway = new BackAway; + fsm = new FiniteStateMachine; + rotateDrive = new RotateDrive; + strafeLeft = new StrafeLeft; + } + HALBot::~HALBot() + { + delete stopped; + delete gettingTote; + delete driveToZone; + delete rotate; + delete rotateDrive; + delete forkGrab; + delete pushContainer; + delete backAway; + delete fsm; + delete strafeLeft; + RoboState::toteCount = 0; + RoboState::neededTCount = 0; + } + void HALBot::init(MecanumDrive* drivebase, MotorGrouping* intake, PneumaticGrouping* lift) + { + int i; + sysLog = Logger::getInstance()->getLog("sysLog"); + RoboState::drivebase = drivebase; + RoboState::intake = intake; + RoboState::lift = lift; + RoboState::liftArms = XMLInput::getInstance()->getPGroup("liftArms"); + RoboState::intakeArms = XMLInput::getInstance()->getPGroup("intakeArms"); + RoboState::pusher1 = XMLInput::getInstance()->getPWMMotor(0); + RoboState::pusher2 = XMLInput::getInstance()->getPWMMotor(1); + RoboState::sysLog = sysLog; + pushContainer->pushConstant = 1; + + //Apply state tables and set the starting state. Note that RoboState::neededTCount is 0 before this. + FSMState* defState = nullptr; + AutonMode mode = GetAutonMode(); + if (mode == AUTON_MODE_STOP) + { + i = 0; + transitionTable[i++] = {stopped, RoboState::no_update, nullptr, stopped}; + transitionTable[i++] = END_STATE_TABLE; + defState = stopped; + } + if (mode == AUTON_MODE_DRIVE) + { + i = 0; + driveToZone->strafe = false; + driveToZone->dir = -1; + transitionTable[i++] = {driveToZone, RoboState::timerExpired, nullptr, rotate}; + transitionTable[i++] = {rotate, RoboState::timerExpired, nullptr, stopped}; + transitionTable[i++] = {stopped, RoboState::no_update, nullptr, stopped}; + transitionTable[i++] = END_STATE_TABLE; + defState = driveToZone; + } + if (mode == AUTON_MODE_TOTE) + { + i = 0; + RoboState::neededTCount = 1; + rotate->rotateConstant = 1; + driveToZone->strafe = false; + transitionTable[i++] = {gettingTote, RoboState::timerExpired, nullptr, forkGrab}; + transitionTable[i++] = {forkGrab, RoboState::finish, nullptr, rotate}; + transitionTable[i++] = {rotate, RoboState::timerExpired, nullptr, driveToZone}; + transitionTable[i++] = {driveToZone, RoboState::timerExpired, nullptr, rotate2}; + transitionTable[i++] = {rotate2, RoboState::timerExpired, nullptr, stopped}; + transitionTable[i++] = {gettingTote, RoboState::eStop, nullptr, stopped}; + transitionTable[i++] = {stopped, RoboState::no_update, nullptr, stopped}; + transitionTable[i++] = END_STATE_TABLE; + defState = gettingTote; + } + if (mode == AUTON_MODE_CONTAINER) + { + driveToZone->strafe = true; + i = 0; + rotate->rotateConstant = -1; + transitionTable[i++] = {rotate, RoboState::timerExpired, nullptr, driveToZone}; + transitionTable[i++] = {driveToZone, RoboState::timerExpired, nullptr, stopped}; + transitionTable[i++] = END_STATE_TABLE; + } + if (mode == AUTON_MODE_BOTH) + { + //Not really used. + i = 0; + RoboState::neededTCount = 1; + transitionTable[i++] = {stopped, RoboState::no_update, nullptr, stopped}; + transitionTable[i++] = END_STATE_TABLE; + defState = stopped; + } + if (mode == AUTON_MODE_STACK2) + { + i = 0; + rotateDrive->rotateConstant = -1; + pushContainer->pushConstant = -1; + pushContainer->enableScaling = true; + driveToZone->strafe = false; + RoboState::toteCount++; //We already have a tote + RoboState::neededTCount = 2; + + transitionTable[i++] = {pushContainer, RoboState::timerExpired, nullptr, gettingTote}; + transitionTable[i++] = {gettingTote, RoboState::timerExpired, nullptr, forkGrab}; + transitionTable[i++] = {gettingTote, RoboState::eStop, nullptr, stopped}; + transitionTable[i++] = {forkGrab, RoboState::finish, nullptr, strafeLeft}; + transitionTable[i++] = {forkGrab, RoboState::nextTote, nullptr, pushContainer}; + transitionTable[i++] = {strafeLeft, RoboState::timerExpired, nullptr, rotateDrive}; + transitionTable[i++] = {rotateDrive, RoboState::timerExpired, nullptr, stopped}; + transitionTable[i++] = {stopped, RoboState::no_update, nullptr, stopped}; + defState = pushContainer; + } + if (mode == AUTON_MODE_STACK3) + { + i = 0; + rotateDrive->rotateConstant = -1; + pushContainer->pushConstant = -1; + pushContainer->enableScaling = true; + driveToZone->strafe = false; + RoboState::toteCount++; //We already have a tote + RoboState::neededTCount = 3; + + transitionTable[i++] = {pushContainer, RoboState::timerExpired, nullptr, gettingTote}; + transitionTable[i++] = {gettingTote, RoboState::timerExpired, nullptr, forkGrab}; + transitionTable[i++] = {forkGrab, RoboState::nextTote, nullptr, pushContainer}; + transitionTable[i++] = {forkGrab, RoboState::finish, nullptr, rotateDrive}; + transitionTable[i++] = {rotateDrive, RoboState::timerExpired, nullptr, backAway}; + transitionTable[i++] = {backAway, RoboState::timerExpired, nullptr, stopped}; + transitionTable[i++] = {gettingTote, RoboState::eStop, nullptr, stopped}; + defState = pushContainer; + } + + fsm->init(transitionTable, defState); + } + void HALBot::update() + { + fsm->update(); + } +} diff --git a/src/Autonomous/HALBot.h b/src/Autonomous/HALBot.h new file mode 100644 index 0000000..280705d --- /dev/null +++ b/src/Autonomous/HALBot.h @@ -0,0 +1,48 @@ +#pragma once + +#include "../../lib/Logger.h" +#include "../DreadbotDIO.h" + +//All states +#include "BackAway.h" +#include "DriveToZone.h" +#include "ForkGrab.h" +#include "GettingTote.h" +#include "PushContainer.h" +#include "Rotate.h" +#include "RotateDrive.h" +#include "Stopped.h" +#include "StrafeLeft.h" +#include "RoboState.h" + +using namespace Hydra; + +namespace dreadbot +{ + //Needs to appear in https://en.wikipedia.org/wiki/Kludge#Computer_science + class HALBot + { + public: + HALBot(); + ~HALBot(); + void setMode(AutonMode newMode); //Called during AutonomousInit. Determines what autonomous mode to run + void init(MecanumDrive* drivebase, MotorGrouping* intake, PneumaticGrouping* lift); //Sets hardware, intializes stuff, and prepares the transition tables. Assumes that the setMode thing has been used already. + void update(); //Basically just a cheap call to FiniteStateMachine::update. + private: + FiniteStateMachine* fsm; + FSMTransition transitionTable[15]; //The transition table used for transitioning. Changes based on the setMode thingy. + Log* sysLog; + + //State objects. These should be self-explanatory. + GettingTote* gettingTote; + DriveToZone* driveToZone; + ForkGrab* forkGrab; + Rotate* rotate; + Rotate* rotate2; + Stopped* stopped; + PushContainer* pushContainer; + BackAway* backAway; + RotateDrive* rotateDrive; + StrafeLeft* strafeLeft; + }; +} diff --git a/src/Autonomous/PushContainer.cpp b/src/Autonomous/PushContainer.cpp new file mode 100644 index 0000000..e917968 --- /dev/null +++ b/src/Autonomous/PushContainer.cpp @@ -0,0 +1,45 @@ +#include "PushContainer.h" + +namespace dreadbot +{ + PushContainer::PushContainer() + { + enableScaling = false; + pushConstant = 1; //OK, since + } + void PushContainer::enter() + { + pushConstant *= -1; //Not used, but it can change the direction the robot pushes containers + sysLog->log("State: PushContainer"); + } + int PushContainer::update() + { + float pushTime = PUSH_TIME; + if (enableScaling) //I refuse comment on this bit. Let's just say that it makes the robot push less. + pushTime += ((float)RoboState::toteCount - 1.f) / 3.f; //Scaling for three-tote autonomous, since the second container is farther away than the first + intakeArms->Set(1); //Intake arms in + if (!timerActive) + { + driveTimer.Reset(); + driveTimer.Start(); + timerActive = true; + } + + if (driveTimer.Get() >= pushTime) + { + timerActive = false; + drivebase->Drive_v(0, 0, 0); + return RoboState::timerExpired; + } + if (RoboState::toteCount >= 2) { + drivebase->Drive_v(DRIVE_STRAFE_CORRECTION, -PUSH_SPEED, DRIVE_ROTATE_CORRECTION); //Straight forward + } else { + drivebase->Drive_v(0.0f, -PUSH_SPEED, 0.0f); + } + if (pusher1 != nullptr) + pusher1->Set(INTAKE_PUSH_SPEED); //Push the container? + if (pusher2 != nullptr) + pusher2->Set(INTAKE_PUSH_SPEED); + return RoboState::no_update; + } +} diff --git a/src/Autonomous/PushContainer.h b/src/Autonomous/PushContainer.h new file mode 100644 index 0000000..61bf326 --- /dev/null +++ b/src/Autonomous/PushContainer.h @@ -0,0 +1,16 @@ +#pragma once + +#include "DriveToZone.h" + +namespace dreadbot +{ + class PushContainer : public DriveToZone + { + public: + PushContainer(); + virtual void enter(); + virtual int update(); + int pushConstant; + bool enableScaling; + }; +} diff --git a/src/Autonomous/RoboState.cpp b/src/Autonomous/RoboState.cpp new file mode 100644 index 0000000..b77ae91 --- /dev/null +++ b/src/Autonomous/RoboState.cpp @@ -0,0 +1,17 @@ +#include "RoboState.h" + +namespace dreadbot +{ + //These are needed for static members... ugh. 1.5 hours. + MecanumDrive* RoboState::drivebase = nullptr; + MotorGrouping* RoboState::intake = nullptr; + PneumaticGrouping* RoboState::lift = nullptr; + PneumaticGrouping* RoboState::liftArms = nullptr; + PneumaticGrouping* RoboState::intakeArms = nullptr; + Talon* RoboState::pusher1 = nullptr; + Talon* RoboState::pusher2 = nullptr; + Log* RoboState::sysLog = nullptr; + + int RoboState::toteCount = 0; + int RoboState::neededTCount = 0; +} diff --git a/src/Autonomous/RoboState.h b/src/Autonomous/RoboState.h new file mode 100644 index 0000000..af5f925 --- /dev/null +++ b/src/Autonomous/RoboState.h @@ -0,0 +1,67 @@ +#pragma once + +#include "WPILib.h" +#include "Timer.h" +#include "../MecanumDrive.h" +#include "../XMLInput.h" +#include "FSM.h" +#include "../DreadbotDIO.h" +#include "../../lib/Logger.h" +using namespace Hydra; + +/******************* +*CALIBRATION VALUES* +*******************/ +#define ESTOP_TIME 5.0f // How long the robot will wait without getting a tote until it e-stops +#define STRAFE_TO_ZONE_TIME 3.1f // Used in some auton modes (!2TA !3TA) - how long does the robot strafe? +#define DRIVE_TO_ZONE_TIME 2.0f // How long the robot normally drives forward + +// Container evacuation parameter +#define INTAKE_PUSH_SPEED 0.9f // Rotational velocity of the intake wheels when pushing a container out of the way +#define PUSH_TIME 1.0f // 0.9 How long the robot pushes containers out of the way +#define PUSH_SPEED 0.7f // How fast the robot drives forward when pushing containers +//#define DRIVE_STRAFE_CORRECTION 0.05f // Aport drift abbertaion correctional factor +//#define DRIVE_ROTATE_CORRECTION 0.03f // Rotate abberation correctional factor +#define DRIVE_STRAFE_CORRECTION 0.0f // Aport drift abbertaion correctional factor +#define DRIVE_ROTATE_CORRECTION 0.0f // Rotate abberation correctional factor + +// Endgame parameters +#define RD_DRIVE_SPEED 1.0f // Astern speed in RotateDrive. +#define RD_ROTATE_SPEED -0.4f // Size of the RotateDrive arc +#define BACK_AWAY_TIME 0.6f // How long the robot backs up after stopping in 3TA +#define ROTATE_TIME 0.6f // Also, timing is modified in RotateDrive::update - 1.0 s is subtracted +#define ROTATE_DRIVE_STRAIGHT 0.6f // How long to drive straight in RotateDrive AFTER rotating + +// Tote collection parameters +#define STACK_CORRECTION_TIME 0.25f // How long the robot jerks backward +#define STACK_CORRECTION_SPEED 0.85f // How quickly the robot jerks backward +#define LIFT_ENGAGEMENT_DELAY 0.5f // How long the robot waits after starting to lower the lift while collecting the third tote before reversing. + + +namespace dreadbot +{ + class RoboState : public FSMState + { + public: + enum rVals {no_update, finish, timerExpired, nextTote, eStop}; + + virtual void enter() = 0; + virtual int update() = 0; + virtual ~RoboState() {} + protected: + //Hardware for access for all states + static MecanumDrive* drivebase; + static MotorGrouping* intake; + static PneumaticGrouping* lift; + static PneumaticGrouping* liftArms; + static PneumaticGrouping* intakeArms; + static Talon* pusher1; + static Talon* pusher2; + static Log* sysLog; + + static int toteCount; + static int neededTCount; //How many totes you need. Substitute for the terrifying HALBot::enoughTotes() function. + + friend class HALBot; + }; +} diff --git a/src/Autonomous/Rotate.cpp b/src/Autonomous/Rotate.cpp new file mode 100644 index 0000000..25457d5 --- /dev/null +++ b/src/Autonomous/Rotate.cpp @@ -0,0 +1,31 @@ +#include "Rotate.h" + +namespace dreadbot +{ + Rotate::Rotate() + { + timerActive = false; + rotateConstant = 1; //Changes the direction that the robot turns. + } + void Rotate::enter() + { + driveTimer.Reset(); + driveTimer.Start(); + timerActive = true; + sysLog->log("State: Rotate"); + } + int Rotate::update() + { + if (driveTimer.Get() >= ROTATE_TIME) + { //Rotated far enough; break + timerActive = false; + drivebase->Drive_v(0, 0, 0); + if (RoboState::toteCount == 3) + lift->Set(-1); //Lower lift + return RoboState::timerExpired; + } + if (drivebase != nullptr) + drivebase->Drive_v(0, 0, 0.5 * rotateConstant); + return RoboState::no_update; + } +} diff --git a/src/Autonomous/Rotate.h b/src/Autonomous/Rotate.h new file mode 100644 index 0000000..116a5f8 --- /dev/null +++ b/src/Autonomous/Rotate.h @@ -0,0 +1,16 @@ +#pragma once + +#include "DriveToZone.h" + +namespace dreadbot +{ + class Rotate : public DriveToZone + { + public: + Rotate(); + virtual void enter(); + virtual int update(); + + int rotateConstant; + }; +} diff --git a/src/Autonomous/RotateDrive.cpp b/src/Autonomous/RotateDrive.cpp new file mode 100644 index 0000000..255be6c --- /dev/null +++ b/src/Autonomous/RotateDrive.cpp @@ -0,0 +1,37 @@ +#include "RotateDrive.h" + +namespace dreadbot +{ + RotateDrive::RotateDrive() + { + timerActive = false; + rotateConstant = 1; + } + void RotateDrive::enter() + { + sysLog->log("State: RotateDrive"); + drivebase->GoFast(); //Gotta go faaaaaaaasssst. + driveTimer.Start(); + //liftArms->Set(0); + } + int RotateDrive::update() + { + if (driveTimer.Get() >= (ROTATE_TIME - 0.5f)) + { //Rotated far enough; break + timerActive = false; + drivebase->GoSpeed(1.0); + drivebase->Drive_v(0, 1, 0); + + // The stack is lowered prior to stopping in order to decelerate properly + if (RoboState::toteCount == 3) + lift->Set(-1); //Lower lift + + Wait(ROTATE_DRIVE_STRAIGHT); + + return RoboState::timerExpired; + } + if (drivebase != nullptr) + drivebase->Drive_v(0, RD_DRIVE_SPEED, RD_ROTATE_SPEED); + return RoboState::no_update; + } +} diff --git a/src/Autonomous/RotateDrive.h b/src/Autonomous/RotateDrive.h new file mode 100644 index 0000000..7939e5e --- /dev/null +++ b/src/Autonomous/RotateDrive.h @@ -0,0 +1,14 @@ +#pragma once + +#include "Rotate.h" + +namespace dreadbot +{ + class RotateDrive : public Rotate + { + public: + RotateDrive(); + int update(); + void enter(); + }; +} diff --git a/src/Autonomous/Stopped.cpp b/src/Autonomous/Stopped.cpp new file mode 100644 index 0000000..dd283e6 --- /dev/null +++ b/src/Autonomous/Stopped.cpp @@ -0,0 +1,17 @@ +#include "Stopped.h" + +namespace dreadbot +{ + void Stopped::enter() + { + sysLog->log("State: Stopped"); + if (drivebase != nullptr) + drivebase->Drive_v(0, 0, 0); + if (RoboState::toteCount == 3) + liftArms->Set(0); //Only lower the forks when the robot is in 3TA - it's not needed elsewhere + } + int Stopped::update() + { + return 0;//HALBot::no_update; //Does nothing of significance. + } +} diff --git a/src/Autonomous/Stopped.h b/src/Autonomous/Stopped.h new file mode 100644 index 0000000..3b3da90 --- /dev/null +++ b/src/Autonomous/Stopped.h @@ -0,0 +1,13 @@ +#pragma once + +#include "RoboState.h" + +namespace dreadbot +{ + class Stopped : public RoboState + { + public: + virtual void enter(); + virtual int update(); + }; +} diff --git a/src/Autonomous/StrafeLeft.cpp b/src/Autonomous/StrafeLeft.cpp new file mode 100644 index 0000000..f07dc5d --- /dev/null +++ b/src/Autonomous/StrafeLeft.cpp @@ -0,0 +1,29 @@ +#include "StrafeLeft.h" + +namespace dreadbot +{ + StrafeLeft::StrafeLeft() + { + timerActive = false; + } + void StrafeLeft::enter() + { + sysLog->log("State: StrafeLeft"); + driveTimer.Start(); + timerActive = true; + drivebase->GoFast(); //Gotta go faaaaaaaasssst. + drivebase->Drive_v(-1, 0, 0); //Left + } + int StrafeLeft::update() + { + if (driveTimer.Get() >= 0.5f) + { + driveTimer.Stop(); + driveTimer.Reset(); + timerActive = false; + drivebase->Drive_v(0, 0, 0); //stop + return RoboState::timerExpired; + } + return RoboState::no_update; + } +} diff --git a/src/Autonomous/StrafeLeft.h b/src/Autonomous/StrafeLeft.h new file mode 100644 index 0000000..65c4101 --- /dev/null +++ b/src/Autonomous/StrafeLeft.h @@ -0,0 +1,14 @@ +#pragma once + +#include "DriveToZone.h" + +namespace dreadbot +{ + class StrafeLeft : public DriveToZone + { + public: + StrafeLeft(); + int update(); + void enter(); + }; +} diff --git a/src/MecanumDrive.cpp b/src/MecanumDrive.cpp index 168da42..5585d2f 100644 --- a/src/MecanumDrive.cpp +++ b/src/MecanumDrive.cpp @@ -16,9 +16,9 @@ void MecanumDrive::Set(int motorId_lf, int motorId_rf, int motorId_lr, int motor motors[m_leftRear] = new CANTalon(motorId_lr, CONTROL_PERIOD); motors[m_rightRear] = new CANTalon(motorId_rr, CONTROL_PERIOD); - motors[m_leftFront]->SetSensorDirection(true); + motors[m_leftFront]->SetSensorDirection(false); motors[m_rightFront]->SetSensorDirection(false); - motors[m_leftRear]->SetSensorDirection(true); + motors[m_leftRear]->SetSensorDirection(false); motors[m_rightRear]->SetSensorDirection(false); // Configure parameters @@ -26,13 +26,10 @@ void MecanumDrive::Set(int motorId_lf, int motorId_rf, int motorId_lr, int motor motors[i]->SetControlMode(CANSpeedController::ControlMode::kSpeed); motors[i]->SetPosition(0.0); motors[i]->SelectProfileSlot(0); + //motors[i]->SetFeedbackDevice(CANTalon::QuadEncoder); motors[i]->SetPID(0.5, 0, 0, 0); - motors[i]->SetVoltageRampRate(0.5f); //Ramp up for drive motors + motors[i]->SetVoltageRampRate(0.5); //Ramp up for drive motors } - - //x_ctrl = new SimplePID(0.2, 0, 0, false); - //y_ctrl = new SimplePID(0.2, 0, 0, false); - //r_ctrl = new SimplePID(0.2, 0, 0, true); } // Constructor @@ -60,25 +57,23 @@ void MecanumDrive::GoSlow() { } } -// Drive to position -void MecanumDrive::Drive_p(double x, double y, double rotation) { - if (mode == drivemode::absolute) { - //vec_out.rotate((double) gyroscope->GetAngle()); - // yield thread until the robot has passed within a certain tolerance of the target? - // a callback might work? +void MecanumDrive::GoSpeed(double speed) { + for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { + motors[i]->SetPID(speed, 0, 0); //Magically makes the robot drive slower. } } // Drive with wheel velocity void MecanumDrive::Drive_v(double x, double y, double rotation) { - Vector2 vec_out(y, x); + double x_out = y; + double y_out = x; double rot_out = -rotation - (sqrt(x*x + y*y) * 0.01f); double wspeeds[4]; - wspeeds[m_leftFront] = vec_out.x + vec_out.y + rot_out; - wspeeds[m_rightFront] = -vec_out.x + vec_out.y - rot_out; - wspeeds[m_leftRear] = -vec_out.x + vec_out.y + rot_out; - wspeeds[m_rightRear] = vec_out.x + vec_out.y - rot_out; + wspeeds[m_leftFront] = x_out + y_out + rot_out; + wspeeds[m_rightFront] = -x_out + y_out - rot_out; + wspeeds[m_leftRear] = -x_out + y_out + rot_out; + wspeeds[m_rightRear] = x_out + y_out - rot_out; float absSpeeds[4]; for (int i = 0; i < 4; ++i) @@ -92,27 +87,21 @@ void MecanumDrive::Drive_v(double x, double y, double rotation) { } //Determines if a motor has stalled and proceeds to do exactly nothing with that information. + /* bool stall = true; for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { // files.andymark.com/CIM-motor-curve.pdf stall = stall && (motors[i]->GetOutputCurrent() > STALL_MOTOR_CURRENT); } - + */ for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { - motors[i]->Set(wspeeds[i]*motorReversals[i]*SmartDashboard::GetNumber("Speed", 1000.0), syncGroup); // *stall + motors[i]->Set(wspeeds[i]*motorReversals[i]*SmartDashboard::GetNumber("Speed", 1023.f), syncGroup); } } void MecanumDrive::SetDriveMode(drivemode newMode) { - mode = newMode; - CANSpeedController::ControlMode cmode; - if (mode == drivemode::relative) { - cmode = CANSpeedController::ControlMode::kSpeed; - } else if (mode == drivemode::absolute) { - cmode = CANSpeedController::ControlMode::kPosition; - } for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { - motors[i]->SetControlMode(cmode); + motors[i]->SetControlMode(CANSpeedController::ControlMode::kSpeed); } } @@ -140,12 +129,12 @@ void MecanumDrive::SD_OutputDiagnostics() { for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { // SmartDashboard::PutNumber(motorNames[i] + ".temp", motors[i]->GetTemperature()); -// SmartDashboard::PutNumber(motorNames[i] + ".setpoint", motors[i]->GetSetpoint()); -// SmartDashboard::PutNumber(motorNames[i] + ".encoder p", motors[i]->GetPosition()); -// SmartDashboard::PutNumber(motorNames[i] + ".encoder v", motors[i]->GetEncVel()); -// SmartDashboard::PutNumber(motorNames[i] + ".error", motors[i]->GetClosedLoopError()); - SmartDashboard::PutNumber(motorNames[i] + ".voltage", motors[i]->GetOutputVoltage()); - SmartDashboard::PutNumber(motorNames[i] + ".amperage", motors[i]->GetOutputCurrent()); + SmartDashboard::PutNumber(motorNames[i] + ".setpoint", motors[i]->GetSetpoint()); + SmartDashboard::PutNumber(motorNames[i] + ".encoder p", motors[i]->GetPosition()); + SmartDashboard::PutNumber(motorNames[i] + ".encoder v", motors[i]->GetEncVel()); + SmartDashboard::PutNumber(motorNames[i] + ".error", motors[i]->GetClosedLoopError()); +// SmartDashboard::PutNumber(motorNames[i] + ".voltage", motors[i]->GetOutputVoltage()); +// SmartDashboard::PutNumber(motorNames[i] + ".amperage", motors[i]->GetOutputCurrent()); } } diff --git a/src/MecanumDrive.h b/src/MecanumDrive.h index 0247340..45fec44 100644 --- a/src/MecanumDrive.h +++ b/src/MecanumDrive.h @@ -1,16 +1,12 @@ #pragma once #include "WPILib.h" -#include "mathutil.h" -#include "rps.h" -#include "SimplePID.h" #include #include -#define SQUARE_INPUTS #define MOTOR_COUNT 4 #define STALL_MOTOR_CURRENT 50 -#define CONTROL_PERIOD 10 +#define CONTROL_PERIOD 3 namespace dreadbot { @@ -34,6 +30,7 @@ namespace dreadbot { void GoSlow(); void GoFast(); + void GoSpeed(double speed); void Drive_p(double x, double y, double rotation); //Unimplemented position-based driving. void Drive_v(double x, double y, double rotation); //Velocity based driving. void SetDriveMode(drivemode newMode); @@ -47,13 +44,9 @@ namespace dreadbot { protected: bool m_enabled = false; const uint8_t syncGroup = 0x00; - const std::string motorNames[MOTOR_COUNT] = {"Left-front", "Right-front", "Left-rear", "Right-rear"}; + const std::string motorNames[MOTOR_COUNT] = {"LF Drive [1]", "RF Drive [2]", "LB Drive [3]", "RB Drive [4]"}; const double motorReversals[MOTOR_COUNT] = {-1.0, 1.0, -1.0, 1.0}; - SimplePID* x_ctrl; - SimplePID* y_ctrl; - SimplePID* r_ctrl; drivemode mode = drivemode::relative; - //rps* positioner; CANTalon* motors[4]; private: diff --git a/src/Robot.cpp b/src/Robot.cpp index da14fc7..05e1030 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -1,15 +1,14 @@ #include #include "MecanumDrive.h" #include "XMLInput.h" -#include "Autonomous.h" +#include "Autonomous/HALBot.h" #include "Robot.h" -#include "DreadbotDIO.h" #include "../lib/Logger.h" using namespace Hydra; namespace dreadbot { - class Robot: public IterativeRobot + class Robot : public IterativeRobot { DriverStation *ds; Joystick* gamepad; @@ -68,6 +67,9 @@ namespace dreadbot viewingBack = false; Cam2Enabled = false; Cam1Enabled = StartCamera(1); + viewerCooldown = 0; + + sysLog->log("Robot ready."); } void GlobalInit() @@ -75,7 +77,7 @@ namespace dreadbot compressor->Start(); drivebase->Engage(); - Input->loadXMLConfig(); //ABSOLUTELY NEEDED! IF THIS IS DELETED, THE WHOLE ROBOT STOPS WORKING! + Input->loadXMLConfig(); gamepad = Input->getController(COM_PRIMARY_DRIVER); gamepad2 = Input->getController(COM_BACKUP_DRIVER); @@ -91,18 +93,11 @@ namespace dreadbot GlobalInit(); if (AutonBot == nullptr) AutonBot = new HALBot; - AutonBot->setMode(GetAutonMode()); //Uses the 10-switch to get the auton mode. + sysLog->log("Auton mode is " + (int)GetAutonMode()); AutonBot->init(drivebase, intake, lift); drivebase->GoSlow(); - if (viewingBack) - { - StopCamera(2); - Cam1Enabled = StartCamera(1); - Cam2Enabled = false; - viewingBack = false; - } - if (AutonBot->getMode() == AUTON_MODE_STACK3 || AutonBot->getMode() == AUTON_MODE_STACK2) + if (GetAutonMode() == AUTON_MODE_STACK3 || GetAutonMode() == AUTON_MODE_STACK2) { lift->Set(1); Wait(0.2); @@ -112,20 +107,13 @@ namespace dreadbot void AutonomousPeriodic() { AutonBot->update(); - - //Vision during auton, because why not? - if (!viewingBack && Cam1Enabled) - { - IMAQdxGrab(sessionCam1, frame1, true, nullptr); - CameraServer::GetInstance()->SetImage(frame1); - } } void TeleopInit() { sysLog->log("Initializing Teleop."); GlobalInit(); - drivebase->GoSlow(); + drivebase->GoFast(); } void TeleopPeriodic() @@ -133,17 +121,12 @@ namespace dreadbot Input->updateDrivebase(); //Makes the robot drive using Config.h controls and a sensativity curve (tested) //Output controls - float intakeInput = gamepad->GetRawAxis(3); - intake->Set(((float) (intakeInput > 0.1) * -0.8) + gamepad2->GetRawAxis(3) - gamepad2->GetRawAxis(2)); - + intake->Set(((float) (gamepad->GetRawAxis(3) > 0.1f) * -0.74f) + gamepad2->GetRawAxis(3) - gamepad2->GetRawAxis(2)); - if (gamepad->GetRawButton(1)) - { + if (gamepad->GetRawButton(1) || gamepad2->GetRawButton(1)) { lift->Set(0.0f); - } - else - { - lift->Set(gamepad->GetRawAxis(2) > 0.1 ? -1.0f : 1.0f); + } else { + lift->Set(gamepad->GetRawAxis(2) > 0.1f ? -1.0f : 1.0f); } intakeArms->Set(-(float) gamepad->GetRawButton(6) + (float) gamepad2->GetRawButton(2) - (float) gamepad2->GetRawButton(3)); @@ -155,7 +138,6 @@ namespace dreadbot viewerCooldown--; if ((gamepad->GetRawButton(8) || gamepad2->GetRawButton(8)) && viewerCooldown == 0) //Start button { - SmartDashboard::PutBoolean("Switched camera", true); viewerCooldown = 10; viewingBack =! viewingBack; if (viewingBack) @@ -192,6 +174,16 @@ namespace dreadbot void TestPeriodic() { + if (viewingBack && Cam2Enabled) + { + IMAQdxGrab(sessionCam2, frame2, true, nullptr); + CameraServer::GetInstance()->SetImage(frame2); + } + if (!viewingBack && Cam1Enabled) + { + IMAQdxGrab(sessionCam1, frame1, true, nullptr); + CameraServer::GetInstance()->SetImage(frame1); + } } void DisabledInit() @@ -245,7 +237,7 @@ namespace dreadbot if (imaqError != IMAQdxErrorSuccess) { sysLog->log( - "cam0 IMAQdxCloseCamera error - " + "cam2 IMAQdxCloseCamera error - " + std::to_string((long) imaqError), Hydra::error); return false; } @@ -270,7 +262,7 @@ namespace dreadbot if (imaqError != IMAQdxErrorSuccess) { sysLog->log( - "cam0 IMAQdxConfigureGrab error - " + "cam1 IMAQdxConfigureGrab error - " + std::to_string((long) imaqError), Hydra::error); return false; } diff --git a/src/Robot.h b/src/Robot.h index 2c4f692..069bf0e 100644 --- a/src/Robot.h +++ b/src/Robot.h @@ -13,18 +13,19 @@ #define AXS_DRIVE_X 0 #define AXS_DRIVE_Y 1 #define AXS_DRIVE_R 4 - #define AXS_LIFT_DOWN 2 // LB - #define AXS_INTAKE_IN 3 // RB - #define BTN_ARMS_OUT 6 // RT - #define BTN_OPEN_FORK 5 // LT - #define BTN_STOP_LIFT 1 // X + #define AXS_LIFT_DOWN 2 //LB + #define AXS_INTAKE_IN 3 //RB + #define BTN_ARMS_OUT 6 //RT + #define BTN_OPEN_FORK 5 //LT + #define BTN_STOP_LIFT 1 //X // Backup driver controls - #define B_AXS_TOTE_OUT 3 - #define B_AXS_TOTE_IN 2 - #define B_BTN_ARMS_OUT 3 - #define B_BTN_ARMS_IN 2 - //#define B_BTN_GEDDAN 5 // Robot does a short dance to recover from an intake failure + #define B_AXS_TOTE_OUT 3 //RB + #define B_AXS_TOTE_IN 2 //LB + #define B_BTN_ARMS_OUT 3 + #define B_BTN_ARMS_IN 2 + #define B_BTN_AUTOTOTE 7 //Back + //#define B_BTN_GEDDAN 5 //Robot does a short dance to recover from an intake failure // Shared controls - #define BTN_SWITCH_CAM 8 // Camera change is start + #define BTN_SWITCH_CAM 8 //Camera change is start #endif diff --git a/src/SimplePID.cpp b/src/SimplePID.cpp deleted file mode 100644 index 75175d9..0000000 --- a/src/SimplePID.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include "SimplePID.h" -#include "Notifier.h" -#include - -namespace dreadbot { - - /** - * Allocate a PID object with the given constants for P, I, D - * @param Kp the proportional coefficient - * @param Ki the integral coefficient - * @param Kd the derivative coefficient - * @param period the loop time for doing calculations. This particularly effects calculations of the - * integral and differental terms. The default is 50ms. - */ - SimplePID::SimplePID(double Kp, double Ki, double Kd, bool continuous, double period) { - m_controlLoop = new Notifier(SimplePID::CallCalculate, this); - m_P = Kp; - m_I = Ki; - m_D = Kd; - m_maximumOutput = 1.0; - m_minimumOutput = -1.0; - m_maximumInput = 1.0; - m_minimumInput = -1.0; - m_continuous = false; - m_enabled = false; - m_setpoint = 0.0; - m_error = 0.0; - m_prevError = 0.0; - m_totalError = 0.0; - m_tolerance = 0.05; - m_result = 0.0; - m_period = period; - m_measuredValue = 0.0; - m_controlLoop->StartPeriodic(m_period); - } - - SimplePID::~SimplePID() { - delete m_controlLoop; - } - - void SimplePID::CallCalculate(void *controller) { - SimplePID *control = (SimplePID*) controller; - control->Calculate(); - } - - void SimplePID::Calculate() { - if (m_enabled) { - double input = m_measuredValue; - m_error = m_setpoint - input; - if (m_continuous) { - if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) { - if (m_error > 0) - m_error = m_error - m_maximumInput + m_minimumInput; - else - m_error = m_error + m_maximumInput - m_minimumInput; - } - } - if (((m_totalError + m_error) * m_I < m_maximumOutput) && ((m_totalError + m_error) * m_I > m_minimumOutput)) - m_totalError += m_error; - m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError); - m_prevError = m_error; - if (m_result > m_maximumOutput) - m_result = m_maximumOutput; - else if (m_result < m_minimumOutput) - m_result = m_minimumOutput; - } - } - - void SimplePID::SetPID(double p, double i, double d) { - m_P = p; - m_I = i; - m_D = d; - } - - double SimplePID::GetOutput() { - return m_result; - } - - void SimplePID::SetMV(double mv) { - m_measuredValue = mv; - } - - void SimplePID::SetInputRange(double minimumInput, double maximumInput) { - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; - SetSetpoint(m_setpoint); - } - - void SimplePID::SetOutputRange(double minimumOutput, double maximumOutput) { - m_minimumOutput = minimumOutput; - m_maximumOutput = maximumOutput; - } - - void SimplePID::SetSetpoint(double setpoint) { - if (m_maximumInput > m_minimumInput) { - if (setpoint > m_maximumInput) - m_setpoint = m_maximumInput; - else if (setpoint < m_minimumInput) - m_setpoint = m_minimumInput; - else - m_setpoint = setpoint; - } else - m_setpoint = setpoint; - } - - void SimplePID::SetTolerance(double tolerance) { - m_tolerance = tolerance; - } - - bool SimplePID::OnTarget() { - return (fabs(m_error) < m_tolerance * (m_maximumInput - m_minimumInput));; - } - - void SimplePID::Enable() { - m_enabled = true; - } - - void SimplePID::Disable() { - m_enabled = false; - } - - void SimplePID::Reset() { - Disable(); - m_prevError = 0; - m_totalError = 0; - m_result = 0; - } -} diff --git a/src/SimplePID.h b/src/SimplePID.h deleted file mode 100644 index ad57cc4..0000000 --- a/src/SimplePID.h +++ /dev/null @@ -1,51 +0,0 @@ -// Bare-bones PID controller object - -#pragma once - -#include "Base.h" - -class Notifier; - -namespace dreadbot { - class SimplePID { - private: - double m_P; // factor for "proportional" control - double m_I; // factor for "integral" control - double m_D; // factor for "derivative" control - double m_maximumOutput; // |maximum output| - double m_minimumOutput; // |minimum output| - double m_maximumInput; // maximum input - limit setpoint to this - double m_minimumInput; // minimum input - limit setpoint to this - bool m_continuous; // do the endpoints wrap around? eg. Absolute encoder - bool m_enabled; //is the pid controller enabled - double m_prevError; // the prior sensor input (used to compute velocity) - double m_totalError; //the sum of the errors for use in the integral calc - double m_tolerance; //the percetage error that is considered on target - double m_setpoint; - double m_error; - double m_result; - double m_period; - double m_measuredValue; //the last value recorded from the measurement device. - Notifier *m_controlLoop; - static void CallCalculate(void *controller); - void Calculate(); - //DISALLOW_COPY_AND_ASSIGN(SimplePID); - - public: - SimplePID(double p, double i, double d, bool continuous, double period = 0.05); - ~SimplePID(); - double GetOutput(); - void SetInputRange(double minimumInput, double maximumInput); - void SetOutputRange(double mimimumOutput, double maximumOutput); - void SetPID(double p, double i, double d); - void SetMV(double mv); - void SetSetpoint(double setpoint); - double GetSetpoint(); - double GetError(); - void SetTolerance(double percent); - bool OnTarget(); - void Enable(); - void Disable(); - void Reset(); - }; -} diff --git a/src/XMLInput.cpp b/src/XMLInput.cpp index 04f64d1..79126ac 100644 --- a/src/XMLInput.cpp +++ b/src/XMLInput.cpp @@ -209,16 +209,14 @@ namespace dreadbot } MotorGrouping* XMLInput::getMGroup(string name) { - for (auto iter = mGroups.begin(); iter != mGroups.end(); iter++) - if (iter->name == name) - return &(*iter); + if (mGroups.count(name) > 0) //Checks how many elements have this key. If none do, return nullptr. + return &mGroups[name]; //This syntax is awesome; note that mGroups absolutely is NOT an array! return nullptr; } PneumaticGrouping* XMLInput::getPGroup(string name) { - for (auto iter = pGroups.begin(); iter != pGroups.end(); iter++) - if (iter->name == name) - return &(*iter); + if (pGroups.count(name) > 0) //Checks out many elements have this key. If none do, return nullptr. + return &pGroups[name]; return nullptr; } void XMLInput::loadXMLConfig() @@ -301,9 +299,8 @@ namespace dreadbot MotorGrouping newMGroup; newMGroup.name = motorgroup.attribute("name").as_string(); //Check for duplicate motor groups, and output if there are - for (auto iter = mGroups.begin(); iter != mGroups.end(); iter++) - if (iter->name == newMGroup.name) - SmartDashboard::PutBoolean("Duplicate Motor Groups", true); + if (mGroups.count(newMGroup.name) > 0) + SmartDashboard::PutBoolean("Duplicate Motor Groups", true); newMGroup.deadzone = fabs(motorgroup.attribute("deadzone").as_float()); for (auto motor = motorgroup.child("motor"); motor; motor = motor.next_sibling()) @@ -318,7 +315,7 @@ namespace dreadbot newMotor.invert = motor.attribute("invert").as_bool(); newMGroup.motors.push_back(newMotor); } - mGroups.push_back(newMGroup); + mGroups[newMGroup.name] = newMGroup; //Synatx is beautiful; mGroups is NOT an array! } //Load all pneumatic groups @@ -328,9 +325,8 @@ namespace dreadbot PneumaticGrouping newPGroup; newPGroup.name = pneumgroup.attribute("name").as_string(); //Check for duplicate motor groups, and output if there are - for (auto iter = pGroups.begin(); iter != pGroups.end(); iter++) - if (iter->name == newPGroup.name) - SmartDashboard::PutBoolean("Duplicate Pneumatic Groups", true); + if (pGroups.count(newPGroup.name) > 0) + SmartDashboard::PutBoolean("Duplicate Pneumatic Groups", true); newPGroup.deadzone = fabs(pneumgroup.attribute("deadzone").as_float()); @@ -356,7 +352,7 @@ namespace dreadbot newPneum.sPneumatic = getSPneum(pneumatic.attribute("ID").as_int()); newPGroup.pneumatics.push_back(newPneum); } - pGroups.push_back(newPGroup); + pGroups[newPGroup.name] = newPGroup; } } } diff --git a/src/XMLInput.h b/src/XMLInput.h index 643229c..83f375f 100644 --- a/src/XMLInput.h +++ b/src/XMLInput.h @@ -3,9 +3,11 @@ #include "../lib/pugixml.hpp" #include "MecanumDrive.h" #include +#include #include using std::string; using std::vector; +using std::unordered_map; /* * Uses an XML config file to create custom control settings @@ -89,8 +91,8 @@ namespace dreadbot private: XMLInput(); - vector pGroups; - vector mGroups; + unordered_map pGroups; + unordered_map mGroups; MecanumDrive* drivebase; static XMLInput* singlePtr; diff --git a/src/mathutil.h b/src/mathutil.h deleted file mode 100644 index bf7591d..0000000 --- a/src/mathutil.h +++ /dev/null @@ -1,128 +0,0 @@ -// Parker Stebbins/1.13.2015 - -#pragma once - -#include - -namespace dreadbot { - template - class Vector2 { - public: - T x, y; - - // Default constructor - Vector2(T _x = 0, T _y = 0) { - x = _x; - y = _y; - } - - // Addition - template // Vector + Number -> Vector' - friend Vector2 operator + (Vector2 v, T0 n) { - return Vector2(v.x + n, v.y + n); - } - template // Number + Vector -> Vector' - friend Vector2 operator + (T0 n, Vector2 v) { - return Vector2(n + v.x, n + v.y); - } - template // Vector + Vector -> Vector' - friend Vector2 operator + (Vector2 v0, Vector2 v1) { - return Vector2(v0.x + v1.x, v0.y + v1.y); - } - - // Subtraction - template // Vector - Number -> Vector' - friend Vector2 operator - (Vector2 v, T0 n) { - return Vector2(v.x - n, v.y - n); - } - template // Number - Vector -> Vector' - friend Vector2 operator - (T0 n, Vector2 v) { - return Vector2(n - v.x, n - v.y); - } - template // Vector - Vector -> Vector' - friend Vector2 operator - (Vector2 v0, Vector2 v1) { - return Vector2(v0.x - v1.x, v0.y - v1.y); - } - - // Multiplication - template // Vector * Number -> Vector' - friend Vector2 operator * (Vector2 v, T0 n) { - return Vector2(v.x * n, v.y * n); - } - template // Number * Vector -> Vector' - friend Vector2 operator * (T0 n, Vector2 v) { - return Vector2(n * v.x, n * v.y); - } - template // Vector * Vector -> Vector' - friend Vector2 operator * (Vector2 v0, Vector2 v1) { - return Vector2(v0.x * v1.x, v0.y * v1.y); - } - - // Division - template // Vector / Number -> Vector' - friend Vector2 operator / (Vector2 v, T0 n) { - return Vector2(v.x / n, v.y / n); - } - template // Number / Vector -> Vector' - friend Vector2 operator / (T0 n, Vector2 v) { - return Vector2(n / v.x, n / v.y); - } - template // Vector / Vector -> Vector' - friend Vector2 operator / (Vector2 v0, Vector2 v1) { - return Vector2(v0.x / v1.x, v0.y / v1.y); - } - - /** - * @description Calculate the length of the vector - */ - double getMagnitude() { - return std::sqrt(x*x + y*y); - } - - /** - * @description Set the length of the vector - */ - void setMagnitude(double mag) { - copy((*this)*(mag/getMagnitude())); - } - - /** - * @description Convert to a unit vector - */ - void normalize() { - double m = getMagnitude(); - x /= m; - y /= m; - } - - /** - * @description Rotate the vector around (0, 0). - * @param theta Rotation angle - */ - void rotate(double theta) { - theta *= 0.01745329251; // Convert to radians - double cosA = std::cos(theta); - double sinA = std::sin(theta); - double xOut = x*cosA - y*sinA; - double yOut = x*sinA + y*cosA; - x = xOut; - y = yOut; - } - /** - * @description Set each field in one statement. - * @param x The x-value - * @param y The y-value - */ - void set(T _x, T _y) { - x = _x; - y = _y; - } - /** - * @description Copies the elements of C into the vector - * @param c The vector to copy - */ - void set(Vector2 c) { - set(c.x, c.y); - } - }; // end vector2 -} // end dreadbot namespace diff --git a/src/rps.h b/src/rps.h deleted file mode 100644 index 233e257..0000000 --- a/src/rps.h +++ /dev/null @@ -1,86 +0,0 @@ -#include "mathutil.h" -#include -#include "WPILib.h" - -namespace dreadbot { - // Stores position, velocity, rotational position, and rotational velocity - struct inertial_frame { - inertial_frame(Vector2 _position, Vector2 _velocity, double _rp, double _rv) { - position = _position; - velocity = _velocity; - rp = _rp; - rv = _rv; - } - Vector2 position; // position - Vector2 velocity; // velocity - double rp; // rotational position - double rv; // rotational velocity - }; - - /** - * Robot positioning system - * Manages the built-in accelerometer and gyroscope, as well as integrates the - * accelerometer values to yield position and velocity. - * Noise accumulation shouldn't be much of an issue in 15 seconds of auton, - * but we can still apply a Madgewick filter if it becomes problematic. - */ - class rps { - public: - rps(uint16_t gyroCh) { - accelerometer = new BuiltInAccelerometer(); - gyro = new Gyro(gyroCh); - gyro->InitGyro(); - current_frame = new inertial_frame(Vector2(0.0, 0.0), Vector2(0.0, 0.0), 0.0, 0.0); - integrationLoop = new Notifier(CallTracker, this); - integrationLoop->StartPeriodic(0.01); // Built-in accelerometer outputs 800 signals per second - // Integration period is less than that in order to reduce noise and to avoid hogging CPU - loopTimer = new Timer(); - tracking_active = false; - } - - void Start() { // Start tracker - tracking_active = true; - loopTimer->Start(); - } - - void Stop() { - tracking_active = false; - loopTimer->Stop(); - } - - void Reset() { - gyro->Reset(); - current_frame = new inertial_frame(Vector2(0.0, 0.0), Vector2(0.0, 0.0), 0.0, 0.0); - } - - inertial_frame GetTrackingData() { - current_frame->rp = gyro->GetAngle(); - current_frame->rv = gyro->GetRate(); - return *current_frame; - } - - private: - void RunCalculations() { - if (tracking_active) { - double dt = loopTimer->Get(); - current_frame->velocity = current_frame->velocity + dt*Vector2(accelerometer->GetX(), accelerometer->GetY()); - current_frame->position = current_frame->position + current_frame->velocity*dt; - loopTimer->Stop(); - loopTimer->Reset(); - loopTimer->Start(); - } - } - - static void CallTracker(void *controller) { - rps *control = (rps*) controller; - control->RunCalculations(); - } - - inertial_frame* current_frame; - bool tracking_active; - Timer* loopTimer; - Notifier* integrationLoop; - BuiltInAccelerometer* accelerometer; - Gyro* gyro; - }; -}