diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index e99d474..f12ace6 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -34,12 +34,17 @@ Autonomous::Autonomous(int seq){ printf("Done"); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75)); AddSequential(new CollectTote()); + AddSequential(new Raise()); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75)); AddSequential(new CollectTote()); + AddSequential(new Lower()); + AddSequential(new Raise()); printf("Three totes?: %d\n",SmartDashboard::GetBoolean("Three totes")); if(SmartDashboard::GetBoolean("Three totes")){ AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75)); AddSequential(new CollectTote()); + AddSequential(new Lower()); + AddSequential(new Raise()); } AddSequential(new Turn(90)); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75)); diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp index 378156d..637a88b 100644 --- a/Commands/Elevator/Lower.cpp +++ b/Commands/Elevator/Lower.cpp @@ -4,7 +4,7 @@ Lower::Lower() : Command("Lower"){ } void Lower::Initialize(){ - SetTimeout(2.5); + SetTimeout(3.0); } void Lower::Execute(){ DentRobot::elevator->Run(-1.0); diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp index 817ed4d..ce42ab8 100644 --- a/Commands/Elevator/Raise.cpp +++ b/Commands/Elevator/Raise.cpp @@ -4,7 +4,7 @@ Raise::Raise() : Command("Raise"){ } void Raise::Initialize(){ - SetTimeout(3.0); + SetTimeout(3.5); } void Raise::Execute(){ DentRobot::elevator->Run(1.0); diff --git a/RobotMap.h b/RobotMap.h index bb163e7..d92e4f1 100644 --- a/RobotMap.h +++ b/RobotMap.h @@ -20,8 +20,8 @@ #define BINELEVATOR_TOP_DIO 8 #define BINELEVATOR_ENCODERA 10 #define BINELEVATOR_ENCODERB 11 -#define BINELEVATOR_SOLDENOID_ONE 1 -#define BINELEVATOR_SOLDENOID_TWO 2 +#define BINELEVATOR_SOLDENOID_ONE 6 +#define BINELEVATOR_SOLDENOID_TWO 7 // Drivetrain #define DRIVE_FRONT_LEFT_CAN 2