Guest mode

src/main/java/frc/robot/Constants.java

@@ -92,6 +92,16 @@ public static class Drive {
         public static final double MAX_TELEOP_DECELERATION = 9;
     }
 
+    /**
+     * Speed limits for guest drivers
+     */
+    public static class GuestMode {
+        public static final double MAX_GUEST_SPEED = 3; // Meters/second
+        public static final double MAX_GUEST_ROTATIONAL_SPEED = Math.toRadians(350);
+        public static final double MAX_GUEST_ACCELERATION = 12; // Maters/second squared
+        public static final double MAX_GUEST_DECELERATION = 20;
+    }
+
     /**
      * Constants for the Indexer
      */

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

@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -60,6 +59,8 @@ public void disabledPeriodic() {}
     /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
     @Override
     public void autonomousInit() {
+        m_robotContainer.leaveGuestMode();
+
         m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
         // schedule the autonomous command (example)
@@ -74,23 +75,30 @@ public void autonomousPeriodic() {}
 
     @Override
     public void teleopInit() {
+        m_robotContainer.leaveGuestMode();
+
         // This makes sure that the autonomous stops running when
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
         // this line or comment it out.
         if (m_autonomousCommand != null) {
-        m_autonomousCommand.cancel();
+            m_autonomousCommand.cancel();
         }
     }
 
     /** This function is called periodically during operator control. */
     @Override
     public void teleopPeriodic() {}
 
+    /**
+     * Enabling test mode causes the robot to enter "Guest Mode," which makes
+     * it easier and safer for untrained individuals to operate.
+     */
     @Override
     public void testInit() {
         // Cancels all running commands at the start of test mode.
-        CommandScheduler.getInstance().cancelAll();
+        //CommandScheduler.getInstance().cancelAll();
+        m_robotContainer.enterGuestMode();
     }
 
     /** This function is called periodically during test mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.auto.TaxiAndPreloadAuto;
 import frc.robot.commands.auto.ThreeBallAuto;
 import frc.robot.commands.auto.TwoBallAuto;
@@ -18,6 +19,7 @@
 import frc.robot.commands.climb.SetClimberPosition;
 import frc.robot.commands.climb.ToggleClimberExtension;
 import frc.robot.commands.drive.DriveAndAim;
+import frc.robot.commands.drive.DriveGuest;
 import frc.robot.commands.drive.DriveTeleop;
 import frc.robot.commands.drive.PointInDirectionOfTravel;
 import frc.robot.commands.intake.IntakeAndIndex;
@@ -149,4 +151,22 @@ private void bindShootingCommand(FiringSolution solution, BooleanSupplier condit
     public Command getAutonomousCommand() {
         return autoChooser.getSelected();
     }
+
+    /**
+     * "Guest Mode" makes the robot easier and safer for untrained individuals
+     * to operate. It changes the following:
+     *      - Completely disables the climber
+     *      - Limits the drivetrain's maximum speed
+     */
+    public void enterGuestMode() {
+        CommandScheduler.getInstance().unregisterSubsystem(climber); // Disable the climber
+        CommandScheduler.getInstance().cancel(drive.getDefaultCommand());
+        drive.setDefaultCommand(new DriveTeleop(drive));
+    }
+
+    public void leaveGuestMode() {
+        CommandScheduler.getInstance().registerSubsystem(climber);
+        CommandScheduler.getInstance().cancel(drive.getDefaultCommand());
+        drive.setDefaultCommand(new DriveTeleop(drive));
+    }
 }

src/main/java/frc/robot/commands/climb/ClimbManual.kt

@@ -3,6 +3,7 @@ package frc.robot.commands.climb
 import edu.wpi.first.wpilibj2.command.CommandBase
 import frc.robot.OI
 import frc.robot.subsystems.ClimberSubsystem
+import kotlin.math.pow
 
 class ClimbManual(private val climber: ClimberSubsystem) : CommandBase() {
     init {
@@ -11,7 +12,13 @@ class ClimbManual(private val climber: ClimberSubsystem) : CommandBase() {
 
     override fun execute() {
         if (climber.isExtended) {
-            climber.setClimberPower(OI.operatorController.leftYWithDeadband)
+            // Squaring the joystick value makes motor power output non-linearly
+            // related to joystick position, which should make low-speed adjustments
+            // easier for the operator
+            var motorPower = OI.operatorController.leftYWithDeadband.pow(2)
+            if (OI.operatorController.leftYWithDeadband < 0) { motorPower = -motorPower }
+
+            climber.setClimberPower(motorPower)
         } else {
             climber.setClimberPower(0.0)
         }

src/main/java/frc/robot/commands/drive/DriveGuest.java

@@ -0,0 +1,79 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.IMU;
+import frc.robot.OI;
+import frc.robot.subsystems.DriveSubsystem;
+import friarLib2.utility.DoubleSlewRateLimiter;
+import friarLib2.utility.Vector3309;
+
+/**
+ * Use the left joystick position to control the robot's direction of 
+ * travel relative to the field. Robot heading change is determined 
+ * by moving the right joystick left and right.
+ * 
+ * <p>
+ * i.e., the robot will move in whatever direction the stick is pushed,
+ * regardless of its orientation on te field.
+ * 
+ * <p>
+ * This class is designed to be subclassed so that other commands can 
+ * use the same translational velcoity calculations for field-relative 
+ * teleop control while being able to set the rotational speed 
+ * independently (based on vision data for example).
+ */
+public class DriveGuest extends CommandBase {
+
+    protected DriveSubsystem drive;
+
+    private static final DoubleSlewRateLimiter xAccelLimiter = new DoubleSlewRateLimiter(Constants.GuestMode.MAX_GUEST_ACCELERATION, Constants.GuestMode.MAX_GUEST_DECELERATION);
+    private static final DoubleSlewRateLimiter yAccelLimiter = new DoubleSlewRateLimiter(Constants.GuestMode.MAX_GUEST_ACCELERATION, Constants.GuestMode.MAX_GUEST_DECELERATION);
+
+    public DriveGuest(DriveSubsystem drive) {
+        this.drive = drive;
+
+        addRequirements(drive);
+    }
+
+    @Override
+    public void initialize() { }
+
+    @Override
+    public void execute() {
+        Vector3309 translationalSpeeds = Vector3309.fromCartesianCoords(
+            -OI.leftStick.getXWithDeadband(),
+            -OI.leftStick.getYWithDeadband()).capMagnitude(1).scale(Constants.GuestMode.MAX_GUEST_SPEED);
+
+        // Limit the drivebase's acceleration to reduce wear on the swerve modules
+        translationalSpeeds.setXComponent(xAccelLimiter.calculate(translationalSpeeds.getXComponent()));
+        translationalSpeeds.setYComponent(yAccelLimiter.calculate(translationalSpeeds.getYComponent()));
+
+        ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+            translationalSpeeds.getXComponent(), 
+            translationalSpeeds.getYComponent(), 
+            calculateRotationalSpeed(translationalSpeeds), 
+            IMU.getRobotYaw());
+
+        drive.setChassisSpeeds(speeds);
+    }
+
+    /**
+     * Given the field-relative translational speeds requested by the
+     * operators, calculate the rotational speed of the robot.
+     * 
+     * @param translationalSpeeds
+     * @return The rotational speed in radians/second
+     */
+    protected double calculateRotationalSpeed (Vector3309 translationalSpeeds) {
+        double rotationalSpeed = Constants.GuestMode.MAX_GUEST_ROTATIONAL_SPEED * -OI.rightStick.getXWithDeadband();
+
+        return rotationalSpeed;
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -12,7 +12,6 @@
 import static frc.robot.Constants.Climber.*;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import frc.robot.util.TalonStatusFrames;
 
 public class ClimberSubsystem extends SubsystemBase {