From dba9a27ffcea905e4a1ac676bc1b7d096ba514ce Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Thu, 4 Aug 2022 20:54:55 -0700 Subject: [PATCH 1/5] Disable climber when in guest mode --- src/main/java/frc/robot/Robot.java | 14 +++++++++++--- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++++++++ .../frc/robot/subsystems/ClimberSubsystem.java | 1 - 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 98da6cb..873797f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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,12 +75,14 @@ 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(); } } @@ -87,10 +90,15 @@ public void teleopInit() { @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. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e30dc96..8df1a41 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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; @@ -149,4 +150,19 @@ 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() { + // Disable the climber + CommandScheduler.getInstance().unregisterSubsystem(climber); + } + + public void leaveGuestMode() { + CommandScheduler.getInstance().registerSubsystem(climber); + } } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 8c46206..5923873 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/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 { From 3da95bfa7176279852075c48ba9392ea06e6b99d Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Thu, 4 Aug 2022 21:15:02 -0700 Subject: [PATCH 2/5] Limit drive speed when in guest mode --- src/main/java/frc/robot/Constants.java | 10 +++ src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/commands/drive/DriveGuest.java | 79 +++++++++++++++++++ 3 files changed, 93 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/drive/DriveGuest.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a2cd394..f390879 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8df1a41..65f4035 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,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; @@ -158,11 +159,12 @@ public Command getAutonomousCommand() { * - Limits the drivetrain's maximum speed */ public void enterGuestMode() { - // Disable the climber - CommandScheduler.getInstance().unregisterSubsystem(climber); + CommandScheduler.getInstance().unregisterSubsystem(climber); // Disable the climber + drive.setDefaultCommand(new DriveGuest(drive)); } public void leaveGuestMode() { CommandScheduler.getInstance().registerSubsystem(climber); + drive.setDefaultCommand(new DriveTeleop(drive)); } } diff --git a/src/main/java/frc/robot/commands/drive/DriveGuest.java b/src/main/java/frc/robot/commands/drive/DriveGuest.java new file mode 100644 index 0000000..7aafda8 --- /dev/null +++ b/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. + * + *

+ * i.e., the robot will move in whatever direction the stick is pushed, + * regardless of its orientation on te field. + * + *

+ * 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; + } +} From 3699eb86c9814978ed6d23bfa0cc5fae86c4edb0 Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Thu, 4 Aug 2022 21:29:04 -0700 Subject: [PATCH 3/5] Reduce climber sensitivity to joystick input --- src/main/java/frc/robot/commands/climb/ClimbManual.kt | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/climb/ClimbManual.kt b/src/main/java/frc/robot/commands/climb/ClimbManual.kt index da73e27..8dafdef 100644 --- a/src/main/java/frc/robot/commands/climb/ClimbManual.kt +++ b/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) } From ea69c6a668d1bde32d09a1e3ba479529b6eec34b Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Fri, 5 Aug 2022 15:41:44 -0700 Subject: [PATCH 4/5] test --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 65f4035..c532e12 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -160,11 +160,13 @@ public Command getAutonomousCommand() { */ public void enterGuestMode() { CommandScheduler.getInstance().unregisterSubsystem(climber); // Disable the climber + CommandScheduler.getInstance().cancel(drive.getDefaultCommand()); drive.setDefaultCommand(new DriveGuest(drive)); } public void leaveGuestMode() { CommandScheduler.getInstance().registerSubsystem(climber); + CommandScheduler.getInstance().cancel(drive.getDefaultCommand()); drive.setDefaultCommand(new DriveTeleop(drive)); } } From d6ec1e2b9b244f9b7ed6af74d6eca858efa60d8b Mon Sep 17 00:00:00 2001 From: Mark Ghebrial Date: Fri, 5 Aug 2022 15:46:59 -0700 Subject: [PATCH 5/5] test2 --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c532e12..3be116c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -161,7 +161,7 @@ public Command getAutonomousCommand() { public void enterGuestMode() { CommandScheduler.getInstance().unregisterSubsystem(climber); // Disable the climber CommandScheduler.getInstance().cancel(drive.getDefaultCommand()); - drive.setDefaultCommand(new DriveGuest(drive)); + drive.setDefaultCommand(new DriveTeleop(drive)); } public void leaveGuestMode() {