added half speed raise/lower button

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);

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 {

Commands/Elevator/Lower.h

@@ -10,14 +10,16 @@
 class Lower: public Command {
  private:
   bool softLimits; //<! Enables/Disables hall effect sensors
+  double speed;
  public:
   /**
    * @brief Constructs Lower
    *
    * @param timeout Timeout in seconds (default: 3.0)
    * @param useSoftLimits Enables/Disables soft limits via hall effect sensors (default: true)
+   * @param liftSpeed Speed at which to lower the lift
    */
-  Lower(double timeout = 3.0, bool useSoftLimits = true);
+  Lower(double timeout = 3.0, bool useSoftLimits = true, double liftSpeed=0);
   /**
    * @brief Initializes the class
    */

Commands/Elevator/Raise.cpp

@@ -1,20 +1,16 @@
 #include "Raise.h"
 #include "../../DentRobot.h"
 #include "../../OI.h"
-Raise::Raise(double timeout, bool useSoftLimits): Command("Raise") {
+Raise::Raise(double timeout, bool useSoftLimits, double liftSpeed): Command("Raise") {
   SetTimeout(timeout);
   softLimits=useSoftLimits;
+  speed=liftSpeed;
 }
 void Raise::Initialize() {}
 void Raise::Execute() {
-  DentRobot::elevator->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() {

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
    */

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);