Safe mode

src/main/java/org/carlmontrobotics/robotcode2023/Robot.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
@@ -24,8 +25,11 @@ public void robotInit() {
     robot = this;
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
-    if(!DriverStation.isFMSAttached()) PathPlannerServer.startServer(5811);
+    if (!DriverStation.isFMSAttached())
+      PathPlannerServer.startServer(5811);
     robotContainer = new RobotContainer();
+
+    SmartDashboard.putBoolean("safeMode", false);
   }
 
   @Override
@@ -40,8 +44,13 @@ public void simulationInit() {
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     MotorErrors.printSparkMaxErrorMessages();
-  }
 
+    if (SmartDashboard.getBoolean("safeMode", false)) {
+      RobotContainer.driverMode = RobotContainer.DriverMode.BABY;
+    } else if (!RobotContainer.driverMode.isSlow()) {
+      RobotContainer.driverMode = RobotContainer.DriverMode.NORM;
+    }
+  }
 
   @Override
   public void disabledInit() {
@@ -58,10 +67,12 @@ public void disabledInit() {
   }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+  }
 
   @Override
-  public void disabledExit() {}
+  public void disabledExit() {
+  }
 
   @Override
   public void autonomousInit() {
@@ -71,10 +82,12 @@ public void autonomousInit() {
   }
 
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+  }
 
   @Override
-  public void autonomousExit() {}
+  public void autonomousExit() {
+  }
 
   @Override
   public void teleopInit() {
@@ -83,10 +96,12 @@ public void teleopInit() {
   }
 
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
-  public void teleopExit() {}
+  public void teleopExit() {
+  }
 
   @Override
   public void testInit() {
@@ -95,8 +110,10 @@ public void testInit() {
   }
 
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+  }
 
   @Override
-  public void testExit() {}
+  public void testExit() {
+  }
 }

src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java

@@ -7,6 +7,7 @@
 import static org.carlmontrobotics.robotcode2023.Constants.Arm.*;
 
 import java.util.function.DoubleSupplier;
+import java.util.function.BooleanSupplier;
 
 import org.carlmontrobotics.robotcode2023.Constants;
 import org.carlmontrobotics.robotcode2023.subsystems.Arm;
@@ -21,13 +22,15 @@ public class ArmTeleop extends CommandBase {
   private Arm armSubsystem;
   private DoubleSupplier arm;
   private DoubleSupplier wrist;
+  private BooleanSupplier baby; // is robot in baby (safe) mode?
   private double lastTime = 0;
 
-  public ArmTeleop(Arm armSubsystem, DoubleSupplier arm, DoubleSupplier wrist) {
+  public ArmTeleop(Arm armSubsystem, DoubleSupplier arm, DoubleSupplier wrist, BooleanSupplier baby) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(this.armSubsystem = armSubsystem);
-    this.arm = arm;
-    this.wrist = wrist;
+		this.baby = baby;
+    this.arm = () -> {return baby.getAsBoolean() ? 0.f : arm.getAsDouble();};
+    this.wrist = () -> {return baby.getAsBoolean() ? 0.f : wrist.getAsDouble();};
   }
 
   // Called when the command is initially scheduled.
@@ -53,11 +56,14 @@ public void execute() {
     goalArmRad = MathUtil.clamp(goalArmRad, ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD);
     goalWristRad = MathUtil.clamp(goalWristRad, WRIST_LOWER_LIMIT_RAD, WRIST_UPPER_LIMIT_RAD);
 
-    // Clamp the goal to within a certain range of the current position to prevent "lag"
-    goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
-    goalWristRad = MathUtil.clamp(goalWristRad, armSubsystem.getWristPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, armSubsystem.getWristPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
+    // Clamp the goal to within a certain range of the current position to prevent
+    // "lag"
+    goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD,
+        armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
+    goalWristRad = MathUtil.clamp(goalWristRad, armSubsystem.getWristPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD,
+        armSubsystem.getWristPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
 
-    if((speeds[0] != 0 || speeds[1] != 0) && !DriverStation.isAutonomous()) {
+    if ((speeds[0] != 0 || speeds[1] != 0) && !DriverStation.isAutonomous()) {
       armSubsystem.setArmTarget(goalArmRad, armSubsystem.getCurrentArmGoal().velocity);
       armSubsystem.setWristTarget(goalWristRad, armSubsystem.getCurrentWristGoal().velocity);
     }
@@ -67,20 +73,24 @@ public void execute() {
 
   // Copy and pasted from drivetrain, handles input from joysticks
   public double[] getRequestedSpeeds() {
-    double rawArmVel, rawWristVel;
     // Sets all values less than or equal to a very small value (determined by the
     // idle joystick state) to zero.
+
+    double rawArmVel, rawWristVel;
+    // prep maximum velocity variable
+    double[] maxFF = baby.getAsBoolean() ? MAX_FF_VEL_BABY : MAX_FF_VEL_MANUAL;
+
     if (Math.abs(arm.getAsDouble()) <= Constants.OI.JOY_THRESH)
       rawArmVel = 0.0;
     else
-      rawArmVel = MAX_FF_VEL_MANUAL[ARM] * arm.getAsDouble();
+      rawArmVel = maxFF[ARM] * arm.getAsDouble();
 
     if (Math.abs(wrist.getAsDouble()) <= Constants.OI.JOY_THRESH)
       rawWristVel = 0.0;
     else
-      rawWristVel = MAX_FF_VEL_MANUAL[WRIST] * wrist.getAsDouble();
+      rawWristVel = maxFF[WRIST] * wrist.getAsDouble();
 
-    return new double[] {rawArmVel, rawWristVel};
+    return new double[] { rawArmVel, rawWristVel };
   }
 
   // Called once the command ends or is interrupted.

src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java

@@ -1,8 +1,7 @@
 package org.carlmontrobotics.robotcode2023.commands;
 
-import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject;
-import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode;
 import org.carlmontrobotics.robotcode2023.subsystems.Roller;
+import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -12,9 +11,9 @@ public class RunRoller extends CommandBase {
 
     private final Roller roller;
     private final Timer timer = new Timer();
-    private final RollerMode mode;
+    private final Roller.RollerMode mode;
 
-    public RunRoller(Roller roller, RollerMode mode) {
+    public RunRoller(Roller roller, Roller.RollerMode mode) {
         addRequirements(this.roller = roller);
         this.mode = mode;
     }
@@ -24,31 +23,38 @@ public void initialize() {
         System.err.println("=============================RunRoller is Started=============================");
 
         if (mode.speed > 0) { // should not interrupt command to stop rollers
-            if(roller.hasGamePiece() && isIntake()) cancel();
-            if(!roller.hasGamePiece() && !isIntake()) cancel(); 
+            if (roller.hasGamePiece() && isIntake())
+                cancel();
+            if (!roller.hasGamePiece() && !isIntake())
+                cancel();
         }
         System.err.println("=============================RunRoller is initialized=============================");
         timer.reset();
         roller.setRollerMode(mode);
     }
 
     @Override
-    public void execute() {}
+    public void execute() {
+    }
 
     @Override
     public void end(boolean interrupted) {
         // keep it running if its a cone
-        if (mode.obj != GameObject.CONE && !interrupted)
-            roller.setSpeed(0);
+        if (mode.obj != GameObject.CONE && !interrupted) {
+            mode.speed = 0;
+            roller.setRollerMode(mode);
+        }
         timer.stop();
-        System.err.println("=============================RunRoller has ended==========================================================");
+        System.err.println(
+                "=============================RunRoller has ended==========================================================");
 
     }
 
     @Override
     public boolean isFinished() {
 
-        if(mode.speed == 0) return true;
+        if (mode.speed == 0)
+            return true;
 
         double time = timer.get();
 

src/main/java/org/carlmontrobotics/robotcode2023/commands/TeleopDrive.java

@@ -8,9 +8,10 @@
 package org.carlmontrobotics.robotcode2023.commands;
 
 import static org.carlmontrobotics.robotcode2023.Constants.Drivetrain.*;
+import org.carlmontrobotics.robotcode2023.RobotContainer.DriverMode;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
 import org.carlmontrobotics.robotcode2023.Constants;
 import org.carlmontrobotics.robotcode2023.Robot;
@@ -27,19 +28,19 @@ public class TeleopDrive extends CommandBase {
   private DoubleSupplier fwd;
   private DoubleSupplier str;
   private DoubleSupplier rcw;
-  private BooleanSupplier slow;
+  private Supplier mode;
   private double currentForwardVel = 0;
   private double currentStrafeVel = 0;
 
   /**
    * Creates a new TeleopDrive.
    */
-  public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, BooleanSupplier slow) {
+  public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, Supplier mode) {
     addRequirements(this.drivetrain = drivetrain);
     this.fwd = fwd;
     this.str = str;
     this.rcw = rcw;
-    this.slow = slow;
+    this.mode = mode;
   }
 
   // Called when the command is initially scheduled.
@@ -55,20 +56,30 @@ public void execute() {
   }
 
   public double[] getRequestedSpeeds() {
-    // Sets all values less than or equal to a very small value (determined by the idle joystick state) to zero.
-    // Used to make sure that the robot does not try to change its angle unless it is moving,
+    // Sets all values less than or equal to a very small value (determined by the
+    // idle joystick state) to zero.
+    // Used to make sure that the robot does not try to change its angle unless it
+    // is moving,
     double forward = fwd.getAsDouble();
     double strafe = str.getAsDouble();
     double rotateClockwise = rcw.getAsDouble();
-    if (Math.abs(forward) <= Constants.OI.JOY_THRESH) forward = 0.0;
-    else forward *= maxForward;
-    if (Math.abs(strafe) <= Constants.OI.JOY_THRESH) strafe = 0.0;
-    else strafe *= maxStrafe;
-    if (Math.abs(rotateClockwise) <= Constants.OI.JOY_THRESH) rotateClockwise = 0.0;
-    else rotateClockwise *= maxRCW;
-
-    double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed;
-    double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation;
+    if (Math.abs(forward) <= Constants.OI.JOY_THRESH)
+      forward = 0.0;
+    else
+      forward *= maxForward;
+    if (Math.abs(strafe) <= Constants.OI.JOY_THRESH)
+      strafe = 0.0;
+    else
+      strafe *= maxStrafe;
+    if (Math.abs(rotateClockwise) <= Constants.OI.JOY_THRESH)
+      rotateClockwise = 0.0;
+    else
+      rotateClockwise *= maxRCW;
+
+    double driveMultiplier = ((DriverMode) mode.get()).isBaby() ? kBabyDriveSpeed
+        : (((DriverMode) mode.get()).isSlow() ? kSlowDriveSpeed : kNormalDriveSpeed);
+    double rotationMultiplier = ((DriverMode) mode.get()).isBaby() ? kBabyTurnSpeed
+        : (((DriverMode) mode.get()).isSlow() ? kSlowDriveRotation : kNormalDriveRotation);
 
     forward *= driveMultiplier;
     strafe *= driveMultiplier;
@@ -78,8 +89,9 @@ public double[] getRequestedSpeeds() {
     double accelerationX = (forward - currentForwardVel) / robotPeriod;
     double accelerationY = (strafe - currentStrafeVel) / robotPeriod;
     double translationalAcceleration = Math.hypot(accelerationX, accelerationY);
-    if(translationalAcceleration > autoMaxAccelMps2) {
-      Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX)));
+    if (translationalAcceleration > autoMaxAccelMps2) {
+      Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2,
+          Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX)));
       Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod);
       currentForwardVel += limitedVelocityVector.getX();
       currentStrafeVel += limitedVelocityVector.getY();
@@ -90,13 +102,16 @@ public double[] getRequestedSpeeds() {
 
     // ATM, there is no rotational acceleration limit
 
-    // If the above math works, no velocity should be greater than the max velocity, so we don't need to limit it.
+    // If the above math works, no velocity should be greater than the max velocity,
+    // so we don't need to limit it.
 
-    return new double[] {currentForwardVel, currentStrafeVel, -rotateClockwise};
+    return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise };
   }
 
   public boolean hasDriverInput() {
-    return Math.abs(fwd.getAsDouble()) > Constants.OI.JOY_THRESH || Math.abs(str.getAsDouble()) > Constants.OI.JOY_THRESH || Math.abs(rcw.getAsDouble()) > Constants.OI.JOY_THRESH;
+    return Math.abs(fwd.getAsDouble()) > Constants.OI.JOY_THRESH
+        || Math.abs(str.getAsDouble()) > Constants.OI.JOY_THRESH
+        || Math.abs(rcw.getAsDouble()) > Constants.OI.JOY_THRESH;
   }
 
   // Called once the command ends or is interrupted.