diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index f45f300..454a239 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -5,6 +5,8 @@ #include "../Elevator/Lower.h" #include "../BinElevator/BinRaise.h" #include "../BinElevator/BinLower.h" +#include "../BinCollector/BinCloseClaw.h" +#include "../BinCollector/BinOpenClaw.h" #include "AutoDrive.h" #include "Turn.h" #include "../Collector/RollIn.h" @@ -25,11 +27,12 @@ Autonomous::Autonomous(int seq) { AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); break; case 2: - //Collect a bin upright into the robot upright, turn, drive to Auto Zone (TM),turn - AddSequential(new RollIn(1.0)); - AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75)); - AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); + //Collect a bin upright into the robot upright and raise elevator + AddSequential(new BinOpenClaw()); + AddSequential(new CollectTote()); + AddSequential(new BinCloseClaw()); + AddParallel(new AutoDrive(SmartDashboard::GetNumber("DriveTime2"), 0.0, 0.75, 0, false)); + AddSequential(new Raise(2.5,false,-1)); break; case 3: // Collect one, two, or three totes, drive to Auto Zone (TM), release totes diff --git a/Commands/Autonomous/CollectTote.cpp b/Commands/Autonomous/CollectTote.cpp index 29a2fd0..917a5e7 100644 --- a/Commands/Autonomous/CollectTote.cpp +++ b/Commands/Autonomous/CollectTote.cpp @@ -3,7 +3,7 @@ #include "AutoDrive.h" #include "../Collector/RollIn.h" CollectTote::CollectTote(double z) { - AddParallel(new AutoDrive(SmartDashboard::GetNumber("DriveTime"), 0.0, 0.75, z, false)); + AddParallel(new AutoDrive(SmartDashboard::GetNumber("DriveTime1"), 0.0, 0.75, z, false)); AddSequential(new RollIn(1.0)); } // vim: ts=2:sw=2:et diff --git a/Commands/Elevator/Lower.h b/Commands/Elevator/Lower.h index c0d063d..051253b 100644 --- a/Commands/Elevator/Lower.h +++ b/Commands/Elevator/Lower.h @@ -21,7 +21,7 @@ class Lower: public Command { * * @param liftSpeed Speed at which to lower the lift */ - Lower(double timeout = 3.0, bool useSoftLimits = true, double liftSpeed=0); + Lower(double timeout = 3.0, bool useSoftLimits = true, double liftSpeed=1); /** * @brief Initializes the class */ diff --git a/Commands/Elevator/Raise.h b/Commands/Elevator/Raise.h index b64517c..aa9ea64 100644 --- a/Commands/Elevator/Raise.h +++ b/Commands/Elevator/Raise.h @@ -21,7 +21,7 @@ class Raise: public Command { * * @param liftSpeed Speed at which to raise the elevator */ - Raise(double timeout = 3.5, bool useSoftLimits = true, double liftSpeed=0); + Raise(double timeout = 3.5, bool useSoftLimits = true, double liftSpeed=-1); /** * @brief Initializes the class */ diff --git a/DentRobot.cpp b/DentRobot.cpp index 04987b5..cb1db14 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -29,7 +29,8 @@ void DentRobot::RobotInit() { // Amount to turn while collecting the initial tote in auto 4 SmartDashboard::PutNumber("CollectToteTurn", 0.25); // Amount of time to collect a tote - SmartDashboard::PutNumber("DriveTime", 1.3); + SmartDashboard::PutNumber("DriveTime1", 1.3); + SmartDashboard::PutNumber("DriveTime2", 1.3); // Sequence of autonomous command SmartDashboard::PutNumber("Auto Sequence", -1.0); SmartDashboard::PutNumber("Auto Wait Time", 0.5); diff --git a/Subsystems/Pneumatics.cpp b/Subsystems/Pneumatics.cpp index 086327d..33637bb 100644 --- a/Subsystems/Pneumatics.cpp +++ b/Subsystems/Pneumatics.cpp @@ -16,7 +16,7 @@ void Pneumatics::SetArmsOpen(bool state) { solenoid2->Set(!state); armState=state; } -void Pneumatics::SetClawOpen(bool state){ +void Pneumatics::SetClawOpen(bool state) { solenoid3->Set(state); solenoid4->Set(!state); clawState=state;