From dad8b025b4a44ffcc7ded4927aa9b74b7b4a60c7 Mon Sep 17 00:00:00 2001 From: Sourec Date: Wed, 8 Apr 2015 11:13:21 -0400 Subject: [PATCH 01/32] Added documentation for Logger files. I forgot to do it in the previous commit. --- lib/Logger.h | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/lib/Logger.h b/lib/Logger.h index 6899cf1..29e8d2d 100644 --- a/lib/Logger.h +++ b/lib/Logger.h @@ -13,30 +13,34 @@ 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}; + enum logFlag {error, hydsys, info, resource}; //All possible flags that could be used. Default is hydsys. class Log { public: - Log(string newName, string newFilename); + 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(); + void flushBuffer(); //!< Forces the contents of the buffer to output to file. private: vector logBuffer; string filename; string name; friend 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. + 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: From 1f648c1c620bd0ef5acaaa91e59230bff9b520a2 Mon Sep 17 00:00:00 2001 From: Steve King Date: Fri, 10 Apr 2015 10:13:22 -0400 Subject: [PATCH 02/32] Added a GoSpeed() function to give me something tweakable without affecting everywhere GoFast/GoSlow are used. Attempted added code to close intake arms when grabbing totes in auton but that didn't seem to do anything. (Including not doing anything bad, which is why I didn't bother to rip it out again.) --- src/Autonomous.cpp | 28 +++++++++++++++++++++++----- src/Autonomous.h | 7 +++++-- src/MecanumDrive.cpp | 6 ++++++ src/MecanumDrive.h | 1 + 4 files changed, 35 insertions(+), 7 deletions(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 411a320..4413380 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -10,6 +10,7 @@ namespace dreadbot drivebase = nullptr; intake = nullptr; timerActive = false; + armTimer = nullptr; } void GettingTote::setHardware(MecanumDrive* newDrivebase, MotorGrouping* newIntake) { @@ -45,11 +46,21 @@ namespace dreadbot 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 + if (HALBot::getToteCount() != 0) { //Open the intake arms for grabbing totes after the first tote is collected XMLInput::getInstance()->getPGroup("intakeArms")->Set(-1); + armTimer = new Timer(); + armTimer->Start(); + } drivebase->Drive_v(0, -0.75, 0); intake->Set(-0.6); + if ((armTimer != nullptr) && (armTimer->Get() >= ARM_TIME)) { + armTimer->Stop(); + delete armTimer; + armTimer = nullptr; + XMLInput::getInstance()->getPGroup("intakeArms")->Set(0); + } + //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) { @@ -159,6 +170,8 @@ namespace dreadbot void Stopped::enter() { sysLog->log("State: Stopped"); + if (drivebase != nullptr) + drivebase->Drive_v(0, 0, 0); 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 } @@ -282,10 +295,10 @@ namespace dreadbot } int RotateDrive::update() { - if (driveTimer.Get() >= (ROTATE_TIME - 1.2f)) + if (driveTimer.Get() >= (ROTATE_TIME - 0.5f)) { //Rotated far enough; break timerActive = false; - drivebase->GoSlow(); + drivebase->GoSpeed(0.6); drivebase->Drive_v(0, 1, 0); Wait(ROTATE_DRIVE_STRAIGHT); if (HALBot::getToteCount() == 3) @@ -366,6 +379,7 @@ namespace dreadbot pushContainer->pusher2 = XMLInput::getInstance()->getPWMMotor(1); pushContainer->pushConstant = 1; stopped->lift = lift; //Don't know if I like these... + stopped->drivebase = drivebase; forkGrab->lift = lift; forkGrab->drivebase = drivebase; backAway->lift = lift; @@ -403,6 +417,7 @@ namespace dreadbot transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, rotate2}; transitionTable[i++] = {rotate2, HALBot::timerExpired, nullptr, stopped}; transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; transitionTable[i++] = END_STATE_TABLE; defState = gettingTote; } @@ -413,6 +428,7 @@ namespace dreadbot rotate->rotateConstant = -1; transitionTable[i++] = {rotate, HALBot::timerExpired, nullptr, driveToZone}; transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; transitionTable[i++] = END_STATE_TABLE; } if (mode == AUTON_MODE_BOTH) @@ -433,10 +449,11 @@ namespace dreadbot transitionTable[i++] = {pushContainer, HALBot::timerExpired, nullptr, gettingTote}; transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; - transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; + transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, rotateDrive}; + transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; defState = pushContainer; } if (mode == AUTON_MODE_STACK3) @@ -455,6 +472,7 @@ namespace dreadbot transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, backAway}; transitionTable[i++] = {backAway, HALBot::timerExpired, nullptr, stopped}; transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; defState = pushContainer; } diff --git a/src/Autonomous.h b/src/Autonomous.h index 7877727..28bb297 100644 --- a/src/Autonomous.h +++ b/src/Autonomous.h @@ -18,10 +18,11 @@ using namespace Hydra; #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 ROTATE_TIME 2.0f //Also, timing is modified in RotateDrive::update - 1.0 s is subtracted +#define ROTATE_DRIVE_STRAIGHT 0.7f //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 +#define ARM_TIME 1.0f namespace dreadbot { @@ -39,6 +40,7 @@ namespace dreadbot bool timerActive; Timer getTimer; Timer eStopTimer; + Timer *armTimer; }; class DriveToZone : public FSMState { @@ -81,6 +83,7 @@ namespace dreadbot virtual void enter(); virtual int update(); PneumaticGrouping* lift; + MecanumDrive* drivebase; }; class PushContainer : public DriveToZone { diff --git a/src/MecanumDrive.cpp b/src/MecanumDrive.cpp index 168da42..ccd74ed 100644 --- a/src/MecanumDrive.cpp +++ b/src/MecanumDrive.cpp @@ -60,6 +60,12 @@ void MecanumDrive::GoSlow() { } } +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 to position void MecanumDrive::Drive_p(double x, double y, double rotation) { if (mode == drivemode::absolute) { diff --git a/src/MecanumDrive.h b/src/MecanumDrive.h index 0247340..02f9876 100644 --- a/src/MecanumDrive.h +++ b/src/MecanumDrive.h @@ -34,6 +34,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); From 5190b87c074da3c975800fb36a7eed7190cf1d9b Mon Sep 17 00:00:00 2001 From: Steve King Date: Fri, 10 Apr 2015 11:35:32 -0400 Subject: [PATCH 03/32] Adding a bit of strafing to avoid knocking over the container in 2-tote auton. --- src/Autonomous.cpp | 30 +++++++++++++++++++++++++++++- src/Autonomous.h | 8 ++++++++ 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 4413380..51d82d4 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -310,6 +310,31 @@ namespace dreadbot return HALBot::no_update; } + StrafeLeft::StrafeLeft() + { + drivebase = nullptr; + 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() + { + int rc = HALBot::no_update; + if (driveTimer.Get() >= 1.0f) { + driveTimer.Stop(); + driveTimer.Reset(); + timerActive = false; + rc = HALBot::timerExpired; + } + return rc; + } + int HALBot::toteCount = 0; AutonMode HALBot::mode = AUTON_MODE_STOP; @@ -350,6 +375,7 @@ namespace dreadbot backAway = new BackAway; fsm = new FiniteStateMachine; rotateDrive = new RotateDrive; + strafeLeft = new StrafeLeft; mode = AUTON_MODE_DRIVE; //Just drive straight forward. Assumes a spherical cow. } HALBot::~HALBot() @@ -371,6 +397,7 @@ namespace dreadbot sysLog = Logger::getInstance()->getLog("sysLog"); gettingTote->setHardware(drivebase, intake); driveToZone->setHardware(drivebase); + strafeLeft->setHardware(drivebase); rotate->setHardware(drivebase); rotateDrive->setHardware(drivebase); rotate2->setHardware(drivebase); @@ -450,8 +477,9 @@ namespace dreadbot transitionTable[i++] = {pushContainer, HALBot::timerExpired, nullptr, gettingTote}; transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; - transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, rotateDrive}; + transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, strafeLeft}; transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; + transitionTable[i++] = {strafeLeft, HALBot::timerExpired, nullptr, rotateDrive}; transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, stopped}; transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; defState = pushContainer; diff --git a/src/Autonomous.h b/src/Autonomous.h index 28bb297..4054d41 100644 --- a/src/Autonomous.h +++ b/src/Autonomous.h @@ -110,6 +110,13 @@ namespace dreadbot int update(); void enter(); }; + class StrafeLeft : public DriveToZone + { + public: + StrafeLeft(); + int update(); + void enter(); + }; //Needs to appear in https://en.wikipedia.org/wiki/Kludge#Computer_science class HALBot @@ -143,5 +150,6 @@ namespace dreadbot PushContainer* pushContainer; BackAway* backAway; RotateDrive* rotateDrive; + StrafeLeft* strafeLeft; }; } From 9da84647aed1583bf6a41aa3b7b4760bf5ee4478 Mon Sep 17 00:00:00 2001 From: Sourec Date: Fri, 10 Apr 2015 16:13:23 -0400 Subject: [PATCH 04/32] Fixed arms not closing when they should. Since the function wasn't checking whether the timer had been started or not, that if statement was always being hit, resulting in the open arms function call always being hit, This also had the amusing side effect of constantly allocating memory for a new timer object. --- src/Autonomous.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 51d82d4..320fc7f 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -46,7 +46,7 @@ namespace dreadbot 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 + if (HALBot::getToteCount() != 0 && armTimer == nullptr) { //Open the intake arms for grabbing totes after the first tote is collected XMLInput::getInstance()->getPGroup("intakeArms")->Set(-1); armTimer = new Timer(); armTimer->Start(); @@ -56,6 +56,7 @@ namespace dreadbot if ((armTimer != nullptr) && (armTimer->Get() >= ARM_TIME)) { armTimer->Stop(); + armTimer->Reset(); delete armTimer; armTimer = nullptr; XMLInput::getInstance()->getPGroup("intakeArms")->Set(0); From 96852d92b8cd31a347b279da3d35a105c5eb94be Mon Sep 17 00:00:00 2001 From: Sourec Date: Fri, 10 Apr 2015 18:10:23 -0400 Subject: [PATCH 05/32] Switched XMLInput over from using vectors to maps. Might not work, hence the new branch Maps are a neat thing in the standard template library that accept a key and a value. They can handle searching for things automatically (reportedly by using a binary search or something). Not only is this incredibly fast compared to the "iterate EVERYTHING" method used before, but it's a lot easier to implement - no more annoying for loops everywhere for the simplest things. --- src/XMLInput.cpp | 24 ++++++++++-------------- src/XMLInput.h | 8 ++++---- 2 files changed, 14 insertions(+), 18 deletions(-) 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..5601d1a 100644 --- a/src/XMLInput.h +++ b/src/XMLInput.h @@ -3,9 +3,9 @@ #include "../lib/pugixml.hpp" #include "MecanumDrive.h" #include -#include +#include using std::string; -using std::vector; +using std::map; /* * Uses an XML config file to create custom control settings @@ -89,8 +89,8 @@ namespace dreadbot private: XMLInput(); - vector pGroups; - vector mGroups; + map pGroups; + map mGroups; MecanumDrive* drivebase; static XMLInput* singlePtr; From d8fc297bb8268e05fbc13c3716888621338f16b1 Mon Sep 17 00:00:00 2001 From: Sourec Date: Fri, 10 Apr 2015 18:20:10 -0400 Subject: [PATCH 06/32] Turns out that #include and using std::vector; WAS important! Vectors are still used in far more appropriate places, so I had to re-add those back in. --- src/XMLInput.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/XMLInput.h b/src/XMLInput.h index 5601d1a..e6fdc10 100644 --- a/src/XMLInput.h +++ b/src/XMLInput.h @@ -4,7 +4,9 @@ #include "MecanumDrive.h" #include #include +#include using std::string; +using std::vector; using std::map; /* From 718f5fd8adfc6831c83a18bc9a2a6fc54de5d939 Mon Sep 17 00:00:00 2001 From: Steve King Date: Mon, 13 Apr 2015 12:19:04 -0400 Subject: [PATCH 07/32] Tweak strafe time in 2TA, make sure to stop afterward --- src/Autonomous.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 320fc7f..2c347fb 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -327,16 +327,16 @@ namespace dreadbot int StrafeLeft::update() { int rc = HALBot::no_update; - if (driveTimer.Get() >= 1.0f) { + if (driveTimer.Get() >= 0.5f) { driveTimer.Stop(); driveTimer.Reset(); timerActive = false; + drivebase->Drive_v(0, 0, 0); //stop rc = HALBot::timerExpired; } return rc; } - int HALBot::toteCount = 0; AutonMode HALBot::mode = AUTON_MODE_STOP; int HALBot::getToteCount() From 0fbd8dc6a81ead4d7cd87eb955a143fea16f9836 Mon Sep 17 00:00:00 2001 From: Steve King Date: Mon, 13 Apr 2015 12:21:30 -0400 Subject: [PATCH 08/32] Removed intake arm change. Even though Chris fixed it it was never needed. --- src/Autonomous.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 2c347fb..48ffcf4 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -10,7 +10,6 @@ namespace dreadbot drivebase = nullptr; intake = nullptr; timerActive = false; - armTimer = nullptr; } void GettingTote::setHardware(MecanumDrive* newDrivebase, MotorGrouping* newIntake) { @@ -46,22 +45,12 @@ namespace dreadbot intake->Set(-0.5); return HALBot::no_update; } - if (HALBot::getToteCount() != 0 && armTimer == nullptr) { //Open the intake arms for grabbing totes after the first tote is collected + if (HALBot::getToteCount() != 0) { //Open the intake arms for grabbing totes after the first tote is collected XMLInput::getInstance()->getPGroup("intakeArms")->Set(-1); - armTimer = new Timer(); - armTimer->Start(); } drivebase->Drive_v(0, -0.75, 0); intake->Set(-0.6); - if ((armTimer != nullptr) && (armTimer->Get() >= ARM_TIME)) { - armTimer->Stop(); - armTimer->Reset(); - delete armTimer; - armTimer = nullptr; - XMLInput::getInstance()->getPGroup("intakeArms")->Set(0); - } - //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) { From 7f8fd15824567f7d7e2245d6a4c2b2293a68e3a6 Mon Sep 17 00:00:00 2001 From: Sourec Date: Tue, 14 Apr 2015 09:57:44 -0400 Subject: [PATCH 09/32] Switched from map to unordered_map, which uses a hash table (average O(1)) instead of a binary search --- src/XMLInput.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/XMLInput.h b/src/XMLInput.h index e6fdc10..83f375f 100644 --- a/src/XMLInput.h +++ b/src/XMLInput.h @@ -3,11 +3,11 @@ #include "../lib/pugixml.hpp" #include "MecanumDrive.h" #include -#include +#include #include using std::string; using std::vector; -using std::map; +using std::unordered_map; /* * Uses an XML config file to create custom control settings @@ -91,8 +91,8 @@ namespace dreadbot private: XMLInput(); - map pGroups; - map mGroups; + unordered_map pGroups; + unordered_map mGroups; MecanumDrive* drivebase; static XMLInput* singlePtr; From 8df17d7d12842bb88ffb87bf75e89c8d4d323d9c Mon Sep 17 00:00:00 2001 From: Steve King Date: Mon, 13 Apr 2015 12:19:04 -0400 Subject: [PATCH 10/32] Tweak strafe time in 2TA, make sure to stop afterward --- src/Autonomous.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 320fc7f..2c347fb 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -327,16 +327,16 @@ namespace dreadbot int StrafeLeft::update() { int rc = HALBot::no_update; - if (driveTimer.Get() >= 1.0f) { + if (driveTimer.Get() >= 0.5f) { driveTimer.Stop(); driveTimer.Reset(); timerActive = false; + drivebase->Drive_v(0, 0, 0); //stop rc = HALBot::timerExpired; } return rc; } - int HALBot::toteCount = 0; AutonMode HALBot::mode = AUTON_MODE_STOP; int HALBot::getToteCount() From 88aa6857e433bcbf8caf8f39b29b692e53d78050 Mon Sep 17 00:00:00 2001 From: Steve King Date: Mon, 13 Apr 2015 12:21:30 -0400 Subject: [PATCH 11/32] Removed intake arm change. Even though Chris fixed it it was never needed. --- src/Autonomous.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 2c347fb..48ffcf4 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -10,7 +10,6 @@ namespace dreadbot drivebase = nullptr; intake = nullptr; timerActive = false; - armTimer = nullptr; } void GettingTote::setHardware(MecanumDrive* newDrivebase, MotorGrouping* newIntake) { @@ -46,22 +45,12 @@ namespace dreadbot intake->Set(-0.5); return HALBot::no_update; } - if (HALBot::getToteCount() != 0 && armTimer == nullptr) { //Open the intake arms for grabbing totes after the first tote is collected + if (HALBot::getToteCount() != 0) { //Open the intake arms for grabbing totes after the first tote is collected XMLInput::getInstance()->getPGroup("intakeArms")->Set(-1); - armTimer = new Timer(); - armTimer->Start(); } drivebase->Drive_v(0, -0.75, 0); intake->Set(-0.6); - if ((armTimer != nullptr) && (armTimer->Get() >= ARM_TIME)) { - armTimer->Stop(); - armTimer->Reset(); - delete armTimer; - armTimer = nullptr; - XMLInput::getInstance()->getPGroup("intakeArms")->Set(0); - } - //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) { From 32cf8b70ebac50f53be01b6775ed3a8fe85719b3 Mon Sep 17 00:00:00 2001 From: Sourec Date: Tue, 14 Apr 2015 18:26:33 -0400 Subject: [PATCH 12/32] Added some kind of incredibly sketchy mod to ForkGrab to shave off time by driving and rotating while grabbing the tote --- src/Autonomous.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index 48ffcf4..0a77893 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -122,9 +122,16 @@ namespace dreadbot void ForkGrab::enter() { sysLog->log("State: ForkGrab"); + grabTimer.Reset(); + grabTimer.Start(); } int ForkGrab::update() { + if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.35) + { + drivebase->GoFast(); + drivebase->Drive_v(0, 1.0, 0.5); + } if (isLiftDown()) { HALBot::incrTote(); //Once the lift is down, it is assumed that the tote is actually collected, i.e. in the fork. From 73a098a377c8c8ff0a912a87a6f3ba8140ea74a0 Mon Sep 17 00:00:00 2001 From: Parker Stebbins Date: Wed, 15 Apr 2015 12:53:08 -0400 Subject: [PATCH 13/32] Co-driver has lift stop controls --- src/Robot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index 37c3ed0..13ba873 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -137,13 +137,13 @@ namespace dreadbot intake->Set(((float) (intakeInput > 0.1) * -0.8) + 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); + lift->Set(gamepad->GetRawAxis(2) > 0.06 ? -1.0f : 1.0f); } intakeArms->Set(-(float) gamepad->GetRawButton(6) + (float) gamepad2->GetRawButton(2) - (float) gamepad2->GetRawButton(3)); From 1e5c9793de7d40fca69a5b20b15b60ebc9f8c531 Mon Sep 17 00:00:00 2001 From: Parker Stebbins Date: Wed, 15 Apr 2015 13:15:44 -0400 Subject: [PATCH 14/32] Misc. Several minor fixes and changes: * Removed an unnecessary call to SD::PutBoolean(). * Disabled vision during autonomous for performance reasons. * Calibrated transit speed of primary driver intake. (The tote was being thrown 6" too far.) Should be tested by drivers. * Fixed camera assertions identifying the wrong camera. --- src/Robot.cpp | 35 +++++++++++++++-------------------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index 13ba873..fda0b62 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -68,6 +68,8 @@ namespace dreadbot viewingBack = false; Cam2Enabled = false; Cam1Enabled = StartCamera(1); + + 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); @@ -105,20 +107,19 @@ namespace dreadbot if (AutonBot->getMode() == AUTON_MODE_STACK3 || AutonBot->getMode() == AUTON_MODE_STACK2) { lift->Set(1); - Wait(0.2); + Wait(0.2); // May be able to lower this. } } void AutonomousPeriodic() { AutonBot->update(); - - //Vision during auton, because why not? - if (!viewingBack && Cam1Enabled) + + /* if (!viewingBack && Cam1Enabled) { IMAQdxGrab(sessionCam1, frame1, true, nullptr); CameraServer::GetInstance()->SetImage(frame1); - } + } */ } void TeleopInit() @@ -132,18 +133,13 @@ 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)); + // Control mappings + intake->Set(((float) (gamepad->GetRawAxis(3) > 0.1f) * -0.65f) + gamepad2->GetRawAxis(3) - gamepad2->GetRawAxis(2)); - - if (gamepad->GetRawButton(1) || gamepad2->GetRawButton(1)) - { + if (gamepad->GetRawButton(1) || gamepad2->GetRawButton(1)) { lift->Set(0.0f); - } - else - { - lift->Set(gamepad->GetRawAxis(2) > 0.06 ? -1.0f : 1.0f); + } else { + lift->Set(gamepad->GetRawAxis(2) > 0.06f ? -1.0f : 1.0f); } intakeArms->Set(-(float) gamepad->GetRawButton(6) + (float) gamepad2->GetRawButton(2) - (float) gamepad2->GetRawButton(3)); @@ -155,7 +151,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) @@ -187,7 +182,7 @@ namespace dreadbot void TestInit() { sysLog->log("Initializing Test mode."); - GlobalInit(); + compressor->Start(); } void TestPeriodic() @@ -245,7 +240,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 +265,7 @@ namespace dreadbot if (imaqError != IMAQdxErrorSuccess) { sysLog->log( - "cam0 IMAQdxConfigureGrab error - " + "cam1 IMAQdxConfigureGrab error - " + std::to_string((long) imaqError), Hydra::error); return false; } From 32dbeaa9a7b78e486536ceaa963d25afbc87f561 Mon Sep 17 00:00:00 2001 From: Sourec Date: Thu, 16 Apr 2015 10:43:17 -0400 Subject: [PATCH 15/32] Added a tab after the timestamp for all logs - should make log files much more clean now. --- lib/Logger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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()); From 3f35c7e68038deb6714755dd5c6122ede1e17ee9 Mon Sep 17 00:00:00 2001 From: Sourec Date: Wed, 22 Apr 2015 20:51:37 -0400 Subject: [PATCH 16/32] Implemented autotote routine - AUTOTOTE BUTTON NEEDS SETTING! --- src/Robot.cpp | 40 ++++++++++++++++++++++++++++++++++++++++ src/Robot.h | 9 +++++---- 2 files changed, 45 insertions(+), 4 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index fda0b62..5925713 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -39,6 +39,7 @@ namespace dreadbot bool Cam2Enabled; bool viewingBack; int viewerCooldown; + int autoToteCooldown; public: void RobotInit() @@ -68,6 +69,9 @@ namespace dreadbot viewingBack = false; Cam2Enabled = false; Cam1Enabled = StartCamera(1); + + viewerCooldown = 0; + autoToteCooldown = 0; sysLog->log("Robot ready."); } @@ -146,6 +150,42 @@ namespace dreadbot liftArms->Set(-(float) gamepad->GetRawButton(5)); + //Autotote system - sketchy but beautiful if it works. Could shave off a lot of time. + if (gamepad2->GetRawButton(B_BTN_AUTOTOTE) && autoToteCooldown == 0) //Warning - B_BTN_AUTOTOTE is set to -1! change it! + { + autoToteCooldown = 20; + + //This cycle is not iterative, because if it wasn't I'd have to make an auton-like routine for this which is nasty. + Timer safetyTimer; + safetyTimer.start(); + drivebase->drive(0, 0, 0); + while (safetyTimer.get() < 2.f && !isToteInTransit()) + intake->set(-0.6); //Same as auton + safetyTimer.stop(); + if (safetyTimer.get() < 2.f && isToteInTransit()) + { + //The robot has contacted the transit wheels; keep intaking + while (isToteInTransit()) + intake->set(-0.6); + //Tote probably ejected by now. + intake->set(0); + lift->set(-1); //Lower lift for grabbing + while (!isLiftDown()) + lift->set(-1.f); + + //Do the little jerk-to-align trick copied *ahem* plagiarized *ahem* straight out of auton, so this works for sure + drivebase->GoFast(); + drivebase->Drive_v(0, 1, 0); + Wait(STACK_CORRECTION_TIME); + drivebase->Drive_v(0, 0, 0); + drivebase->GoSlow(); + + lift->set(1.f); //Raise the lift again - lift cycle complete. + } + } + if (autoToteCooldown > 0) + autoToteCooldown--; + //Vision switch control if (viewerCooldown > 0) viewerCooldown--; diff --git a/src/Robot.h b/src/Robot.h index 2c4f692..c17af5b 100644 --- a/src/Robot.h +++ b/src/Robot.h @@ -19,10 +19,11 @@ #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_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 -1 //Temporary - Parker, find a control for this! //#define B_BTN_GEDDAN 5 // Robot does a short dance to recover from an intake failure // Shared controls From 04d3c54fa87a60a410a7924317ee07abca638934 Mon Sep 17 00:00:00 2001 From: Sourec Date: Thu, 23 Apr 2015 08:57:54 -0400 Subject: [PATCH 17/32] Fixed autotote control, now actually compiles --- src/Robot.cpp | 24 ++++++++++++------------ src/Robot.h | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index 5925713..cbaaebd 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -151,27 +151,27 @@ namespace dreadbot liftArms->Set(-(float) gamepad->GetRawButton(5)); //Autotote system - sketchy but beautiful if it works. Could shave off a lot of time. - if (gamepad2->GetRawButton(B_BTN_AUTOTOTE) && autoToteCooldown == 0) //Warning - B_BTN_AUTOTOTE is set to -1! change it! + if (gamepad2->GetRawButton(B_BTN_AUTOTOTE) && autoToteCooldown == 0) { autoToteCooldown = 20; //This cycle is not iterative, because if it wasn't I'd have to make an auton-like routine for this which is nasty. Timer safetyTimer; - safetyTimer.start(); - drivebase->drive(0, 0, 0); - while (safetyTimer.get() < 2.f && !isToteInTransit()) - intake->set(-0.6); //Same as auton - safetyTimer.stop(); - if (safetyTimer.get() < 2.f && isToteInTransit()) + safetyTimer.Start(); + drivebase->Drive_v(0, 0, 0); + while (safetyTimer.Get() < 2.f && !isToteInTransit()) + intake->Set(-0.6); //Same as auton + safetyTimer.Stop(); + if (safetyTimer.Get() < 2.f && isToteInTransit()) { //The robot has contacted the transit wheels; keep intaking while (isToteInTransit()) - intake->set(-0.6); + intake->Set(-0.6); //Tote probably ejected by now. - intake->set(0); - lift->set(-1); //Lower lift for grabbing + intake->Set(0); + lift->Set(-1); //Lower lift for grabbing while (!isLiftDown()) - lift->set(-1.f); + lift->Set(-1.f); //Do the little jerk-to-align trick copied *ahem* plagiarized *ahem* straight out of auton, so this works for sure drivebase->GoFast(); @@ -180,7 +180,7 @@ namespace dreadbot drivebase->Drive_v(0, 0, 0); drivebase->GoSlow(); - lift->set(1.f); //Raise the lift again - lift cycle complete. + lift->Set(1.f); //Raise the lift again - lift cycle complete. } } if (autoToteCooldown > 0) diff --git a/src/Robot.h b/src/Robot.h index c17af5b..4f85bb7 100644 --- a/src/Robot.h +++ b/src/Robot.h @@ -23,7 +23,7 @@ #define B_AXS_TOTE_IN 2 //LB #define B_BTN_ARMS_OUT 3 #define B_BTN_ARMS_IN 2 - #define B_BTN_AUTOTOTE -1 //Temporary - Parker, find a control for this! + #define B_BTN_AUTOTOTE 8 //#define B_BTN_GEDDAN 5 // Robot does a short dance to recover from an intake failure // Shared controls From f8c87ddacd47a796d0eaf174cc7f9a2db2466c08 Mon Sep 17 00:00:00 2001 From: Sourec Date: Thu, 23 Apr 2015 12:09:18 -0400 Subject: [PATCH 18/32] Switched autotote control to start button --- src/Robot.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/Robot.h b/src/Robot.h index 4f85bb7..069bf0e 100644 --- a/src/Robot.h +++ b/src/Robot.h @@ -13,19 +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 //RB #define B_AXS_TOTE_IN 2 //LB #define B_BTN_ARMS_OUT 3 #define B_BTN_ARMS_IN 2 - #define B_BTN_AUTOTOTE 8 - //#define B_BTN_GEDDAN 5 // Robot does a short dance to recover from an intake failure + #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 From 33d4ccba41e06e133c429fb108ea6f980d478465 Mon Sep 17 00:00:00 2001 From: Parker Date: Fri, 24 Apr 2015 09:39:06 -0400 Subject: [PATCH 19/32] auto --- README.md | 18 ----- iomap.txt | 103 -------------------------- lib/Logger.cpp | 2 +- lib/Logger.h | 32 ++++---- src/Autonomous.cpp | 172 ++++++++++++++++++++++++++++++------------- src/Autonomous.h | 94 +++++++++++++++++------ src/Config.h | 5 ++ src/FSM.h | 2 +- src/MecanumDrive.cpp | 76 +++++++------------ src/MecanumDrive.h | 20 ++--- src/Robot.cpp | 96 +++++++++++++----------- src/Robot.h | 2 + src/SimplePID.cpp | 128 -------------------------------- src/SimplePID.h | 51 ------------- src/XMLInput.cpp | 24 +++--- src/XMLInput.h | 20 ++--- src/mathutil.h | 128 -------------------------------- src/rps.h | 86 ---------------------- 18 files changed, 332 insertions(+), 727 deletions(-) delete mode 100644 README.md delete mode 100644 iomap.txt delete mode 100644 src/SimplePID.cpp delete mode 100644 src/SimplePID.h delete mode 100644 src/mathutil.h delete mode 100644 src/rps.h 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 index 2ad1a91..12ac184 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -24,59 +24,70 @@ namespace dreadbot } int GettingTote::update() { - if (isToteInTransit() && !timerActive) + 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. + 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) - { + if (timerActive) { //Keep sucking a tote in - intake->Set(-0.5); + if (HALBot::getToteCount() >= 2) { + intake->Set(-1); + } else { + 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 + 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) - { + } + drivebase->Drive_v(0, -0.65, 0); + if (HALBot::getToteCount() >= 2) { + intake->Set(-1); + } else { + intake->Set(-0.6); + } + if (eStopTimer.Get() >= 3.5) { + XMLInput::getInstance()->getPGroup("intakeArms")->Set(1); + } else if (eStopTimer.Get() >= 2.5 && eStopTimer.Get() < 3.5) { + XMLInput::getInstance()->getPGroup("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(); - if (drivebase != nullptr) drivebase->Drive_v(0, 0, 0); - if (intake != nullptr) intake->Set(0); - sysLog->log("E-stopped in GettingTote", Hydra::error); + if (drivebase != nullptr) // Stop the drivebase + drivebase->Drive_v(0, 0, 0); + if (intake != nullptr) // Stop the intake wheels + intake->Set(0); + XMLInput::getInstance()->getPGroup("lift")->Set(1); // Raise the lift + sysLog->log("Emergency-stopped in GettingTote", Hydra::error); return HALBot::eStop; } return HALBot::no_update; } - DriveToZone::DriveToZone() - { + DriveToZone::DriveToZone() { drivebase = nullptr; timerActive = false; strafe = false; dir = 1; //Multiplier that changes the direction the robot moves. } - void DriveToZone::setHardware(MecanumDrive* newDrivebase) - { + void DriveToZone::setHardware(MecanumDrive* newDrivebase) { drivebase = newDrivebase; } - void DriveToZone::enter() - { + void DriveToZone::enter() { driveTimer.Reset(); driveTimer.Start(); timerActive = true; @@ -121,14 +132,32 @@ namespace dreadbot void ForkGrab::enter() { sysLog->log("State: ForkGrab"); + grabTimer.Reset(); + grabTimer.Start(); } int ForkGrab::update() { + if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= LIFT_ENGAGEMENT_DELAY) { + XMLInput::getInstance()->getPGroup("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 HALBot::finish; + } else { + return HALBot::no_update; + } + } else if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.2) { + //XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); + } if (isLiftDown()) { - HALBot::incrTote(); + 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 sketchy. Causes the robot to NOT lift before rotating/driving + //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 @@ -136,11 +165,12 @@ namespace dreadbot lift->Set(0); return HALBot::finish; } - //Raise the lift and cheat to alight the tote - drivebase->Drive_v(0, 1, 0); // @todo Calibrate + 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 (HALBot::enoughTotes()) @@ -157,6 +187,10 @@ namespace dreadbot void Stopped::enter() { sysLog->log("State: Stopped"); + if (drivebase != nullptr) + drivebase->Drive_v(0, 0, 0); + 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() { @@ -194,9 +228,7 @@ namespace dreadbot { sysLog->log("State: BackAway"); drivebase->Drive_v(0, 0, 0); - grabTimer.Reset(); - grabTimer.Start(); - timerActive = false; //Cheat way of figuring out if the lift is down + timerActive = false; //Cheat way of figuring out if the lift is down. Used elsewhere } int BackAway::update() { @@ -210,8 +242,10 @@ namespace dreadbot { timerActive = true; XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); + Wait(0.13); //Uber cheap way of getting the totes to disengage + grabTimer.Reset(); + grabTimer.Start(); } - return HALBot::no_update; } @@ -221,8 +255,10 @@ namespace dreadbot return HALBot::timerExpired; } - if (drivebase != nullptr) + if (drivebase != nullptr) { + drivebase->GoFast(); drivebase->Drive_v(0, -1, 0); + } XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); return HALBot::no_update; } @@ -239,7 +275,7 @@ namespace dreadbot int PushContainer::update() { float pushTime = PUSH_TIME; - if (enableScaling) + 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) @@ -257,11 +293,11 @@ namespace dreadbot } if (drivebase != nullptr) - drivebase->Drive_v(0, -PUSH_SPEED, 0); //Straight forward + drivebase->Drive_v(DRIVE_STRAFE_CORRECTION, -PUSH_SPEED, DRIVE_ROTATE_CORRECTION); //Straight forward if (pusher1 != nullptr) - pusher1->Set(1); //Push the container? + pusher1->Set(INTAKE_PUSH_SPEED); //Push the container? if (pusher2 != nullptr) - pusher2->Set(1); + pusher2->Set(INTAKE_PUSH_SPEED); return HALBot::no_update; } @@ -278,18 +314,49 @@ namespace dreadbot } int RotateDrive::update() { - if (driveTimer.Get() >= (ROTATE_TIME - 1.0f)) + if (driveTimer.Get() >= (ROTATE_TIME - 0.5f)) { //Rotated far enough; break timerActive = false; + drivebase->GoSpeed(1.0); + drivebase->Drive_v(0, 1, 0); +// Change: The stack is lowered prior to stopping in order to decelerate properly if (HALBot::getToteCount() == 3) XMLInput::getInstance()->getPGroup("lift")->Set(-1); //Lower lift + + Wait(ROTATE_DRIVE_STRAIGHT); + return HALBot::timerExpired; } if (drivebase != nullptr) - drivebase->Drive_v(0, 1.0 * dir, 0.5 * rotateConstant); // @todo Add RotateDrive coefficients to preprocessor definitions + drivebase->Drive_v(0, RD_DRIVE_SPEED, RD_ROTATE_SPEED); return HALBot::no_update; } + StrafeLeft::StrafeLeft() + { + drivebase = nullptr; + 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() + { + int rc = HALBot::no_update; + if (driveTimer.Get() >= 0.5f) { + driveTimer.Stop(); + driveTimer.Reset(); + timerActive = false; + drivebase->Drive_v(0, 0, 0); //stop + rc = HALBot::timerExpired; + } + return rc; + } int HALBot::toteCount = 0; AutonMode HALBot::mode = AUTON_MODE_STOP; @@ -330,7 +397,8 @@ namespace dreadbot backAway = new BackAway; fsm = new FiniteStateMachine; rotateDrive = new RotateDrive; - mode = AUTON_MODE_DRIVE; + strafeLeft = new StrafeLeft; + mode = AUTON_MODE_DRIVE; //Just drive straight forward. Assumes a spherical cow. } HALBot::~HALBot() { @@ -351,6 +419,7 @@ namespace dreadbot sysLog = Logger::getInstance()->getLog("sysLog"); gettingTote->setHardware(drivebase, intake); driveToZone->setHardware(drivebase); + strafeLeft->setHardware(drivebase); rotate->setHardware(drivebase); rotateDrive->setHardware(drivebase); rotate2->setHardware(drivebase); @@ -359,6 +428,7 @@ namespace dreadbot pushContainer->pusher2 = XMLInput::getInstance()->getPWMMotor(1); pushContainer->pushConstant = 1; stopped->lift = lift; //Don't know if I like these... + stopped->drivebase = drivebase; forkGrab->lift = lift; forkGrab->drivebase = drivebase; backAway->lift = lift; @@ -396,6 +466,7 @@ namespace dreadbot transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, rotate2}; transitionTable[i++] = {rotate2, HALBot::timerExpired, nullptr, stopped}; transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; transitionTable[i++] = END_STATE_TABLE; defState = gettingTote; } @@ -406,6 +477,7 @@ namespace dreadbot rotate->rotateConstant = -1; transitionTable[i++] = {rotate, HALBot::timerExpired, nullptr, driveToZone}; transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; transitionTable[i++] = END_STATE_TABLE; } if (mode == AUTON_MODE_BOTH) @@ -418,22 +490,21 @@ namespace dreadbot if (mode == AUTON_MODE_STACK2) { i = 0; - rotate->rotateConstant = 1; - rotate2->rotateConstant = -1; + rotateDrive->rotateConstant = -1; pushContainer->pushConstant = -1; + pushContainer->enableScaling = true; driveToZone->strafe = false; + incrTote(); //We already have a tote - transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; - transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; - transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, rotate}; transitionTable[i++] = {pushContainer, HALBot::timerExpired, nullptr, gettingTote}; - transitionTable[i++] = {rotate, HALBot::timerExpired, nullptr, driveToZone}; - transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, rotate2}; - transitionTable[i++] = {rotate2, HALBot::timerExpired, nullptr, stopped}; - //transitionTable[i++] = {backAway, HALBot::timerExpired, nullptr, stopped}; + transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; - transitionTable[i++] = END_STATE_TABLE; - defState = gettingTote; + transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, strafeLeft}; + transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; + transitionTable[i++] = {strafeLeft, HALBot::timerExpired, nullptr, rotateDrive}; + transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; + defState = pushContainer; } if (mode == AUTON_MODE_STACK3) { @@ -451,6 +522,7 @@ namespace dreadbot transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, backAway}; transitionTable[i++] = {backAway, HALBot::timerExpired, nullptr, stopped}; transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; defState = pushContainer; } diff --git a/src/Autonomous.h b/src/Autonomous.h index dfb3115..02e08b0 100644 --- a/src/Autonomous.h +++ b/src/Autonomous.h @@ -10,20 +10,59 @@ #include "../lib/Logger.h" using namespace Hydra; -//All timings -#define STRAFE_TO_ZONE_TIME 3.1f -#define DRIVE_TO_ZONE_TIME 2.0f +// Autonomous operational parameters + +#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 1.0f // Rotational velocity of the intake wheels when pushing a container out of the way +#define PUSH_TIME 0.9f // How long the robot pushes containers out of the way +#define PUSH_SPEED 0.75f // How fast the robot drives forward when pushing containers +#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.6f // Size of the RotateDrive arc +#define BACK_AWAY_TIME 0.75f // How long the robot backs up after stopping in 3TA +#define ROTATE_TIME 1.16f // Also, timing is modified in RotateDrive::update - 0.5 s is subtracted +#define ROTATE_DRIVE_STRAIGHT 0.5f // 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.9f // 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. -#define PUSH_TIME 0.9 -#define PUSH_SPEED 0.75 -#define BACK_AWAY_TIME 4.0f -#define ROTATE_TIME 2.5f //Also, timing is modified in RotateDrive::update - 1.0 s is subtracted -#define ESTOP_TIME 6.0f -#define STACK_CORRECTION_TIME 0.35f namespace dreadbot { + #define STRAFE_TO_ZONE_TIME = SmartDashboard::GetNumber('STRAFE_TO_ZONE_TIME', 3.1f // Used in some auton modes (!2TA !3TA) - how long does the robot strafe? + #define DRIVE_TO_ZONE_TIME = SmartDashboard::GetNumber('DRIVE_TO_ZONE_TIME', 2.0f // How long the robot normally drives forward + + // Container evacuation parameter + #define INTAKE_PUSH_SPEED = SmartDashboard::GetNumber(' ', 0.95f); // Rotational velocity of the intake wheels when pushing a container out of the way + #define PUSH_TIME = SmartDashboard::GetNumber(' ', 0.9f); // How long the robot pushes containers out of the way + #define PUSH_SPEED = SmartDashboard::GetNumber(' ', 0.75f); // How fast the robot drives forward when pushing containers + #define DRIVE_STRAFE_CORRECTION = SmartDashboard::GetNumber('', 0.0f); // Aport drift abbertaion correctional factor + #define DRIVE_ROTATE_CORRECTION 0.0f // Rotate abberation correctional factor + + // Endgame parameters + #define RD_DRIVE_SPEED = SmartDashboard::GetNumber('', 0.8f); // Astern speed in RotateDrive. + #define RD_ROTATE_SPEED -0.6f // Size of the RotateDrive arc + + #define BACK_AWAY_TIME 0.6f // How long the robot backs up after stopping in 3TA + #define ROTATE_TIME 1.16f // Also, timing is modified in RotateDrive::update - 1.0 s is subtracted + #define ROTATE_DRIVE_STRAIGHT 0.5f // 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.9f // 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. + + //States class GettingTote : public FSMState { @@ -31,13 +70,14 @@ namespace dreadbot GettingTote(); virtual void enter(); virtual int update(); - void setHardware(MecanumDrive* newDrivebase, MotorGrouping* newIntake); + void setHardware(MecanumDrive* newDrivebase, MotorGrouping* newIntake); private: MecanumDrive* drivebase; MotorGrouping* intake; bool timerActive; Timer getTimer; Timer eStopTimer; + Timer *armTimer; }; class DriveToZone : public FSMState { @@ -80,6 +120,7 @@ namespace dreadbot virtual void enter(); virtual int update(); PneumaticGrouping* lift; + MecanumDrive* drivebase; }; class PushContainer : public DriveToZone { @@ -106,7 +147,15 @@ namespace dreadbot int update(); void enter(); }; + class StrafeLeft : public DriveToZone + { + public: + StrafeLeft(); + int update(); + void enter(); + }; + //Needs to appear in https://en.wikipedia.org/wiki/Kludge#Computer_science class HALBot { public: @@ -114,20 +163,22 @@ namespace dreadbot HALBot(); ~HALBot(); - static bool enoughTotes(); - static void incrTote(); - static int getToteCount(); - void setMode(AutonMode newMode); - AutonMode getMode(); - void init(MecanumDrive* drivebase, MotorGrouping* intake, PneumaticGrouping* lift); - void update(); + 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; + static int toteCount; //How many totes have been acquired. FiniteStateMachine* fsm; - static AutonMode mode; + 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. - FSMTransition transitionTable[15]; - GettingTote* gettingTote; + //State objects. These should be self-explanatory. + GettingTote* gettingTote; DriveToZone* driveToZone; ForkGrab* forkGrab; Rotate* rotate; @@ -136,5 +187,6 @@ namespace dreadbot PushContainer* pushContainer; BackAway* backAway; RotateDrive* rotateDrive; + StrafeLeft* strafeLeft; }; } diff --git a/src/Config.h b/src/Config.h index 73d5f2f..38f7bc8 100644 --- a/src/Config.h +++ b/src/Config.h @@ -2,6 +2,11 @@ #include using std::string; +/* +* Contents of the original Bot_Config.xml file. Uses a neat multiline trick +* to hold the contents of a human/computer readable XML file that PugiXML +* can parse. +*/ #define MULTILINE(...) #__VA_ARGS__ diff --git a/src/FSM.h b/src/FSM.h index cc09d26..916341d 100644 --- a/src/FSM.h +++ b/src/FSM.h @@ -9,7 +9,7 @@ namespace dreadbot class FSMState { public: - virtual void enter() = 0; + virtual void enter() = 0; //Automatically when this state is first entered. virtual int update() = 0; virtual ~FSMState() {} }; diff --git a/src/MecanumDrive.cpp b/src/MecanumDrive.cpp index 1d26ee9..1d0a1f2 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 @@ -44,84 +41,67 @@ MecanumDrive::MecanumDrive(int motorId_lf, int motorId_rf, int motorId_lr, int m MecanumDrive::~MecanumDrive() { Disengage(); for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { - delete motors[i]; } } void MecanumDrive::GoFast() { for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { - motors[i]->SetPID(1, 0, 0); + motors[i]->SetPID(1, 0, 0); //Magically makes the robot drive faster. } } void MecanumDrive::GoSlow() { for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { - motors[i]->SetPID(0.5, 0, 0); + motors[i]->SetPID(0.5, 0, 0); //Magically makes the robot drive slower. } } -// 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 rot_out = -rotation - (sqrt(x*x + y*y) * 0.01); - - if (mode == drivemode::relative) { - /* - #ifdef SQUARE_INPUTS - vec_out.x = vec_out.x*std::abs(vec_out.x); - vec_out.y = vec_out.y*std::abs(vec_out.y); - rot_out = rot_out*std::abs(rot_out); - #endif - */ - } + 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) absSpeeds[i] = fabs(wspeeds[i]); double maxMagnitude = *std::max_element(absSpeeds, absSpeeds + MOTOR_COUNT); - if (maxMagnitude > 1.0) { + if (maxMagnitude > 1.0) { //Normalizes wheel speeds if any one speed's absolute value is above 1, since the range of accepted inputs is between 1 and -1. for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { wspeeds[i] /= maxMagnitude; } } + //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]*1023, 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); } } @@ -149,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 d8e99c8..7a15439 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 { @@ -28,32 +24,28 @@ namespace dreadbot { m_rightRear = 3 }; - void Set(int motorId_lf, int motorId_rf, int motorId_lr, int motorId_rr); + void Set(int motorId_lf, int motorId_rf, int motorId_lr, int motorId_rr); //Sets the CAN IDs used for the motors. MecanumDrive(int motorId_lf, int motorId_rf, int motorId_lr, int motorId_rr); ~MecanumDrive(); void GoSlow(); void GoFast(); - void Drive_p(double x, double y, double rotation); - void Drive_v(double x, double y, double rotation); + void GoSpeed(double speed); + void Drive_v(double x, double y, double rotation); //Velocity based driving. void SetDriveMode(drivemode newMode); void Engage(); void Disengage(); void SD_RetrievePID(); - void SD_OutputDiagnostics(); + void SD_OutputDiagnostics(); //Outputs a bunch of useful motor stats, many of which are disabled (commented out) 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 9e2dbe4..cc3b905 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -68,6 +68,8 @@ namespace dreadbot viewingBack = false; Cam2Enabled = false; Cam1Enabled = StartCamera(1); + + sysLog->log("Robot ready."); } void GlobalInit() @@ -91,28 +93,32 @@ namespace dreadbot GlobalInit(); if (AutonBot == nullptr) AutonBot = new HALBot; - AutonBot->setMode(AUTON_MODE_STACK3); - sysLog->log("Auton mode is " + (int)GetAutonMode()); + AutonBot->setMode(GetAutonMode()); //Uses the 10-switch to get the auton mode. AutonBot->init(drivebase, intake, lift); drivebase->GoSlow(); - if (AutonBot->getMode() == AUTON_MODE_STACK3) + + if (viewingBack) + { + StopCamera(2); + Cam1Enabled = StartCamera(1); + Cam2Enabled = false; + viewingBack = false; + } + if (AutonBot->getMode() == AUTON_MODE_STACK3 || AutonBot->getMode() == AUTON_MODE_STACK2) { lift->Set(1); - Wait(0.2); + Wait(0.2); // May be able to lower this. } } void AutonomousPeriodic() { - drivebase->SD_RetrievePID(); - AutonBot->update(); - - //Vision during auton, because why not - if (viewingBack && Cam2Enabled) - { - IMAQdxGrab(sessionCam2, frame2, true, nullptr); - CameraServer::GetInstance()->SetImage(frame2); - } + //if (ds->GetMatchTime() >= 0.0) { + AutonBot->update(); + //} else { + // drivebase->Drive_v(0, 0, 0); + //} + if (!viewingBack && Cam1Enabled) { IMAQdxGrab(sessionCam1, frame1, true, nullptr); @@ -122,25 +128,24 @@ namespace dreadbot void TeleopInit() { - sysLog->log("Initializing Teleop"); + sysLog->log("Initializing Teleop."); GlobalInit(); drivebase->GoSlow(); } void TeleopPeriodic() { - Input->updateDrivebase(); + Input->updateDrivebase(); //Makes the robot drive using Config.h controls and a sensitivity curve (tested) - //Output controls - float intakeInput = gamepad->GetRawAxis(3); - intake->Set(((float) (intakeInput > 0.15) * -0.8) + gamepad2->GetRawAxis(3) - gamepad2->GetRawAxis(2)); + // Control mappings + 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); + 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)); liftArms->Set(-(float) gamepad->GetRawButton(5)); @@ -150,7 +155,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) @@ -182,11 +186,21 @@ namespace dreadbot void TestInit() { sysLog->log("Initializing Test mode."); - GlobalInit(); + compressor->Start(); } 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() @@ -227,9 +241,9 @@ namespace dreadbot imaqError = IMAQdxCloseCamera(sessionCam1); if (imaqError != IMAQdxErrorSuccess) { - DriverStation::ReportError( - "cam1 IMAQdxCloseCamera error: " - + std::to_string((long) imaqError) + "\n"); + sysLog->log( + "cam1 IMAQdxCloseCamera error - " + + std::to_string((long) imaqError), Hydra::error); return false; } } @@ -239,9 +253,9 @@ namespace dreadbot imaqError = IMAQdxCloseCamera(sessionCam2); if (imaqError != IMAQdxErrorSuccess) { - DriverStation::ReportError( - "cam0 IMAQdxCloseCamera error: " - + std::to_string((long) imaqError) + "\n"); + sysLog->log( + "cam2 IMAQdxCloseCamera error - " + + std::to_string((long) imaqError), Hydra::error); return false; } } @@ -256,17 +270,17 @@ namespace dreadbot IMAQdxCameraControlModeController, &sessionCam1); if (imaqError != IMAQdxErrorSuccess) { - DriverStation::ReportError( - "cam1 IMAQdxOpenCamera error: " - + std::to_string((long) imaqError) + "\n"); + sysLog->log( + "cam1 IMAQdxOpenCamera error - " + + std::to_string((long) imaqError), Hydra::error); return false; } imaqError = IMAQdxConfigureGrab(sessionCam1); if (imaqError != IMAQdxErrorSuccess) { - DriverStation::ReportError( - "cam0 IMAQdxConfigureGrab error: " - + std::to_string((long) imaqError) + "\n"); + sysLog->log( + "cam1 IMAQdxConfigureGrab error - " + + std::to_string((long) imaqError), Hydra::error); return false; } // acquire images @@ -278,17 +292,17 @@ namespace dreadbot IMAQdxCameraControlModeController, &sessionCam2); if (imaqError != IMAQdxErrorSuccess) { - DriverStation::ReportError( - "cam0 IMAQdxOpenCamera error: " - + std::to_string((long) imaqError) + "\n"); + sysLog->log( + "cam2 IMAQdxOpenCamera - " + + std::to_string((long) imaqError), Hydra::error); return false; } imaqError = IMAQdxConfigureGrab(sessionCam2); if (imaqError != IMAQdxErrorSuccess) { - DriverStation::ReportError( - "cam0 IMAQdxConfigureGrab error: " - + std::to_string((long) imaqError) + "\n"); + sysLog->log( + "cam2 IMAQdxConfigureGrab - " + + std::to_string((long) imaqError), Hydra::error); return false; } // acquire images diff --git a/src/Robot.h b/src/Robot.h index e10acf4..2c4f692 100644 --- a/src/Robot.h +++ b/src/Robot.h @@ -1,3 +1,5 @@ +//Due to encountered issues, most of the controls defined here are NOT used. + // Control mappings #define LOGITECH_F310_X // Use Logitech F310 controllers 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 38cd264..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 @@ -31,7 +33,7 @@ namespace dreadbot DoubleSolenoid* dPneumatic; Solenoid* sPneumatic; int actionCount; - bool invert; + bool invert; //If true, the given directions are inverted. Forward, Backward = Backward, Forward. On, Off = Off, On friend class XMLInput; }; class SimpleMotor @@ -71,26 +73,26 @@ namespace dreadbot friend class XMLInput; }; + //Singleton class for managing pretty much all motors, pneumatics, and controllers. class XMLInput { public: static XMLInput* getInstance(); - void setDrivebase(MecanumDrive* newDrivebase); - void loadXMLConfig(); - void updateDrivebase(); - void zeroVels(); + void setDrivebase(MecanumDrive* newDrivebase); //Sets the drivebase that velocity information is sent to. + void loadXMLConfig(); //Clears previous configuration and loads from the XML doc in Config.h + void updateDrivebase(); //Handels all drivebase-related stuff, including inverts, deadzones, and the sensativity curve. Joystick* getController(int ID); //!< Gets a joystick with the given ID. If joystick does not exist, creates joystick with ID and returns it. CANTalon* getCANMotor(int ID); //!< Gets a CANTalon with the given ID. If the CANTalon does not exist, creates CANTalon with ID and returns it. Talon* getPWMMotor(int ID); //!< Gets a Talon with the given ID. If the Talon does not exist, creates CANTalon with ID and returns it. DoubleSolenoid* getDPneum(int forwardID); //!< Gets a DoubleSolenoid based on the ID. The ID is for the FORWARD output thingy. Solenoid* getSPneum(int ID); //!< Gets a single solenoid based on the ID. - PneumaticGrouping* getPGroup(string name); - MotorGrouping* getMGroup(string name); + PneumaticGrouping* getPGroup(string name); //Find a pneumatic grouping by name (found in Config.h) + MotorGrouping* getMGroup(string name); //Find a motor grouping by name (found in Config.h) 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; - }; -} From b0b9290315165bf39ae61f2b614557777b4f1998 Mon Sep 17 00:00:00 2001 From: Parker Date: Fri, 24 Apr 2015 11:37:47 -0400 Subject: [PATCH 20/32] Timing tweaks - mostly working --- src/Autonomous.h | 39 ++++++--------------------------------- 1 file changed, 6 insertions(+), 33 deletions(-) diff --git a/src/Autonomous.h b/src/Autonomous.h index 02e08b0..d72b473 100644 --- a/src/Autonomous.h +++ b/src/Autonomous.h @@ -10,8 +10,6 @@ #include "../lib/Logger.h" using namespace Hydra; -// Autonomous operational parameters - #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 @@ -20,49 +18,24 @@ using namespace Hydra; #define INTAKE_PUSH_SPEED 1.0f // Rotational velocity of the intake wheels when pushing a container out of the way #define PUSH_TIME 0.9f // How long the robot pushes containers out of the way #define PUSH_SPEED 0.75f // How fast the robot drives forward when pushing containers -#define DRIVE_STRAFE_CORRECTION 0.0f // Aport drift abbertaion correctional factor +#define DRIVE_STRAFE_CORRECTION 0.04f // 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.6f // Size of the RotateDrive arc -#define BACK_AWAY_TIME 0.75f // How long the robot backs up after stopping in 3TA -#define ROTATE_TIME 1.16f // Also, timing is modified in RotateDrive::update - 0.5 s is subtracted -#define ROTATE_DRIVE_STRAIGHT 0.5f // How long to drive straight in RotateDrive AFTER rotating +#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.8f // Also, timing is modified in RotateDrive::update - 1.0 s is subtracted +#define ROTATE_DRIVE_STRAIGHT 0.2f // 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.9f // How quickly 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 { - #define STRAFE_TO_ZONE_TIME = SmartDashboard::GetNumber('STRAFE_TO_ZONE_TIME', 3.1f // Used in some auton modes (!2TA !3TA) - how long does the robot strafe? - #define DRIVE_TO_ZONE_TIME = SmartDashboard::GetNumber('DRIVE_TO_ZONE_TIME', 2.0f // How long the robot normally drives forward - - // Container evacuation parameter - #define INTAKE_PUSH_SPEED = SmartDashboard::GetNumber(' ', 0.95f); // Rotational velocity of the intake wheels when pushing a container out of the way - #define PUSH_TIME = SmartDashboard::GetNumber(' ', 0.9f); // How long the robot pushes containers out of the way - #define PUSH_SPEED = SmartDashboard::GetNumber(' ', 0.75f); // How fast the robot drives forward when pushing containers - #define DRIVE_STRAFE_CORRECTION = SmartDashboard::GetNumber('', 0.0f); // Aport drift abbertaion correctional factor - #define DRIVE_ROTATE_CORRECTION 0.0f // Rotate abberation correctional factor - - // Endgame parameters - #define RD_DRIVE_SPEED = SmartDashboard::GetNumber('', 0.8f); // Astern speed in RotateDrive. - #define RD_ROTATE_SPEED -0.6f // Size of the RotateDrive arc - - #define BACK_AWAY_TIME 0.6f // How long the robot backs up after stopping in 3TA - #define ROTATE_TIME 1.16f // Also, timing is modified in RotateDrive::update - 1.0 s is subtracted - #define ROTATE_DRIVE_STRAIGHT 0.5f // 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.9f // 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. - - //States class GettingTote : public FSMState { From da651785822ebb6c50335e9f027a26bf1a0bfd3f Mon Sep 17 00:00:00 2001 From: Parker Date: Fri, 24 Apr 2015 15:54:35 -0400 Subject: [PATCH 21/32] Moar auton updates --- src/Autonomous.cpp | 18 ++++++++++++------ src/Autonomous.h | 14 ++++++++------ 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index cdef985..e1b7289 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -141,6 +141,7 @@ namespace dreadbot grabTimer.Reset(); grabTimer.Start(); } + bool thing = true; int ForkGrab::update() { if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= LIFT_ENGAGEMENT_DELAY) @@ -158,13 +159,15 @@ namespace dreadbot } else return HALBot::no_update; - } - else if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.2){ //You win here! + } + } else if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.2 && thing) { //XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); + thing = false; } if (isLiftDown()) { + //XMLInput::getInstance()->getPGroup("liftArms")->Set(0); 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 @@ -270,7 +273,7 @@ namespace dreadbot drivebase->GoFast(); drivebase->Drive_v(0, -1, 0); } - XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); + //XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); return HALBot::no_update; } @@ -302,9 +305,11 @@ namespace dreadbot drivebase->Drive_v(0, 0, 0); return HALBot::timerExpired; } - - if (drivebase != nullptr) + if (HALBot::getToteCount() >= 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) @@ -322,6 +327,7 @@ namespace dreadbot sysLog->log("State: RotateDrive"); drivebase->GoFast(); //Gotta go faaaaaaaasssst. driveTimer.Start(); + //XMLInput::getInstance()->getPGroup("liftArms")->Set(0); } int RotateDrive::update() { @@ -331,7 +337,7 @@ namespace dreadbot drivebase->GoSpeed(1.0); drivebase->Drive_v(0, 1, 0); - // Change: The stack is lowered prior to stopping in order to decelerate properly + // The stack is lowered prior to stopping in order to decelerate properly if (HALBot::getToteCount() == 3) XMLInput::getInstance()->getPGroup("lift")->Set(-1); //Lower lift diff --git a/src/Autonomous.h b/src/Autonomous.h index 9cb8d53..af0bf72 100644 --- a/src/Autonomous.h +++ b/src/Autonomous.h @@ -15,18 +15,20 @@ using namespace Hydra; #define DRIVE_TO_ZONE_TIME 2.0f // How long the robot normally drives forward // Container evacuation parameter -#define INTAKE_PUSH_SPEED 1.0f // Rotational velocity of the intake wheels when pushing a container out of the way +#define INTAKE_PUSH_SPEED 0.9f // Rotational velocity of the intake wheels when pushing a container out of the way #define PUSH_TIME 0.9f // How long the robot pushes containers out of the way -#define PUSH_SPEED 0.75f // How fast the robot drives forward when pushing containers -#define DRIVE_STRAFE_CORRECTION 0.04f // Aport drift abbertaion correctional factor -#define DRIVE_ROTATE_CORRECTION 0.0f // Rotate abberation correctional factor +#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.03f // 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.8f // Also, timing is modified in RotateDrive::update - 1.0 s is subtracted -#define ROTATE_DRIVE_STRAIGHT 0.2f // How long to drive straight in RotateDrive AFTER rotating +#define ROTATE_TIME 0.6f // Also, timing is modified in RotateDrive::update - 1.0 s is subtracted +#define ROTATE_DRIVE_STRAIGHT 0.3f // How long to drive straight in RotateDrive AFTER rotating // Tote collection parameters #define STACK_CORRECTION_TIME 0.25f // How long the robot jerks backward From 88805144e7e852f027229e5246b9ee81ec38b8d8 Mon Sep 17 00:00:00 2001 From: Parker Date: Fri, 24 Apr 2015 17:30:54 -0400 Subject: [PATCH 22/32] Fixed push time --- src/Autonomous.cpp | 5 ++--- src/Autonomous.h | 6 +++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/Autonomous.cpp b/src/Autonomous.cpp index e1b7289..c95f6fe 100644 --- a/src/Autonomous.cpp +++ b/src/Autonomous.cpp @@ -156,8 +156,7 @@ namespace dreadbot Wait(0.3); //Totes must engage first lift->Set(0); return HALBot::finish; - } - else + } else { return HALBot::no_update; } } else if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.2 && thing) { @@ -290,7 +289,7 @@ namespace dreadbot { 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 + pushTime += ((float)HALBot::getToteCount() - 1.f) / 3.f; //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) { diff --git a/src/Autonomous.h b/src/Autonomous.h index af0bf72..7edfa5a 100644 --- a/src/Autonomous.h +++ b/src/Autonomous.h @@ -16,19 +16,19 @@ using namespace Hydra; // 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 0.9f // How long the robot pushes containers 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.03f // Rotate abberation 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.3f // How long to drive straight in RotateDrive AFTER rotating +#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 From a8894901b4688bcba4ffcd3dd2ca9b4a9c4ab10e Mon Sep 17 00:00:00 2001 From: Sourec Date: Sat, 25 Apr 2015 09:33:27 -0400 Subject: [PATCH 23/32] Deleted unnecessary .gitmodules file --- .gitmodules | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 .gitmodules diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index e69de29..0000000 From 7d9048cd7b1282e8de19e552df6d686f1537e66c Mon Sep 17 00:00:00 2001 From: Sourec Date: Mon, 4 May 2015 11:51:55 -0400 Subject: [PATCH 24/32] Moved Autonomous files to src/Autonomous (there are a lot of them), added RoboState subclass thingy. --- src/{ => Autonomous}/Autonomous.cpp | 0 src/{ => Autonomous}/Autonomous.h | 39 +----------------- src/{ => Autonomous}/FSM.cpp | 0 src/{ => Autonomous}/FSM.h | 0 src/Autonomous/RoboState.h | 62 +++++++++++++++++++++++++++++ 5 files changed, 63 insertions(+), 38 deletions(-) rename src/{ => Autonomous}/Autonomous.cpp (100%) rename src/{ => Autonomous}/Autonomous.h (62%) rename src/{ => Autonomous}/FSM.cpp (100%) rename src/{ => Autonomous}/FSM.h (100%) create mode 100644 src/Autonomous/RoboState.h diff --git a/src/Autonomous.cpp b/src/Autonomous/Autonomous.cpp similarity index 100% rename from src/Autonomous.cpp rename to src/Autonomous/Autonomous.cpp diff --git a/src/Autonomous.h b/src/Autonomous/Autonomous.h similarity index 62% rename from src/Autonomous.h rename to src/Autonomous/Autonomous.h index 7edfa5a..ac951c5 100644 --- a/src/Autonomous.h +++ b/src/Autonomous/Autonomous.h @@ -1,41 +1,4 @@ -#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; - -#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 { //States 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 100% rename from src/FSM.h rename to src/Autonomous/FSM.h diff --git a/src/Autonomous/RoboState.h b/src/Autonomous/RoboState.h new file mode 100644 index 0000000..44ad9ec --- /dev/null +++ b/src/Autonomous/RoboState.h @@ -0,0 +1,62 @@ +#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: + virtual void enter() = 0; + virtual int update() = 0; + virtual ~RoboState() {} + + protected: + //Hardware for access everywhere + static MecanumDrive* drivebase; + static MotorGrouping* intake; + static PneumaticGrouping* lift; + static PneumaticGrouping* intakeArms; + static Talon* pusher1; + static Talon* pusher2; + static Log* sysLog; + + friend class HALBot; //Lets HALBot set values without need for getters/setters + }; +} From f13536a96e2bb83702f92e2ae156b202a343d9a4 Mon Sep 17 00:00:00 2001 From: Sourec Date: Mon, 4 May 2015 12:42:34 -0400 Subject: [PATCH 25/32] Moved everything into its own file, states now inherit RoboState instead. --- src/Autonomous/Autonomous.cpp | 559 ------------------------------- src/Autonomous/Autonomous.h | 129 ------- src/Autonomous/BackAway.cpp | 44 +++ src/Autonomous/BackAway.h | 13 + src/Autonomous/DriveToZone.cpp | 47 +++ src/Autonomous/DriveToZone.h | 19 ++ src/Autonomous/ForkGrab.cpp | 69 ++++ src/Autonomous/ForkGrab.h | 17 + src/Autonomous/GettingTote.cpp | 70 ++++ src/Autonomous/GettingTote.h | 18 + src/Autonomous/HALBot.cpp | 178 ++++++++++ src/Autonomous/HALBot.h | 38 +++ src/Autonomous/PushContainer.cpp | 44 +++ src/Autonomous/PushContainer.h | 16 + src/Autonomous/RoboState.h | 3 +- src/Autonomous/Rotate.cpp | 31 ++ src/Autonomous/Rotate.h | 16 + src/Autonomous/RotateDrive.cpp | 37 ++ src/Autonomous/RotateDrive.h | 14 + src/Autonomous/Stopped.cpp | 17 + src/Autonomous/Stopped.h | 13 + src/Autonomous/StrafeLeft.cpp | 30 ++ src/Autonomous/StrafeLeft.h | 14 + 23 files changed, 747 insertions(+), 689 deletions(-) delete mode 100644 src/Autonomous/Autonomous.cpp delete mode 100644 src/Autonomous/Autonomous.h create mode 100644 src/Autonomous/BackAway.cpp create mode 100644 src/Autonomous/BackAway.h create mode 100644 src/Autonomous/DriveToZone.cpp create mode 100644 src/Autonomous/DriveToZone.h create mode 100644 src/Autonomous/ForkGrab.cpp create mode 100644 src/Autonomous/ForkGrab.h create mode 100644 src/Autonomous/GettingTote.cpp create mode 100644 src/Autonomous/GettingTote.h create mode 100644 src/Autonomous/HALBot.cpp create mode 100644 src/Autonomous/HALBot.h create mode 100644 src/Autonomous/PushContainer.cpp create mode 100644 src/Autonomous/PushContainer.h create mode 100644 src/Autonomous/Rotate.cpp create mode 100644 src/Autonomous/Rotate.h create mode 100644 src/Autonomous/RotateDrive.cpp create mode 100644 src/Autonomous/RotateDrive.h create mode 100644 src/Autonomous/Stopped.cpp create mode 100644 src/Autonomous/Stopped.h create mode 100644 src/Autonomous/StrafeLeft.cpp create mode 100644 src/Autonomous/StrafeLeft.h diff --git a/src/Autonomous/Autonomous.cpp b/src/Autonomous/Autonomous.cpp deleted file mode 100644 index c95f6fe..0000000 --- a/src/Autonomous/Autonomous.cpp +++ /dev/null @@ -1,559 +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 - if (HALBot::getToteCount() >= 2) - intake->Set(-1); - else - 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.65, 0); - if (HALBot::getToteCount() >= 2) - intake->Set(-1); - else - intake->Set(-0.6); - - if (eStopTimer.Get() >= 3.5) - XMLInput::getInstance()->getPGroup("intakeArms")->Set(1); - else if (eStopTimer.Get() >= 2.5 && eStopTimer.Get() < 3.5) - XMLInput::getInstance()->getPGroup("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(); - if (drivebase != nullptr) // Stop the drivebase - drivebase->Drive_v(0, 0, 0); - if (intake != nullptr) // Stop the intake wheels - intake->Set(0); - XMLInput::getInstance()->getPGroup("lift")->Set(1); // Raise the lift - sysLog->log("Emergency-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"); - grabTimer.Reset(); - grabTimer.Start(); - } - bool thing = true; - int ForkGrab::update() - { - if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= LIFT_ENGAGEMENT_DELAY) - { - XMLInput::getInstance()->getPGroup("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 HALBot::finish; - } else { - return HALBot::no_update; - } - } else if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.2 && thing) { - //XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); - thing = false; - } - - if (isLiftDown()) - { - //XMLInput::getInstance()->getPGroup("liftArms")->Set(0); - 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, 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 (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 (drivebase != nullptr) - drivebase->Drive_v(0, 0, 0); - 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.13); //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->GoFast(); - 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.f) / 3.f; //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 (HALBot::getToteCount() >= 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 HALBot::no_update; - } - - RotateDrive::RotateDrive() - { - timerActive = false; - rotateConstant = 1; - } - void RotateDrive::enter() - { - sysLog->log("State: RotateDrive"); - drivebase->GoFast(); //Gotta go faaaaaaaasssst. - driveTimer.Start(); - //XMLInput::getInstance()->getPGroup("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 (HALBot::getToteCount() == 3) - XMLInput::getInstance()->getPGroup("lift")->Set(-1); //Lower lift - - Wait(ROTATE_DRIVE_STRAIGHT); - - return HALBot::timerExpired; - } - if (drivebase != nullptr) - drivebase->Drive_v(0, RD_DRIVE_SPEED, RD_ROTATE_SPEED); - return HALBot::no_update; - } - - StrafeLeft::StrafeLeft() - { - drivebase = nullptr; - 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() - { - int rc = HALBot::no_update; - if (driveTimer.Get() >= 0.5f) { - driveTimer.Stop(); - driveTimer.Reset(); - timerActive = false; - drivebase->Drive_v(0, 0, 0); //stop - rc = HALBot::timerExpired; - } - return rc; - } - - 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; - strafeLeft = new StrafeLeft; - 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; - delete strafeLeft; - 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); - strafeLeft->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... - stopped->drivebase = drivebase; - 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++] = {stopped, HALBot::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, 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++] = {gettingTote, HALBot::eStop, nullptr, stopped}; - transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, strafeLeft}; - transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; - transitionTable[i++] = {strafeLeft, HALBot::timerExpired, nullptr, rotateDrive}; - transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = {stopped, HALBot::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; - 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/Autonomous.h b/src/Autonomous/Autonomous.h deleted file mode 100644 index ac951c5..0000000 --- a/src/Autonomous/Autonomous.h +++ /dev/null @@ -1,129 +0,0 @@ -# -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; - MecanumDrive* drivebase; - }; - 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(); - }; - class StrafeLeft : public DriveToZone - { - public: - StrafeLeft(); - 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; - StrafeLeft* strafeLeft; - }; -} diff --git a/src/Autonomous/BackAway.cpp b/src/Autonomous/BackAway.cpp new file mode 100644 index 0000000..4e95228 --- /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 HALBot::no_update; + } + + if (grabTimer.Get() >= BACK_AWAY_TIME) + { + drivebase->Drive_v(0, 0, 0); + return HALBot::timerExpired; + } + + if (drivebase != nullptr) + { + drivebase->GoFast(); + drivebase->Drive_v(0, -1, 0); + } + //liftArms->Set(-1); + return HALBot::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..9aabb48 --- /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 (HALBot::getToteCount() < 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 (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; + } +} 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/Autonomous/ForkGrab.cpp b/src/Autonomous/ForkGrab.cpp new file mode 100644 index 0000000..b8b2e73 --- /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 (HALBot::getToteCount() >= 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 HALBot::finish; + } else { + return HALBot::no_update; + } + } else if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.2 && thing) { + //XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); + thing = false; + } + + if (isLiftDown()) + { + //XMLInput::getInstance()->getPGroup("liftArms")->Set(0); + 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, 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 (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; + } +} 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..9722117 --- /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 HALBot::timerExpired; + } + if (timerActive) + { + //Keep sucking a tote in + if (HALBot::getToteCount() >= 2) + intake->Set(-1); + else + 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 + intakeArms->Set(-1); + + drivebase->Drive_v(0, -0.65, 0); + if (HALBot::getToteCount() >= 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 HALBot::eStop; + } + + return HALBot::no_update; + } +} diff --git a/src/Autonomous/GettingTote.h b/src/Autonomous/GettingTote.h new file mode 100644 index 0000000..0bb9026 --- /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; + }; +} \ No newline at end of file diff --git a/src/Autonomous/HALBot.cpp b/src/Autonomous/HALBot.cpp new file mode 100644 index 0000000..f6c9509 --- /dev/null +++ b/src/Autonomous/HALBot.cpp @@ -0,0 +1,178 @@ +#include "HALBot.h" + +namespace dreadbot +{ + 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; + strafeLeft = new StrafeLeft; + 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; + delete strafeLeft; + toteCount = 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::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 + 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++] = {stopped, HALBot::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, 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++] = {gettingTote, HALBot::eStop, nullptr, stopped}; + transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, strafeLeft}; + transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; + transitionTable[i++] = {strafeLeft, HALBot::timerExpired, nullptr, rotateDrive}; + transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, stopped}; + transitionTable[i++] = {stopped, HALBot::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; + 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/HALBot.h b/src/Autonomous/HALBot.h new file mode 100644 index 0000000..5df7f62 --- /dev/null +++ b/src/Autonomous/HALBot.h @@ -0,0 +1,38 @@ +namespace dreadbot +{ + //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. + Log* sysLog; + + 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; + StrafeLeft* strafeLeft; + }; +} diff --git a/src/Autonomous/PushContainer.cpp b/src/Autonomous/PushContainer.cpp new file mode 100644 index 0000000..b37914c --- /dev/null +++ b/src/Autonomous/PushContainer.cpp @@ -0,0 +1,44 @@ +#include "PushContainer.h" + +namespace dreadbot +{ + 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.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 HALBot::timerExpired; + } + if (HALBot::getToteCount() >= 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 HALBot::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.h b/src/Autonomous/RoboState.h index 44ad9ec..3ddd6e1 100644 --- a/src/Autonomous/RoboState.h +++ b/src/Autonomous/RoboState.h @@ -7,6 +7,7 @@ #include "FSM.h" #include "DreadbotDIO.h" #include "../../lib/Logger.h" +#include "HALBot.h" //Ugh - needed for now using namespace Hydra; /******************* @@ -48,7 +49,7 @@ namespace dreadbot virtual ~RoboState() {} protected: - //Hardware for access everywhere + //Hardware for access for all states static MecanumDrive* drivebase; static MotorGrouping* intake; static PneumaticGrouping* lift; diff --git a/src/Autonomous/Rotate.cpp b/src/Autonomous/Rotate.cpp new file mode 100644 index 0000000..54326d4 --- /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 (HALBot::getToteCount() == 3) + lift->Set(-1); //Lower lift + return HALBot::timerExpired; + } + if (drivebase != nullptr) + drivebase->Drive_v(0, 0, 0.5 * rotateConstant); + return HALBot::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..8424cdf --- /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 (HALBot::getToteCount() == 3) + lift->Set(-1); //Lower lift + + Wait(ROTATE_DRIVE_STRAIGHT); + + return HALBot::timerExpired; + } + if (drivebase != nullptr) + drivebase->Drive_v(0, RD_DRIVE_SPEED, RD_ROTATE_SPEED); + return HALBot::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..f5ad0d0 --- /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 (HALBot::getToteCount() == 3) + 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. + } +} diff --git a/src/Autonomous/Stopped.h b/src/Autonomous/Stopped.h new file mode 100644 index 0000000..22b8800 --- /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(); + }; +} \ No newline at end of file diff --git a/src/Autonomous/StrafeLeft.cpp b/src/Autonomous/StrafeLeft.cpp new file mode 100644 index 0000000..c60384d --- /dev/null +++ b/src/Autonomous/StrafeLeft.cpp @@ -0,0 +1,30 @@ +#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() + { + int rc = HALBot::no_update; + if (driveTimer.Get() >= 0.5f) + { + driveTimer.Stop(); + driveTimer.Reset(); + timerActive = false; + drivebase->Drive_v(0, 0, 0); //stop + return HALBot::timerExpired; + } + return HALBot::no_update; + } +} diff --git a/src/Autonomous/StrafeLeft.h b/src/Autonomous/StrafeLeft.h new file mode 100644 index 0000000..a3e85c4 --- /dev/null +++ b/src/Autonomous/StrafeLeft.h @@ -0,0 +1,14 @@ +#pragma once + +#include "RoboState.h" + +namespace dreadbot +{ + class StrafeLeft : public RoboState + { + public: + StrafeLeft(); + int update(); + void enter(); + }; +} From 4e44c42c5f2414fd6d58583ead14286f74775e2e Mon Sep 17 00:00:00 2001 From: Sourec Date: Mon, 4 May 2015 12:50:25 -0400 Subject: [PATCH 26/32] Fixed a load of #includes, added liftArms static member for RoboState. --- src/Autonomous/HALBot.cpp | 2 ++ src/Autonomous/HALBot.h | 16 ++++++++++++++++ src/Autonomous/RoboState.h | 7 ++++--- src/Robot.cpp | 2 +- 4 files changed, 23 insertions(+), 4 deletions(-) diff --git a/src/Autonomous/HALBot.cpp b/src/Autonomous/HALBot.cpp index f6c9509..5e9fe94 100644 --- a/src/Autonomous/HALBot.cpp +++ b/src/Autonomous/HALBot.cpp @@ -32,6 +32,7 @@ namespace dreadbot } HALBot::HALBot() { + sysLog = nullptr; stopped = new Stopped; gettingTote = new GettingTote; driveToZone = new DriveToZone; @@ -66,6 +67,7 @@ namespace dreadbot 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); diff --git a/src/Autonomous/HALBot.h b/src/Autonomous/HALBot.h index 5df7f62..211b1dc 100644 --- a/src/Autonomous/HALBot.h +++ b/src/Autonomous/HALBot.h @@ -1,3 +1,19 @@ +#pragma once + +#include "../../lib/Logger.h" +#include "RoboState.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" + namespace dreadbot { //Needs to appear in https://en.wikipedia.org/wiki/Kludge#Computer_science diff --git a/src/Autonomous/RoboState.h b/src/Autonomous/RoboState.h index 3ddd6e1..8ce9ad3 100644 --- a/src/Autonomous/RoboState.h +++ b/src/Autonomous/RoboState.h @@ -2,10 +2,10 @@ #include "WPILib.h" #include "Timer.h" -#include "MecanumDrive.h" -#include "XMLInput.h" +#include "../MecanumDrive.h" +#include "../XMLInput.h" #include "FSM.h" -#include "DreadbotDIO.h" +#include "../DreadbotDIO.h" #include "../../lib/Logger.h" #include "HALBot.h" //Ugh - needed for now using namespace Hydra; @@ -53,6 +53,7 @@ namespace dreadbot static MecanumDrive* drivebase; static MotorGrouping* intake; static PneumaticGrouping* lift; + static PneumaticGrouping* liftArms; static PneumaticGrouping* intakeArms; static Talon* pusher1; static Talon* pusher2; diff --git a/src/Robot.cpp b/src/Robot.cpp index 60087fe..04393b6 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -1,7 +1,7 @@ #include #include "MecanumDrive.h" #include "XMLInput.h" -#include "Autonomous.h" +#include "Autonomous/HALBot.h" #include "Robot.h" #include "DreadbotDIO.h" #include "../lib/Logger.h" From 3c254943609b0160bacdde06da9418846b1cfc6d Mon Sep 17 00:00:00 2001 From: Sourec Date: Mon, 4 May 2015 13:58:10 -0400 Subject: [PATCH 27/32] Fixes (still doesn't compile!) -StrafeLeft inherits/includes the right thing -Removed a no-multiple-return kludge -Fixed new line at end of file problems -Fixed namespace name capitalization issue -Removed unneeded WPILib.h include in FSM.h --- src/Autonomous/FSM.h | 2 -- src/Autonomous/HALBot.h | 3 ++- src/Autonomous/RoboState.cpp | 14 ++++++++++++++ src/Autonomous/Rotate.cpp | 2 +- src/Autonomous/Stopped.h | 2 +- src/Autonomous/StrafeLeft.cpp | 1 - src/Autonomous/StrafeLeft.h | 4 ++-- 7 files changed, 20 insertions(+), 8 deletions(-) create mode 100644 src/Autonomous/RoboState.cpp diff --git a/src/Autonomous/FSM.h b/src/Autonomous/FSM.h index 916341d..cc3c567 100644 --- a/src/Autonomous/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/HALBot.h b/src/Autonomous/HALBot.h index 211b1dc..f154ef2 100644 --- a/src/Autonomous/HALBot.h +++ b/src/Autonomous/HALBot.h @@ -13,6 +13,7 @@ #include "RotateDrive.h" #include "Stopped.h" #include "StrafeLeft.h" +using namespace Hydra; namespace dreadbot { @@ -40,7 +41,7 @@ namespace dreadbot FSMTransition transitionTable[15]; //The transition table used for transitioning. Changes based on the setMode thingy. //State objects. These should be self-explanatory. - GettingTote* gettingTote; + GettingTote* gettingTote; DriveToZone* driveToZone; ForkGrab* forkGrab; Rotate* rotate; diff --git a/src/Autonomous/RoboState.cpp b/src/Autonomous/RoboState.cpp new file mode 100644 index 0000000..855f58f --- /dev/null +++ b/src/Autonomous/RoboState.cpp @@ -0,0 +1,14 @@ +#include "RoboState.h" + +namespace dreadbot +{ + //These are needed for static members... ugh. 1.5 hours. + MecanumDrive* RoboState::drivebase; + MotorGrouping* RoboState::intake; + PneumaticGrouping* RoboState::lift; + PneumaticGrouping* RoboState::liftArms; + PneumaticGrouping* RoboState::intakeArms; + Talon* RoboState::pusher1; + Talon* RoboState::pusher2; + Log* RoboState::sysLog; +} diff --git a/src/Autonomous/Rotate.cpp b/src/Autonomous/Rotate.cpp index 54326d4..e4cf795 100644 --- a/src/Autonomous/Rotate.cpp +++ b/src/Autonomous/Rotate.cpp @@ -1,6 +1,6 @@ #include "Rotate.h" -namespace Dreadbot +namespace dreadbot { Rotate::Rotate() { diff --git a/src/Autonomous/Stopped.h b/src/Autonomous/Stopped.h index 22b8800..3b3da90 100644 --- a/src/Autonomous/Stopped.h +++ b/src/Autonomous/Stopped.h @@ -10,4 +10,4 @@ namespace dreadbot virtual void enter(); virtual int update(); }; -} \ No newline at end of file +} diff --git a/src/Autonomous/StrafeLeft.cpp b/src/Autonomous/StrafeLeft.cpp index c60384d..91b8044 100644 --- a/src/Autonomous/StrafeLeft.cpp +++ b/src/Autonomous/StrafeLeft.cpp @@ -16,7 +16,6 @@ namespace dreadbot } int StrafeLeft::update() { - int rc = HALBot::no_update; if (driveTimer.Get() >= 0.5f) { driveTimer.Stop(); diff --git a/src/Autonomous/StrafeLeft.h b/src/Autonomous/StrafeLeft.h index a3e85c4..65c4101 100644 --- a/src/Autonomous/StrafeLeft.h +++ b/src/Autonomous/StrafeLeft.h @@ -1,10 +1,10 @@ #pragma once -#include "RoboState.h" +#include "DriveToZone.h" namespace dreadbot { - class StrafeLeft : public RoboState + class StrafeLeft : public DriveToZone { public: StrafeLeft(); From ca448c5284e1e4b218542c6074fdad5e6e4e944e Mon Sep 17 00:00:00 2001 From: Sourec Date: Tue, 5 May 2015 11:08:50 -0400 Subject: [PATCH 28/32] Vanquished all HALBot static member function kludges, RoboState now defines return codes There was a slightly disguised circular dependancy - HALBot needed RoboState but RoboState's subclasses (terminology?) needed HALBot. This gave both me and the compiler a headache. --- src/Autonomous/BackAway.cpp | 6 +-- src/Autonomous/DriveToZone.cpp | 8 +-- src/Autonomous/ForkGrab.cpp | 22 ++++---- src/Autonomous/GettingTote.cpp | 14 ++--- src/Autonomous/GettingTote.h | 2 +- src/Autonomous/HALBot.cpp | 93 ++++++++++++-------------------- src/Autonomous/HALBot.h | 11 ++-- src/Autonomous/PushContainer.cpp | 9 ++-- src/Autonomous/RoboState.cpp | 19 ++++--- src/Autonomous/RoboState.h | 11 ++-- src/Autonomous/Rotate.cpp | 6 +-- src/Autonomous/RotateDrive.cpp | 6 +-- src/Autonomous/Stopped.cpp | 4 +- 13 files changed, 93 insertions(+), 118 deletions(-) diff --git a/src/Autonomous/BackAway.cpp b/src/Autonomous/BackAway.cpp index 4e95228..59c1c35 100644 --- a/src/Autonomous/BackAway.cpp +++ b/src/Autonomous/BackAway.cpp @@ -24,13 +24,13 @@ namespace dreadbot grabTimer.Reset(); grabTimer.Start(); } - return HALBot::no_update; + return RoboState::no_update; } if (grabTimer.Get() >= BACK_AWAY_TIME) { drivebase->Drive_v(0, 0, 0); - return HALBot::timerExpired; + return RoboState::timerExpired; } if (drivebase != nullptr) @@ -39,6 +39,6 @@ namespace dreadbot drivebase->Drive_v(0, -1, 0); } //liftArms->Set(-1); - return HALBot::no_update; + return RoboState::no_update; } } diff --git a/src/Autonomous/DriveToZone.cpp b/src/Autonomous/DriveToZone.cpp index 9aabb48..930ce99 100644 --- a/src/Autonomous/DriveToZone.cpp +++ b/src/Autonomous/DriveToZone.cpp @@ -13,14 +13,14 @@ namespace dreadbot driveTimer.Reset(); driveTimer.Start(); timerActive = true; - if (HALBot::getToteCount() < 3) + 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 (HALBot::getToteCount() == 0) + 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. @@ -30,7 +30,7 @@ namespace dreadbot driveTimer.Reset(); timerActive = false; drivebase->Drive_v(0, 0, 0); - return HALBot::timerExpired; + return RoboState::timerExpired; } //Apply actual velocity changes @@ -42,6 +42,6 @@ namespace dreadbot drivebase->Drive_v(0, 0.8 * dir, 0); // Do a short dance } - return HALBot::no_update; + return RoboState::no_update; } } diff --git a/src/Autonomous/ForkGrab.cpp b/src/Autonomous/ForkGrab.cpp index b8b2e73..59a2161 100644 --- a/src/Autonomous/ForkGrab.cpp +++ b/src/Autonomous/ForkGrab.cpp @@ -15,7 +15,7 @@ namespace dreadbot bool thing = true; //PARKER, NO! int ForkGrab::update() { - if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= LIFT_ENGAGEMENT_DELAY) + if (RoboState::toteCount >= 2 && grabTimer.Get() >= LIFT_ENGAGEMENT_DELAY) { intakeArms->Set(1); drivebase->GoFast(); @@ -26,11 +26,11 @@ namespace dreadbot lift->Set(1); //Raise lift Wait(0.3); //Totes must engage first lift->Set(0); - return HALBot::finish; + return RoboState::finish; } else { - return HALBot::no_update; + return RoboState::no_update; } - } else if (HALBot::getToteCount() >= 2 && grabTimer.Get() >= 0.2 && thing) { + } else if (RoboState::toteCount >= 2 && grabTimer.Get() >= 0.2 && thing) { //XMLInput::getInstance()->getPGroup("liftArms")->Set(-1); thing = false; } @@ -38,15 +38,15 @@ namespace dreadbot if (isLiftDown()) { //XMLInput::getInstance()->getPGroup("liftArms")->Set(0); - HALBot::incrTote(); //Once the lift is down, it is assumed that the tote is actually collected, i.e. in the fork. + 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 (HALBot::getToteCount() >= 3 && HALBot::enoughTotes()) + if (RoboState::toteCount >= 3 && RoboState::toteCount >= RoboState::neededTCount) { lift->Set(1); //Raise lift Wait(0.25); //Totes must engage first lift->Set(0); - return HALBot::finish; + return RoboState::finish; } //Raise the lift and cheat to alight the tote drivebase->GoFast(); @@ -56,14 +56,14 @@ namespace dreadbot drivebase->GoSlow(); lift->Set(1); Wait(0.3); - if (HALBot::enoughTotes()) - return HALBot::finish; + if (RoboState::toteCount >= RoboState::neededTCount) + return RoboState::finish; else - return HALBot::nextTote; + return RoboState::nextTote; } drivebase->Drive_v(0, 0, 0); if (lift != nullptr) lift->Set(-1); //Lower the lift for grabbing - return HALBot::no_update; + return RoboState::no_update; } } diff --git a/src/Autonomous/GettingTote.cpp b/src/Autonomous/GettingTote.cpp index 9722117..2a483f2 100644 --- a/src/Autonomous/GettingTote.cpp +++ b/src/Autonomous/GettingTote.cpp @@ -27,23 +27,23 @@ namespace dreadbot timerActive = false; eStopTimer.Stop(); eStopTimer.Reset(); - return HALBot::timerExpired; + return RoboState::timerExpired; } if (timerActive) { //Keep sucking a tote in - if (HALBot::getToteCount() >= 2) + if (RoboState::toteCount >= 2) intake->Set(-1); else intake->Set(-0.5); - return HALBot::no_update; + return RoboState::no_update; } - if (HALBot::getToteCount() != 0) //Open the intake arms for grabbing totes after the first tote is collected + 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 (HALBot::getToteCount() >= 2) + if (RoboState::toteCount >= 2) intake->Set(-1); else intake->Set(-0.6); @@ -62,9 +62,9 @@ namespace dreadbot drivebase->Drive_v(0, 0, 0);intake->Set(0); lift->Set(1); // Raise the lift sysLog->log("Emergency-stopped in GettingTote", Hydra::error); - return HALBot::eStop; + return RoboState::eStop; } - return HALBot::no_update; + return RoboState::no_update; } } diff --git a/src/Autonomous/GettingTote.h b/src/Autonomous/GettingTote.h index 0bb9026..29e82da 100644 --- a/src/Autonomous/GettingTote.h +++ b/src/Autonomous/GettingTote.h @@ -15,4 +15,4 @@ namespace dreadbot Timer getTimer; Timer eStopTimer; }; -} \ No newline at end of file +} diff --git a/src/Autonomous/HALBot.cpp b/src/Autonomous/HALBot.cpp index 5e9fe94..73b6b06 100644 --- a/src/Autonomous/HALBot.cpp +++ b/src/Autonomous/HALBot.cpp @@ -2,34 +2,6 @@ namespace dreadbot { - 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() { sysLog = nullptr; @@ -58,7 +30,8 @@ namespace dreadbot delete backAway; delete fsm; delete strafeLeft; - toteCount = 0; + RoboState::toteCount = 0; + RoboState::neededTCount = 1; } void HALBot::init(MecanumDrive* drivebase, MotorGrouping* intake, PneumaticGrouping* lift) { @@ -79,7 +52,7 @@ namespace dreadbot if (mode == AUTON_MODE_STOP) { i = 0; - transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; + transitionTable[i++] = {stopped, RoboState::no_update, nullptr, stopped}; transitionTable[i++] = END_STATE_TABLE; defState = stopped; } @@ -88,9 +61,9 @@ namespace dreadbot 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++] = {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; } @@ -99,13 +72,13 @@ namespace dreadbot 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++] = {stopped, HALBot::no_update, nullptr, stopped}; + 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; } @@ -114,14 +87,14 @@ namespace dreadbot driveToZone->strafe = true; i = 0; rotate->rotateConstant = -1; - transitionTable[i++] = {rotate, HALBot::timerExpired, nullptr, driveToZone}; - transitionTable[i++] = {driveToZone, HALBot::timerExpired, nullptr, stopped}; + transitionTable[i++] = {rotate, RoboState::timerExpired, nullptr, driveToZone}; + transitionTable[i++] = {driveToZone, RoboState::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++] = {stopped, RoboState::no_update, nullptr, stopped}; transitionTable[i++] = END_STATE_TABLE; defState = stopped; } @@ -132,16 +105,16 @@ namespace dreadbot pushContainer->pushConstant = -1; pushContainer->enableScaling = true; driveToZone->strafe = false; - incrTote(); //We already have a tote + RoboState::toteCount++; //We already have a tote - transitionTable[i++] = {pushContainer, HALBot::timerExpired, nullptr, gettingTote}; - transitionTable[i++] = {gettingTote, HALBot::timerExpired, nullptr, forkGrab}; - transitionTable[i++] = {gettingTote, HALBot::eStop, nullptr, stopped}; - transitionTable[i++] = {forkGrab, HALBot::finish, nullptr, strafeLeft}; - transitionTable[i++] = {forkGrab, HALBot::nextTote, nullptr, pushContainer}; - transitionTable[i++] = {strafeLeft, HALBot::timerExpired, nullptr, rotateDrive}; - transitionTable[i++] = {rotateDrive, HALBot::timerExpired, nullptr, stopped}; - transitionTable[i++] = {stopped, HALBot::no_update, nullptr, stopped}; + 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) @@ -151,15 +124,15 @@ namespace dreadbot pushContainer->pushConstant = -1; pushContainer->enableScaling = true; driveToZone->strafe = false; - incrTote(); //We already have a tote + RoboState::toteCount++; //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}; + 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; } diff --git a/src/Autonomous/HALBot.h b/src/Autonomous/HALBot.h index f154ef2..06bc41f 100644 --- a/src/Autonomous/HALBot.h +++ b/src/Autonomous/HALBot.h @@ -1,7 +1,6 @@ #pragma once #include "../../lib/Logger.h" -#include "RoboState.h" //All states #include "BackAway.h" @@ -13,6 +12,8 @@ #include "RotateDrive.h" #include "Stopped.h" #include "StrafeLeft.h" +#include "RoboState.h" + using namespace Hydra; namespace dreadbot @@ -21,21 +22,15 @@ namespace dreadbot 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. + AutonMode mode; //The mode that the robot is in. Also dumb. Log* sysLog; FSMTransition transitionTable[15]; //The transition table used for transitioning. Changes based on the setMode thingy. diff --git a/src/Autonomous/PushContainer.cpp b/src/Autonomous/PushContainer.cpp index b37914c..e917968 100644 --- a/src/Autonomous/PushContainer.cpp +++ b/src/Autonomous/PushContainer.cpp @@ -5,6 +5,7 @@ namespace dreadbot PushContainer::PushContainer() { enableScaling = false; + pushConstant = 1; //OK, since } void PushContainer::enter() { @@ -15,7 +16,7 @@ namespace dreadbot { 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.f) / 3.f; //Scaling for three-tote autonomous, since the second container is farther away than the first + 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) { @@ -28,9 +29,9 @@ namespace dreadbot { timerActive = false; drivebase->Drive_v(0, 0, 0); - return HALBot::timerExpired; + return RoboState::timerExpired; } - if (HALBot::getToteCount() >= 2) { + 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); @@ -39,6 +40,6 @@ namespace dreadbot pusher1->Set(INTAKE_PUSH_SPEED); //Push the container? if (pusher2 != nullptr) pusher2->Set(INTAKE_PUSH_SPEED); - return HALBot::no_update; + return RoboState::no_update; } } diff --git a/src/Autonomous/RoboState.cpp b/src/Autonomous/RoboState.cpp index 855f58f..b77ae91 100644 --- a/src/Autonomous/RoboState.cpp +++ b/src/Autonomous/RoboState.cpp @@ -3,12 +3,15 @@ namespace dreadbot { //These are needed for static members... ugh. 1.5 hours. - MecanumDrive* RoboState::drivebase; - MotorGrouping* RoboState::intake; - PneumaticGrouping* RoboState::lift; - PneumaticGrouping* RoboState::liftArms; - PneumaticGrouping* RoboState::intakeArms; - Talon* RoboState::pusher1; - Talon* RoboState::pusher2; - Log* RoboState::sysLog; + 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 index 8ce9ad3..af5f925 100644 --- a/src/Autonomous/RoboState.h +++ b/src/Autonomous/RoboState.h @@ -7,13 +7,11 @@ #include "FSM.h" #include "../DreadbotDIO.h" #include "../../lib/Logger.h" -#include "HALBot.h" //Ugh - needed for now 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 @@ -39,15 +37,17 @@ using namespace Hydra; #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; @@ -59,6 +59,9 @@ namespace dreadbot static Talon* pusher2; static Log* sysLog; - friend class HALBot; //Lets HALBot set values without need for getters/setters + 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 index e4cf795..25457d5 100644 --- a/src/Autonomous/Rotate.cpp +++ b/src/Autonomous/Rotate.cpp @@ -20,12 +20,12 @@ namespace dreadbot { //Rotated far enough; break timerActive = false; drivebase->Drive_v(0, 0, 0); - if (HALBot::getToteCount() == 3) + if (RoboState::toteCount == 3) lift->Set(-1); //Lower lift - return HALBot::timerExpired; + return RoboState::timerExpired; } if (drivebase != nullptr) drivebase->Drive_v(0, 0, 0.5 * rotateConstant); - return HALBot::no_update; + return RoboState::no_update; } } diff --git a/src/Autonomous/RotateDrive.cpp b/src/Autonomous/RotateDrive.cpp index 8424cdf..255be6c 100644 --- a/src/Autonomous/RotateDrive.cpp +++ b/src/Autonomous/RotateDrive.cpp @@ -23,15 +23,15 @@ namespace dreadbot drivebase->Drive_v(0, 1, 0); // The stack is lowered prior to stopping in order to decelerate properly - if (HALBot::getToteCount() == 3) + if (RoboState::toteCount == 3) lift->Set(-1); //Lower lift Wait(ROTATE_DRIVE_STRAIGHT); - return HALBot::timerExpired; + return RoboState::timerExpired; } if (drivebase != nullptr) drivebase->Drive_v(0, RD_DRIVE_SPEED, RD_ROTATE_SPEED); - return HALBot::no_update; + return RoboState::no_update; } } diff --git a/src/Autonomous/Stopped.cpp b/src/Autonomous/Stopped.cpp index f5ad0d0..dd283e6 100644 --- a/src/Autonomous/Stopped.cpp +++ b/src/Autonomous/Stopped.cpp @@ -7,11 +7,11 @@ namespace dreadbot sysLog->log("State: Stopped"); if (drivebase != nullptr) drivebase->Drive_v(0, 0, 0); - if (HALBot::getToteCount() == 3) + 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 HALBot::no_update; //Does nothing of significance. + return 0;//HALBot::no_update; //Does nothing of significance. } } From 1e6062e6511fa639e87b0bc8acedeb74a0322cf4 Mon Sep 17 00:00:00 2001 From: Sourec Date: Tue, 5 May 2015 11:09:26 -0400 Subject: [PATCH 29/32] Fixed an invalid return code (used the old HALBot-defined return code) --- src/Autonomous/StrafeLeft.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Autonomous/StrafeLeft.cpp b/src/Autonomous/StrafeLeft.cpp index 91b8044..f07dc5d 100644 --- a/src/Autonomous/StrafeLeft.cpp +++ b/src/Autonomous/StrafeLeft.cpp @@ -22,8 +22,8 @@ namespace dreadbot driveTimer.Reset(); timerActive = false; drivebase->Drive_v(0, 0, 0); //stop - return HALBot::timerExpired; + return RoboState::timerExpired; } - return HALBot::no_update; + return RoboState::no_update; } } From 3a85eb013ba4725ccc189380b5b1e7ec1f113649 Mon Sep 17 00:00:00 2001 From: Sourec Date: Tue, 5 May 2015 11:13:00 -0400 Subject: [PATCH 30/32] Tote count comparisons now work properly. --- src/Autonomous/HALBot.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/Autonomous/HALBot.cpp b/src/Autonomous/HALBot.cpp index 73b6b06..1c70340 100644 --- a/src/Autonomous/HALBot.cpp +++ b/src/Autonomous/HALBot.cpp @@ -31,7 +31,7 @@ namespace dreadbot delete fsm; delete strafeLeft; RoboState::toteCount = 0; - RoboState::neededTCount = 1; + RoboState::neededTCount = 0; } void HALBot::init(MecanumDrive* drivebase, MotorGrouping* intake, PneumaticGrouping* lift) { @@ -47,7 +47,7 @@ namespace dreadbot RoboState::sysLog = sysLog; pushContainer->pushConstant = 1; - //Apply state tables and set the starting state + //Apply state tables and set the starting state. Note that RoboState::neededTCount is 0 before this. FSMState* defState = nullptr; if (mode == AUTON_MODE_STOP) { @@ -70,6 +70,7 @@ namespace dreadbot if (mode == AUTON_MODE_TOTE) { i = 0; + RoboState::neededTCount = 1; rotate->rotateConstant = 1; driveToZone->strafe = false; transitionTable[i++] = {gettingTote, RoboState::timerExpired, nullptr, forkGrab}; @@ -93,7 +94,9 @@ namespace dreadbot } 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; @@ -106,6 +109,7 @@ namespace dreadbot 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}; @@ -125,6 +129,7 @@ namespace dreadbot 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}; From 2558a150268032d19eed19c82a3ac958fcb0706a Mon Sep 17 00:00:00 2001 From: Sourec Date: Sat, 9 May 2015 09:32:15 -0400 Subject: [PATCH 31/32] HALBot no longer has absurd get/setMode() functions HALBot now directly polls the GetAutonMode function on its own, and so does the regular robot class, eliminating the need for get/set mode functions. --- .gitignore | 1 + src/Autonomous/HALBot.cpp | 10 +--------- src/Autonomous/HALBot.h | 8 +++----- src/Robot.cpp | 6 ++---- 4 files changed, 7 insertions(+), 18 deletions(-) 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/src/Autonomous/HALBot.cpp b/src/Autonomous/HALBot.cpp index 1c70340..4850349 100644 --- a/src/Autonomous/HALBot.cpp +++ b/src/Autonomous/HALBot.cpp @@ -16,7 +16,6 @@ namespace dreadbot fsm = new FiniteStateMachine; rotateDrive = new RotateDrive; strafeLeft = new StrafeLeft; - mode = AUTON_MODE_DRIVE; //Just drive straight forward. Assumes a spherical cow. } HALBot::~HALBot() { @@ -49,6 +48,7 @@ namespace dreadbot //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; @@ -147,12 +147,4 @@ namespace dreadbot { 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/HALBot.h b/src/Autonomous/HALBot.h index 06bc41f..280705d 100644 --- a/src/Autonomous/HALBot.h +++ b/src/Autonomous/HALBot.h @@ -1,6 +1,7 @@ #pragma once #include "../../lib/Logger.h" +#include "../DreadbotDIO.h" //All states #include "BackAway.h" @@ -24,16 +25,13 @@ namespace dreadbot public: HALBot(); ~HALBot(); - 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 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; - AutonMode mode; //The mode that the robot is in. Also dumb. - Log* sysLog; - 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; diff --git a/src/Robot.cpp b/src/Robot.cpp index 04393b6..9cf5999 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -3,13 +3,12 @@ #include "XMLInput.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; @@ -94,12 +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 (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); From d0d58f673538ac727566c53eb696b705dd9ef268 Mon Sep 17 00:00:00 2001 From: Sourec Date: Fri, 15 May 2015 18:15:47 -0400 Subject: [PATCH 32/32] Added a slider for SFX dashboard that allows speed tweaking --- src/MecanumDrive.cpp | 2 +- src/Robot.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MecanumDrive.cpp b/src/MecanumDrive.cpp index 1d0a1f2..5585d2f 100644 --- a/src/MecanumDrive.cpp +++ b/src/MecanumDrive.cpp @@ -95,7 +95,7 @@ void MecanumDrive::Drive_v(double x, double y, double rotation) { } */ for (uint8_t i = 0; i < MOTOR_COUNT; ++i) { - motors[i]->Set(wspeeds[i]*motorReversals[i]*1023, syncGroup); + motors[i]->Set(wspeeds[i]*motorReversals[i]*SmartDashboard::GetNumber("Speed", 1023.f), syncGroup); } } diff --git a/src/Robot.cpp b/src/Robot.cpp index 9cf5999..05e1030 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -113,7 +113,7 @@ namespace dreadbot { sysLog->log("Initializing Teleop."); GlobalInit(); - drivebase->GoSlow(); + drivebase->GoFast(); } void TeleopPeriodic()