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.3; // 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;
 
@@ -187,7 +189,7 @@ public static final class Arm {
         public static double[] posToleranceRad = { .07, .07 }; // rad
         public static double[] velToleranceRadPSec = { 1, 1 }; // rad/s
 
-        public static double[] offsetRad = { 0.865, 2.93 + Math.PI / 2 }; // rad
+        public static double[] offsetRad = { 2.936 + 0.301, 2.93 + Math.PI / 2 }; // rad
         public static final double rumbleFullPower = 1;
         public static final double rumbleNoPower = 0;
 
@@ -222,6 +224,9 @@ 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[] MAX_FF_VEL_BABY = {0.675, 0.7};
+				public static double[] MAX_FF_ACCEL_BABY = {4, 4};
         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
@@ -233,7 +238,7 @@ public static final class Arm {
         //#region Ports
 
         public static final boolean[] motorInverted = { true, false };
-        public static final boolean[] encoderInverted = { false, true };
+        public static final boolean[] encoderInverted = { true, true };
         public static final double rotationToRad = 2 * Math.PI;
 
         public static final int armMotorPort = 17;
@@ -250,6 +255,7 @@ public static final class Arm {
         public static final int WRIST_CURRENT_LIMIT_AMP = 15;
         public static final double ROLLER_COM_CORRECTION_RAD = Units.degreesToRadians(18.3);
         public static double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = .5;
+        public static final double DISCONNECTED_ENCODER_TIMEOUT_SEC = .25;
 
         //#endregion
 
@@ -374,32 +380,6 @@ public static final class Roller {
         //#endregion
 
         //#region Command Constants
-        // TODO: Determine actual speeds/timings for roller
-        public static class RollerMode {
-            public static RollerMode INTAKE_CONE = new RollerMode(-0.5, .5, GameObject.CONE, conePickupColor);
-            public static RollerMode INTAKE_CUBE = new RollerMode(0.4, .25, GameObject.CUBE, cubePickupColor);
-            // The obj indicates which game object the roller is trying to intake
-            // if obj == NONE, that means it is trying to outtake rather than intake
-            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;
-            public GameObject obj;
-            public Color ledColor;
-
-            /**
-             * @param speed  A number between -1 and 1
-             * @param time   Amount of time in seconds to keep the motor running after
-             *               distance sensor has detected an object
-             * @param intake Whether the roller is outtaking or intaking
-             */
-            public RollerMode(double speed, double time, GameObject obj, Color ledColor) {
-                this.speed = speed;
-                this.time = time;
-                this.obj = obj;
-                this.ledColor = ledColor;
-            }
-        }
 
         //#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

@@ -14,7 +14,6 @@
 import org.carlmontrobotics.robotcode2023.Constants.GoalPos;
 import org.carlmontrobotics.robotcode2023.Constants.OI.Driver;
 import org.carlmontrobotics.robotcode2023.Constants.OI.Manipulator;
-import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode;
 import org.carlmontrobotics.robotcode2023.commands.AlignChargingStation;
 import org.carlmontrobotics.robotcode2023.commands.ArmTeleop;
 import org.carlmontrobotics.robotcode2023.commands.DriveOverChargeStation;
@@ -51,6 +50,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);
 
@@ -74,7 +75,7 @@ public RobotContainer() {
       eventMap.put("Cone High Pos.", new SetArmWristGoalPreset(GoalPos.HIGH, () -> false, () -> false, arm));
       // Command fakeArmCommand = new InstantCommand(() -> System.err.println("==============Store================="), arm);
       // eventMap.put("Stored Pos.", new SequentialCommandGroup(fakeArmCommand, new WaitCommand(2)));
-      eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CUBE)));
+      eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE)));
       eventMap.put("DriveOverChargeStation", new DriveOverChargeStation(drivetrain));
       eventMap.put("Extend Arm High Cube", new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm));
       eventMap.put("Extend Arm Mid Cube", new SetArmWristGoalPreset(GoalPos.MID, () -> true, () -> false, arm));
@@ -85,9 +86,9 @@ public RobotContainer() {
         new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm),
         new PrintCommand("================================Cube High Pos. Ended==================================")
         ));
-      eventMap.put("Run Cube Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CUBE));
-      eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CONE)));
-      eventMap.put("Run Cone Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CONE));
+      eventMap.put("Run Cube Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE));
+      eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CONE)));
+      eventMap.put("Run Cone Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE));
       //eventMap.put("Move Arm Back", new SetArmWristPositionV3((-5*Math.PI)/8, Constants.Arm.WRIST_STOW_POS_RAD, arm));
       eventMap.put("Cone Intake Pos.", new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm));
       eventMap.put("Cube Intake Pos.", new SequentialCommandGroup(
@@ -129,18 +130,20 @@ public RobotContainer() {
 
     drivetrain.setDefaultCommand(new TeleopDrive(
       drivetrain,
-      () -> inputProcessing(getStickValue(driverController, Axis.kLeftY)),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             
+      () -> 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)));},
+			() -> {return isdriverchild;}
     ));
   }
   private void configureButtonBindingsDriver() {
@@ -159,32 +162,31 @@ 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));
     new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm));
     new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm));
-    // new Trigger(() -> {return arm.getForbFlag();}).onTrue(new InstantCommand(() -> {manipulatorController.setRumble(RumbleType.kBothRumble,rumbleFullPower);}))
-    //                                               .onFalse(new InstantCommand(() -> {manipulatorController.setRumble(RumbleType.kBothRumble,rumbleNoPower);}));
+
     new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm));
-    new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CONE)));
-    new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CONE)));
-    new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CUBE)));
-    new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(roller, RollerMode.STOP));
+    new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE)));
+    new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE)));
+    new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE)));
+    new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(roller, Roller.RollerMode.STOP));
 
     // axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton)
     //   .onTrue(new RunRoller(roller, RollerMode.INTAKE_CONE, Constants.Roller.conePickupColor));
     axisTrigger(manipulatorController, Manipulator.rollerIntakeCubeButton)
       .onTrue(new ConditionalCommand(
-        new RunRoller(roller, RollerMode.INTAKE_CUBE), 
-        new RunRoller(roller, RollerMode.OUTTAKE_CUBE), 
+        new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE), 
+        new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE), 
         isIntake
       )); 
     axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton)
       .onTrue(new ConditionalCommand(
-        new RunRoller(roller, RollerMode.INTAKE_CONE), 
-        new RunRoller(roller, RollerMode.OUTTAKE_CONE), 
+        new RunRoller(roller, Roller.RollerMode.INTAKE_CONE), 
+        new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE), 
         isIntake
       ));
 

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;
   }
 
   // Called when the command is initially scheduled.
@@ -67,18 +70,22 @@ 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
+		// 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};
   }

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;
     }
@@ -38,8 +37,10 @@ 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==========================================================");
 

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,12 @@ 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;