diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index 8c4229d..db2a725 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -3,6 +3,8 @@ #include "../../DentRobot.h" #include "../Elevator/Raise.h" #include "../Elevator/Lower.h" +#include "../BinElevator/BinRaise.h" +#include "../BinElevator/BinLower.h" #include "AutoDrive.h" #include "Turn.h" #include "../Collector/RollIn.h" @@ -18,15 +20,15 @@ Autonomous::Autonomous(int seq){ case 1: // Wait a desigated value, drive to Auto Zone (TM) Wait(SmartDashboard::GetNumber("Auto Wait Time")); - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75,0)); + AddSequential(new BinRaise(1.2)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0,1)); + AddSequential(new BinLower(.9)); + //AddSequential(new AutoDrive(1.2, -0.75,0)); break; case 2: - // Get one tote and go to Auto Zone (TM) - AddSequential(new CollectTote()); - AddSequential(new Turn(90)); - AddSequential(new AutoDrive(2.0, 1.0, 0.0)); - AddSequential(new ReleaseTote()); - AddSequential(new AutoDrive(0.5, -1.0, 0.0)); + // Wait a desigated value, drive to Auto Zone (TM) + Wait(SmartDashboard::GetNumber("Auto Wait Time")); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0,1)); break; case 3: // Collect three totes, drive to Auto Zone (TM) diff --git a/Commands/BinElevator/BinLower.cpp b/Commands/BinElevator/BinLower.cpp index b4001d8..ee2bc43 100644 --- a/Commands/BinElevator/BinLower.cpp +++ b/Commands/BinElevator/BinLower.cpp @@ -1,10 +1,11 @@ #include "BinLower.h" #include "../../DentRobot.h" #include "../../OI.h" -BinLower::BinLower() : Command("BinLower"){ +BinLower::BinLower(float t) : Command("BinLower"){ + timeout=t; } void BinLower::Initialize(){ - SetTimeout(2.5); + SetTimeout(timeout); } void BinLower::Execute(){ DentRobot::binElevator->Run(-1.0); diff --git a/Commands/BinElevator/BinLower.h b/Commands/BinElevator/BinLower.h index a3504b4..49813b0 100644 --- a/Commands/BinElevator/BinLower.h +++ b/Commands/BinElevator/BinLower.h @@ -5,8 +5,10 @@ #include "WPILib.h" class BinLower: public Command{ + private: + float timeout; public: - BinLower(); + BinLower(float); void Initialize(); void Execute(); bool IsFinished(); diff --git a/Commands/BinElevator/BinRaise.cpp b/Commands/BinElevator/BinRaise.cpp index 1fe0148..21f46f7 100644 --- a/Commands/BinElevator/BinRaise.cpp +++ b/Commands/BinElevator/BinRaise.cpp @@ -1,10 +1,11 @@ #include "BinRaise.h" #include "../../DentRobot.h" #include "../../OI.h" -BinRaise::BinRaise() : Command("BinRaise"){ +BinRaise::BinRaise(float t) : Command("BinRaise"){ + timeout=t; } void BinRaise::Initialize(){ - SetTimeout(3.0); + SetTimeout(timeout); } void BinRaise::Execute(){ DentRobot::binElevator->Run(1.0); diff --git a/Commands/BinElevator/BinRaise.h b/Commands/BinElevator/BinRaise.h index ea73454..d173430 100644 --- a/Commands/BinElevator/BinRaise.h +++ b/Commands/BinElevator/BinRaise.h @@ -5,8 +5,10 @@ #include "WPILib.h" class BinRaise: public Command{ + private: + float timeout; public: - BinRaise(); + BinRaise(float); void Initialize(); void Execute(); bool IsFinished(); diff --git a/DentRobot.cpp b/DentRobot.cpp index 7795d1e..d9465db 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -32,7 +32,7 @@ void DentRobot::RobotInit(){ SmartDashboard::PutBoolean("Three totes", true); // TODO: Calibrate the following two values // Distance (in time) to auto zone - SmartDashboard::PutNumber("Auto Zone Distance", 1.0); + SmartDashboard::PutNumber("Auto Zone Distance", 5.7); // Distance (in time) to auto tote (used in sequence 3) SmartDashboard::PutNumber("Auto Tote Distance", 0.5); diff --git a/OI.cpp b/OI.cpp index 916216b..3dcfaa2 100644 --- a/OI.cpp +++ b/OI.cpp @@ -35,8 +35,8 @@ OI::OI() { //JoystickButton *right8=new JoystickButton(rightStick, 8); //right7->WhenPressed(new BinOpenArms()); //right8->WhenPressed(new BinCloseArms()); - binRaise=new BinRaise(); - binLower=new BinLower(); + binRaise=new BinRaise(3.0); + binLower=new BinLower(2.0); right3->WhenPressed(binLower); right3->CancelWhenPressed(binRaise); right5->WhenPressed(binRaise);