Safe mode

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

@@ -142,6 +142,8 @@ public static final class Drivetrain {
         public static final double kNormalDriveRotation = 0.5; // Percent Multiplier
         public static final double kSlowDriveSpeed = 0.4; // Percent Multiplier
         public static final double kSlowDriveRotation = 0.250; // Percent Multiplier
+        public static final double kBabyDriveSpeed = 0.1; // Percent Multiplier
+        public static final double kBabyTurnSpeed = 0.2; // Percent Multiplier
         public static final double kAlignMultiplier = 1D/3D;
         public static final double kAlignForward = 0.6;
 
@@ -222,6 +224,8 @@ public static final class Arm {
 
         // TODO: Determine actual max vel/accel
         // public static double[] MAX_FF_VEL = {.25, .25}; // rad / s
+        //Arm, Wrist
+        public static double[] ARM_SLOWMODE_MULT = {.1, .5};
         public static double[] MAX_FF_VEL_MANUAL = {1, 3}; // rad / s
         public static double[] MAX_FF_VEL_AUTO = {1.25, 5}; // rad / s
         public static double[] MAX_FF_ACCEL = {5, 5}; // rad / s^2
@@ -383,7 +387,8 @@ public static class RollerMode {
             public static RollerMode OUTTAKE_CONE = new RollerMode(0.5, .5, GameObject.NONE, defaultColor);
             public static RollerMode OUTTAKE_CUBE = new RollerMode(-0.5, .5, GameObject.NONE, defaultColor);
             public static RollerMode STOP = new RollerMode(0, .1, GameObject.NONE, defaultColor);
-            public double speed, time;
+            private double speed;
+            public double time;
             public GameObject obj;
             public Color ledColor;
 
@@ -399,6 +404,14 @@ public RollerMode(double speed, double time, GameObject obj, Color ledColor) {
                 this.obj = obj;
                 this.ledColor = ledColor;
             }
+
+            public double getSpeed() {
+                return speed;
+            }
+
+            public void setSpeed(double speed) {
+                this.speed = speed;
+            }
         }
 
         //#endregion

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 {
@@ -26,6 +27,8 @@ public void robotInit() {
     DriverStation.startDataLog(DataLogManager.getLog());
     if(!DriverStation.isFMSAttached()) PathPlannerServer.startServer(5811);
     robotContainer = new RobotContainer();
+
+    SmartDashboard.putBoolean("safeMode", false);
   }
 
   @Override
@@ -40,6 +43,8 @@ public void simulationInit() {
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     MotorErrors.printSparkMaxErrorMessages();
+
+    RobotContainer.isdriverchild = SmartDashboard.getBoolean("safeMode", false);
   }
 
 

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

@@ -51,6 +51,8 @@
 
 public class RobotContainer {
 
+  public static boolean isdriverchild = false;
+
   public final GenericHID driverController = new GenericHID(Driver.port);
   public final GenericHID manipulatorController = new GenericHID(Manipulator.port);
 
@@ -132,15 +134,16 @@ public RobotContainer() {
       () -> inputProcessing(getStickValue(driverController, Axis.kLeftY)),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             
       () -> inputProcessing(getStickValue(driverController, Axis.kLeftX)),
       () -> inputProcessing(getStickValue(driverController, Axis.kRightX)),
-      () -> driverController.getRawButton(Driver.slowDriveButton)
+      () -> driverController.getRawButton(Driver.slowDriveButton),
+      () -> {return isdriverchild;}
     ));
 
     configureButtonBindingsDriver();
     configureButtonBindingsManipulator();
     arm.setDefaultCommand(new ArmTeleop(
       arm,
-      () -> inputProcessing(getStickValue(manipulatorController, Axis.kLeftY)),
-      () -> inputProcessing(getStickValue(manipulatorController, Axis.kRightY))
+      () -> {return (isdriverchild ? 0 : inputProcessing(getStickValue(manipulatorController, Axis.kLeftY)));},
+      () -> {return (isdriverchild ? 0 : inputProcessing(getStickValue(manipulatorController, Axis.kRightY)));}
     ));
   }
   private void configureButtonBindingsDriver() {
@@ -159,7 +162,7 @@ private void configureButtonBindingsManipulator() {
     BooleanSupplier isIntake = () -> !isCube.getAsBoolean();
 
     new JoystickButton(manipulatorController, Manipulator.storePosButton).onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm));
-    new JoystickButton(manipulatorController, Manipulator.lowPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.LOW, isCube, isFront, arm));
+    //new JoystickButton(manipulatorController, Manipulator.lowPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.LOW, isCube, isFront, arm));
     new JoystickButton(manipulatorController, Manipulator.midPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.MID, isCube, isFront, arm));
     new JoystickButton(manipulatorController, Manipulator.highPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.HIGH, isCube, isFront, arm));
     new POVButton(manipulatorController, Manipulator.shelfPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SHELF, isCube, isFront, arm));

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

@@ -23,7 +23,7 @@ public RunRoller(Roller roller, RollerMode mode) {
     public void initialize() {
         System.err.println("=============================RunRoller is Started=============================");
 
-        if (mode.speed > 0) { // should not interrupt command to stop rollers
+        if (mode.getSpeed() > 0) { // should not interrupt command to stop rollers
             if(roller.hasGamePiece() && isIntake()) cancel();
             if(!roller.hasGamePiece() && !isIntake()) cancel(); 
         }
@@ -48,7 +48,7 @@ public void end(boolean interrupted) {
     @Override
     public boolean isFinished() {
 
-        if(mode.speed == 0) return true;
+        if(mode.getSpeed() == 0) return true;
 
         double time = timer.get();
 

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

@@ -28,18 +28,20 @@ public class TeleopDrive extends CommandBase {
   private DoubleSupplier str;
   private DoubleSupplier rcw;
   private BooleanSupplier slow;
+  private BooleanSupplier baby;
   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, BooleanSupplier slow, BooleanSupplier baby) {
     addRequirements(this.drivetrain = drivetrain);
     this.fwd = fwd;
     this.str = str;
     this.rcw = rcw;
     this.slow = slow;
+    this.baby = baby;
   }
 
   // Called when the command is initially scheduled.
@@ -67,8 +69,8 @@ public double[] getRequestedSpeeds() {
     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;
+    double driveMultiplier = baby.getAsBoolean() ? kBabyDriveSpeed : (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed);
+    double rotationMultiplier = baby.getAsBoolean() ? kBabyTurnSpeed : (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation);
 
     forward *= driveMultiplier;
     strafe *= driveMultiplier;

src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Arm.java

@@ -4,6 +4,7 @@
 
 import org.carlmontrobotics.MotorConfig;
 import org.carlmontrobotics.lib199.MotorControllerFactory;
+import org.carlmontrobotics.robotcode2023.RobotContainer;
 import org.carlmontrobotics.robotcode2023.Constants.GoalPos;
 import org.carlmontrobotics.robotcode2023.commands.ArmTeleop;
 
@@ -149,9 +150,10 @@ public void autoCancelArmCommand() {
     //#region Drive Methods
 
     private void driveArm(TrapezoidProfile.State state) {
+        double speedMult = RobotContainer.isdriverchild ? ARM_SLOWMODE_MULT[0] : 1;
         double kgv = getKg();
         double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0);
-        double armPIDVolts = armPID.calculate(getArmPos(), state.position);
+        double armPIDVolts = armPID.calculate(getArmPos(), state.position) * speedMult;
         if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) || 
             (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) {
               forbFlag = true;  
@@ -169,9 +171,10 @@ private void driveArm(TrapezoidProfile.State state) {
     }
 
     private void driveWrist(TrapezoidProfile.State state) {
+        double speedMult = RobotContainer.isdriverchild ? ARM_SLOWMODE_MULT[1] : 1;
         double kgv = wristFeed.calculate(getWristPosRelativeToGround(), state.velocity, 0);
         double wristFeedVolts = wristFeed.calculate(getWristPosRelativeToGround(), state.velocity, 0);
-        double wristPIDVolts = wristPID.calculate(getWristPos(), state.position);
+        double wristPIDVolts = wristPID.calculate(getWristPos(), state.position) * speedMult;
         if ((getWristPos() > WRIST_UPPER_LIMIT_RAD && state.velocity > 0) || 
             (getWristPos() < WRIST_LOWER_LIMIT_RAD && state.velocity < 0)) {
             forbFlag = true;

src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java

@@ -89,26 +89,26 @@ public double getGamePieceDistanceIn() {
     }
 
     public void setRollerMode(RollerMode mode) {
-        setSpeed(mode.speed);
+        setSpeed(mode.getSpeed());
         setLedColor(mode.ledColor);
     }
 
     public void putRollerConstsOnSmartDashboard() {
-        SmartDashboard.putNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.speed);
-        SmartDashboard.putNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.speed);
-        SmartDashboard.putNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.speed);
-        SmartDashboard.putNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.speed);
+        SmartDashboard.putNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.getSpeed());
+        SmartDashboard.putNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.getSpeed());
+        SmartDashboard.putNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.getSpeed());
+        SmartDashboard.putNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.getSpeed());
         SmartDashboard.putNumber("Intake Cone Time", RollerMode.INTAKE_CONE.time);
         SmartDashboard.putNumber("Outtake Cone Time", RollerMode.OUTTAKE_CONE.time);
         SmartDashboard.putNumber("Intake Cube Time", RollerMode.INTAKE_CUBE.time);
         SmartDashboard.putNumber("Outtake Cube Time", RollerMode.OUTTAKE_CUBE.time);
     }
 
     public void getRollerConstsOnSmartDashboard() {
-        RollerMode.INTAKE_CONE.speed = SmartDashboard.getNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.speed);
-        RollerMode.OUTTAKE_CONE.speed = SmartDashboard.getNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.speed);
-        RollerMode.INTAKE_CUBE.speed = SmartDashboard.getNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.speed);
-        RollerMode.OUTTAKE_CUBE.speed = SmartDashboard.getNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.speed);
+        RollerMode.INTAKE_CONE.setSpeed(SmartDashboard.getNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.getSpeed()));
+        RollerMode.OUTTAKE_CONE.setSpeed(SmartDashboard.getNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.getSpeed()));
+        RollerMode.INTAKE_CUBE.setSpeed(SmartDashboard.getNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.getSpeed()));
+        RollerMode.OUTTAKE_CUBE.setSpeed(SmartDashboard.getNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.getSpeed()));
         RollerMode.INTAKE_CONE.time = SmartDashboard.getNumber("Intake Cone Time", RollerMode.INTAKE_CONE.time);
         RollerMode.OUTTAKE_CONE.time = SmartDashboard.getNumber("Outtake Cone Time", RollerMode.OUTTAKE_CONE.time);
         RollerMode.INTAKE_CUBE.time = SmartDashboard.getNumber("Intake Cube Time", RollerMode.INTAKE_CUBE.time);