diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index 5d50896..5eed551 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -20,15 +20,16 @@ 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"), -.8,0)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.8,0.0)); break; case 2: // Wait a desigated value, drive to Auto Zone (TM) Wait(SmartDashboard::GetNumber("Auto Wait Time")); + AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); 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)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"),0.75,0.0)); + //AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0,-1.0)); + AddSequential(new BinLower(1.0)); break; case 3: // Collect three totes, drive to Auto Zone (TM) diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp index 27b9e93..508eb1e 100644 --- a/Commands/Autonomous/Turn.cpp +++ b/Commands/Autonomous/Turn.cpp @@ -3,14 +3,13 @@ // Drive for a short while then stop. Just for testing Turn::Turn(int k) : Command("Turn"){ Requires(DentRobot::drivetrain); - degrees=k; - SetTimeout(0.85); + SetTimeout(k); } void Turn::Initialize(){ } void Turn::Execute(){ //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro - DentRobot::drivetrain->DriveMecanum(0.0,0.0,1.0,0.9,0.0); + DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.65,0.9,0.0); } bool Turn::IsFinished(){ return IsTimedOut(); diff --git a/DentRobot.cpp b/DentRobot.cpp index 960383d..47fc043 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -26,15 +26,16 @@ void DentRobot::RobotInit(){ SmartDashboard::PutNumber("CodeVersion",CODE_VERSION); // Autonomous // Sequence of autonomous command - SmartDashboard::PutNumber("Auto Sequence",1.0); - SmartDashboard::PutNumber("Auto Wait Time",1.0); + SmartDashboard::PutNumber("Auto Sequence",2.0); + SmartDashboard::PutNumber("Auto Wait Time",3.0); // If the robot will be picking up three totes in sequence 3 SmartDashboard::PutBoolean("Three totes", true); // TODO: Calibrate the following two values // Distance (in time) to auto zone - SmartDashboard::PutNumber("Auto Zone Distance", 4.2); + SmartDashboard::PutNumber("Auto Zone Distance", 3.0); // Distance (in time) to auto tote (used in sequence 3) SmartDashboard::PutNumber("Auto Tote Distance", 0.5); + SmartDashboard::PutNumber("TurnAmount",1.6); // Elevators SmartDashboard::PutBoolean("Bin Elevator Bottom", false); diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp index 1cfe13e..bb26991 100644 --- a/Subsystems/Elevator.cpp +++ b/Subsystems/Elevator.cpp @@ -26,14 +26,14 @@ double Elevator::GetHeight(){ return elevatorEncoder->Get(); } bool Elevator::GetElevatorBottom(){ - SmartDashboard::PutBoolean("Elevator Bottom", elevatorBottom->Get()); + SmartDashboard::PutBoolean("Elevator Bottom", !elevatorBottom->Get()); return elevatorBottom->Get(); } bool Elevator::GetElevatorMiddle(){ return elevatorMiddle->Get(); } bool Elevator::GetElevatorTop(){ - SmartDashboard::PutBoolean("Elevator Top", elevatorTop->Get()); + SmartDashboard::PutBoolean("Elevator Top", !elevatorTop->Get()); return elevatorTop->Get(); } void Elevator::SetUseEncoder(bool param){