Outreach mode (#106)

* more competition changes

* hatch move back

* disable characterized drive

* Implement Outreach Mode

* Set outreach mode to false by default

Author: alexander-mcdowell

Robot2019/src/main/java/frc/robot/Robot.java

@@ -59,6 +59,7 @@ public void robotInit() {
     dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy));
     SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0);
 
+    SmartDashboard.putBoolean("Outreach Mode", false);
     timey = new Timer();
   }
 

Robot2019/src/main/java/frc/robot/commands/Climb.java

@@ -8,6 +8,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Lights;
 import frc.robot.subsystems.Drivetrain;
@@ -50,33 +51,35 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    switch (state) {
-    case CLIMBING:
-      dt.drive(backDrive, backDrive);
-      climber.runClimber(climbUp);
-      if (!climber.needToClimb()) {
-        state = State.LEVELING;
+    if (!SmartDashboard.getBoolean("Outreach Mode", false)) {
+      switch (state) {
+      case CLIMBING:
+        dt.drive(backDrive, backDrive);
+        climber.runClimber(climbUp);
+        if (!climber.needToClimb()) {
+          state = State.LEVELING;
+        }
+        if (robotOnPlatform())
+          state = State.RETRACTING;
+        break;
+      case LEVELING:
+        dt.drive(backDrive, backDrive);
+        climber.runClimber(climbDown);
+        if (!climber.canDrop()) {
+          state = State.CLIMBING;
+        }
+        if (robotOnPlatform())
+          state = State.RETRACTING;
+        break;
+      case RETRACTING:
+        climber.runClimber(retract);
+        if (climber.getEncDistance() <= retractGoal) {
+          state = State.FINISHED;
+        }
+        break;
+      case FINISHED:
+        end();
       }
-      if (robotOnPlatform())
-        state = State.RETRACTING;
-      break;
-    case LEVELING:
-      dt.drive(backDrive, backDrive);
-      climber.runClimber(climbDown);
-      if (!climber.canDrop()) {
-        state = State.CLIMBING;
-      }
-      if (robotOnPlatform())
-        state = State.RETRACTING;
-      break;
-    case RETRACTING:
-      climber.runClimber(retract);
-      if (climber.getEncDistance() <= retractGoal) {
-        state = State.FINISHED;
-      }
-      break;
-    case FINISHED:
-      end();
     }
   }
 

Robot2019/src/main/java/frc/robot/commands/ManualClimb.java

@@ -53,28 +53,30 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    double climbSpeed = manip.getRawAxis(climbJoyAxis);
+    if (!SmartDashboard.getBoolean("Outreach Mode", false)) {
+      double climbSpeed = manip.getRawAxis(climbJoyAxis);
 
-    // if (climbSpeed > 0 && climber.getEncDistance() >= climbDist) {
-    //   climbSpeed = 0;
-    // } else if (climbSpeed < 0 && climber.getEncDistance() <= retractDist) {
-    //   climbSpeed = 0;
-    // }
-    
-    // climber soft stop to prevent bindinng
-    if (climber.getEncDistance() > SmartDashboard.getNumber("Climber Max Height", 25) && (-climbSpeed) > SmartDashboard.getNumber("Climber Drive Limit", 0)) {
-      climbSpeed = -SmartDashboard.getNumber("Climber Drive Limit", 0);
-    }
+      // if (climbSpeed > 0 && climber.getEncDistance() >= climbDist) {
+      //   climbSpeed = 0;
+      // } else if (climbSpeed < 0 && climber.getEncDistance() <= retractDist) {
+      //   climbSpeed = 0;
+      // }
+      
+      // climber soft stop to prevent bindinng
+      if (climber.getEncDistance() > SmartDashboard.getNumber("Climber Max Height", 25) && (-climbSpeed) > SmartDashboard.getNumber("Climber Drive Limit", 0)) {
+        climbSpeed = -SmartDashboard.getNumber("Climber Drive Limit", 0);
+      }
 
-    climber.runClimber(climbSpeed);
+      climber.runClimber(climbSpeed);
 
-    double angle = climber.getAngle();
-    SmartDashboard.putNumber("Current Angle", angle);
-    if (angle > SmartDashboard.getNumber("Max Angle", 0)) {
-      SmartDashboard.putNumber("Max Angle", angle);
-    }
-    if (angle < SmartDashboard.getNumber("Min Angle", 0)) {
-      SmartDashboard.putNumber("Min Angle", angle);
+      double angle = climber.getAngle();
+      SmartDashboard.putNumber("Current Angle", angle);
+      if (angle > SmartDashboard.getNumber("Max Angle", 0)) {
+        SmartDashboard.putNumber("Max Angle", angle);
+      }
+      if (angle < SmartDashboard.getNumber("Min Angle", 0)) {
+        SmartDashboard.putNumber("Min Angle", angle);
+      }
     }
   }
 

Robot2019/src/main/java/frc/robot/commands/MoveBack.java

@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.Drivetrain;
 
 public class MoveBack extends Command {
@@ -34,7 +35,9 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    dt.drive(-1, -1);
+    if (!SmartDashboard.getBoolean("Outreach Mode", false)) {
+      dt.drive(-1, -1);
+    }
   }
 
   // Make this return true when this Command no longer needs to run execute()

Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java

@@ -18,6 +18,8 @@ public class TeleopDrive extends Command {
 
   double prevSpeed = 0, prevLeft = 0, prevRight = 0;
 
+  double outreachSpeed = 0.3;
+
   /**
    * Handles all the teleoperated driving functionality
    * 
@@ -113,7 +115,11 @@ private void arcadeDrive() {
       charDrive(left, right);
     } else {
       SmartDashboard.putBoolean("is in char drive", false);
-      dt.drive(left, right);
+      if (SmartDashboard.getBoolean("Outreach Mode", false)) {
+        dt.drive(left * outreachSpeed, right * outreachSpeed);
+      } else {
+        dt.drive(left, right);
+      }
     }
   }
 
@@ -148,7 +154,11 @@ private void tankDrive() {
     if (SmartDashboard.getBoolean("Characterized Drive", false)) {
       charDrive(left, right);
     } else {
-      dt.drive(left, right);
+      if (SmartDashboard.getBoolean("Outreach Mode", false)) {
+        dt.drive(left * outreachSpeed, right * outreachSpeed);
+      } else {
+        dt.drive(left, right);
+      }
     }
   }
 
@@ -212,7 +222,11 @@ private void charDrive(double left, double right) {
     SmartDashboard.putNumber("Right Volts", rightV);
     //System.out.println("LeftV: " + leftV + ", RightV: " + rightV);
 
-    dt.drive(leftV / maxV, rightV / maxV);
+    if (SmartDashboard.getBoolean("Outreach Mode", false)) {
+      dt.drive(leftV / maxV * outreachSpeed, rightV / maxV * outreachSpeed);
+    } else {
+      dt.drive(leftV / maxV, rightV / maxV);
+    }
     dt.disableVoltageCompensation();
   }
 

Robot2019/src/main/java/frc/robot/subsystems/Climber.java

@@ -42,10 +42,12 @@ public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) {
   }
 
   public void toggleRails() {
-    if (pistons.get() == DoubleSolenoid.Value.kForward) {
-      pistons.set(DoubleSolenoid.Value.kReverse);
-    } else {
-      pistons.set(DoubleSolenoid.Value.kForward);
+    if (!SmartDashboard.getBoolean("Outreach Mode", false)) {
+      if (pistons.get() == DoubleSolenoid.Value.kForward) {
+        pistons.set(DoubleSolenoid.Value.kReverse);
+      } else {
+        pistons.set(DoubleSolenoid.Value.kForward);
+      }
     }
   }