diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index 9db0e2b..2beda05 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -36,17 +36,17 @@ Autonomous::Autonomous(int seq) { AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn"))); if(SmartDashboard::GetBoolean("Two totes")) { AddParallel(new Turn(0.81)); - AddSequential(new Raise(3.5)); + AddSequential(new Raise(3.5,false,-1)); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75)); AddSequential(new CollectTote()); - AddSequential(new Lower(3.0)); - AddSequential(new Raise(3.5)); + AddSequential(new Lower(3.0,false,1)); + AddSequential(new Raise(3.5,false,-1)); if(SmartDashboard::GetBoolean("Three totes")) { AddSequential(new Turn(3.8)); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Three Tote Distance"), 0.0, 0.75)); AddSequential(new CollectTote()); - AddSequential(new Lower(3.0)); - AddSequential(new Raise(3.5)); + AddSequential(new Lower(3.0,false,1)); + AddSequential(new Raise(3.5,false,-1)); } } AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); @@ -67,10 +67,10 @@ Autonomous::Autonomous(int seq) { break; case 6: //Drive forward and collect a single gray tote - AddSequential(new Lower(3.0)); + AddSequential(new Lower(3.0,false,1)); AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn"))); - AddSequential(new Lower(3.0)); - AddSequential(new Raise(3.5)); + AddSequential(new Lower(3.0,false,1)); + AddSequential(new Raise(3.5,false,-1)); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75)); default: printf("Invalid seq: %d\n", seq); diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp index 0bd9580..042670c 100644 --- a/Commands/Elevator/Lower.cpp +++ b/Commands/Elevator/Lower.cpp @@ -1,20 +1,16 @@ #include "Lower.h" #include "../../DentRobot.h" #include "../../OI.h" -Lower::Lower(double timeout, bool useSoftLimits): Command("Lower") { +Lower::Lower(double timeout, bool useSoftLimits, double liftSpeed): Command("Lower") { SetTimeout(timeout); softLimits=useSoftLimits; + speed=liftSpeed; } void Lower::Initialize() {} void Lower::Execute() { - DentRobot::elevator->Run(DentRobot::oi->GetRightThrottle()*1.0); + DentRobot::elevator->Run(speed); } bool Lower::IsFinished() { -// if(softLimits) { -// if(!DentRobot::elevator->GetElevatorBottom()) { -// return true; -// } -// } if(IsTimedOut()) { return true; } else { diff --git a/Commands/Elevator/Lower.h b/Commands/Elevator/Lower.h index 2472ac4..490392f 100644 --- a/Commands/Elevator/Lower.h +++ b/Commands/Elevator/Lower.h @@ -10,14 +10,16 @@ class Lower: public Command { private: bool softLimits; //Run(DentRobot::oi->GetRightThrottle()*-1.0); + DentRobot::elevator->Run(speed); } bool Raise::IsFinished() { -// if(softLimits) { -// if(!DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle()) { -// return true; -// } -// } if(IsTimedOut()) { return true; } else { @@ -22,10 +18,6 @@ bool Raise::IsFinished() { } } void Raise::End() { - // If the elevator is at the top -// if(DentRobot::elevator->GetElevatorTop()) { -// DentRobot::elevator->SetUseEncoder(true); -// } DentRobot::elevator->Run(0.0f); } void Raise::Interrupted() { diff --git a/Commands/Elevator/Raise.h b/Commands/Elevator/Raise.h index 51961ba..cb2769e 100644 --- a/Commands/Elevator/Raise.h +++ b/Commands/Elevator/Raise.h @@ -10,6 +10,7 @@ class Raise: public Command { private: bool softLimits; + double speed; public: /** * @brief Constructs Raise @@ -17,8 +18,10 @@ class Raise: public Command { * @param timeout Timeout in seconds (default: 3.5) * * @param useSoftLimits Enables/Disables soft limits via hall effect sensors (default: true) + * + * @param speed Speed at which to raise the elevator */ - Raise(double timeout = 3.5, bool useSoftLimits = true); + Raise(double timeout = 3.5, bool useSoftLimits = true, double liftSpeed=0); /** * @brief Initializes the class */ diff --git a/OI.cpp b/OI.cpp index 291c468..be73f90 100644 --- a/OI.cpp +++ b/OI.cpp @@ -28,43 +28,28 @@ OI::OI() { // 0.8 is the multiplier, so they roll at 80% power left7->WhileHeld(new RollVar(0.8)); // Elevator - raise = new Raise(3.5); + raise = new Raise(3.5,false,1); lower = new Lower(3.0); cycle = new ElevatorCycle(); JoystickButton *left11 = new JoystickButton(leftStick, 11); JoystickButton *left12 = new JoystickButton(leftStick, 12); + JoystickButton *right3 = new JoystickButton(rightStick, 3); JoystickButton *right4 = new JoystickButton(rightStick, 4); + JoystickButton *right5 = new JoystickButton(rightStick, 5); JoystickButton *right6 = new JoystickButton(rightStick, 6); - JoystickButton *right7 = new JoystickButton(rightStick, 7); JoystickButton *right10 = new JoystickButton(rightStick, 10); JoystickButton *right9 = new JoystickButton(rightStick, 9); - JoystickButton *right12 = new JoystickButton(rightStick, 12); left11->WhenPressed(new OpenArm(2)); left12->WhenPressed(new CloseArm(2)); - right4->WhileHeld(lower); - right6->WhileHeld(raise); - right7->WhenPressed(cycle); + //Full speed lift + right4->WhileHeld(new Lower(3.5,false,1)); + right6->WhileHeld(new Raise(3.5,false,-1)); + //Half speed lift + right5->WhileHeld(new Raise(3.5,false,-0.5)); + right3->WhileHeld(new Lower(3.5,false,0.5)); + right9->WhenPressed(new BinOpenArms(2)); right10->WhenPressed(new BinCloseArms(2)); - right4->CancelWhenPressed(raise); - right6->CancelWhenPressed(lower); - right4->CancelWhenPressed(cycle); - right6->CancelWhenPressed(cycle); - right7->CancelWhenPressed(raise); - right7->CancelWhenPressed(lower); - // Killall elevator functions - right12->CancelWhenPressed(raise); - right12->CancelWhenPressed(lower); - right12->CancelWhenPressed(cycle); - // BinElevator - JoystickButton *right3 = new JoystickButton(rightStick, 3); - JoystickButton *right5 = new JoystickButton(rightStick, 5); - binRaise = new BinRaise(3.0); - binLower = new BinLower(2.0); - right3->WhileHeld(binLower); - right3->CancelWhenPressed(binRaise); - right5->WhileHeld(binRaise); - right5->CancelWhenPressed(binLower); // BinCollector JoystickButton *left3 = new JoystickButton(leftStick, 3);