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;
 
@@ -181,13 +183,13 @@ public static final class Arm {
         // Arm, Wrist
         public static double[] kP = {4.2736, 4.8804}; // 4.2736 for arm from sysid was tested and it worked fine (V / rad)
         public static double[] kI = {0, 0}; // (V / (rad * s) )
-        public static double[] kD = {0, 0.90262}; // 0 for arm from sysid was tested and it worked fine (V / (rad / s) )
+        public static double[] kD = {0.1, 0.90262}; // 0 for arm from sysid was tested and it worked fine (V / (rad / s) )
 
         // Arm, Wrist
         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;
 
@@ -212,7 +214,7 @@ public static final class Arm {
         public static final double DT_EXTENSION_FOR_ROLLER = Units.inchesToMeters(14);
 
         // TODO: Replace these values with Design's actual values
-        public static final double MARGIN_OF_ERROR = Math.toRadians(10);
+        public static final double MARGIN_OF_ERROR = Math.PI/18;
         public static final double ARM_LOWER_LIMIT_RAD = -3.569 + MARGIN_OF_ERROR;
         public static final double ARM_UPPER_LIMIT_RAD = .36 - MARGIN_OF_ERROR;
         public static final double ARM_DISCONTINUITY_RAD = (ARM_LOWER_LIMIT_RAD + ARM_UPPER_LIMIT_RAD) / 2 - Math.PI;
@@ -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
 
@@ -383,7 +389,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 +406,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,13 +4,15 @@
 
 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;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxAbsoluteEncoder;
 import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -54,7 +56,14 @@ public class Arm extends SubsystemBase {
     // rad, rad/s
     public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), new TrapezoidProfile.State(0, 0) };
 
+    private double lastArmPos;
+    private double lastMeasuredTime;
+    private boolean isArmEncoderConnected = false;
+
     public Arm() {
+        Timer.getFPGATimestamp();
+        armMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20);
+        wristMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20);
         armMotor.setInverted(motorInverted[ARM]);
         wristMotor.setInverted(motorInverted[WRIST]);
         armMotor.setIdleMode(IdleMode.kBrake);
@@ -81,53 +90,79 @@ public Arm() {
         setArmTarget(goalState[ARM].position, 0);
         setWristTarget(goalState[WRIST].position, 0);
         wristMotor.setSmartCurrentLimit(WRIST_CURRENT_LIMIT_AMP);
+        lastArmPos = getArmPos();
+        lastMeasuredTime = Timer.getFPGATimestamp();
 
         // SmartDashboard.putNumber("Arm Max Vel", MAX_FF_VEL[ARM]);
         // SmartDashboard.putNumber("Wrist Max Vel", MAX_FF_VEL[WRIST]);
-        SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
-        SmartDashboard.putNumber("Arm Tolerance Pos", posToleranceRad[ARM]);
-        SmartDashboard.putNumber("Wrist Tolerance Pos", posToleranceRad[WRIST]);
-        SmartDashboard.putNumber("Arm Tolerance Vel", velToleranceRadPSec[ARM]);
-        SmartDashboard.putNumber("Wrist Tolerance Vel", velToleranceRadPSec[WRIST]);
+        // SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
+        // SmartDashboard.putNumber("Arm Tolerance Pos", posToleranceRad[ARM]);
+        // SmartDashboard.putNumber("Wrist Tolerance Pos", posToleranceRad[WRIST]);
+        // SmartDashboard.putNumber("Arm Tolerance Vel", velToleranceRadPSec[ARM]);
+        // SmartDashboard.putNumber("Wrist Tolerance Vel", velToleranceRadPSec[WRIST]);
     }
 
     @Override
     public void periodic() {
 
         if(DriverStation.isDisabled()) resetGoal();
 
+        double currTime = Timer.getFPGATimestamp();
+        SmartDashboard.putNumber("Current Time", currTime);
+        SmartDashboard.putNumber("Last Update (s)", lastMeasuredTime);
+
+        // when the value is different
+        double currentArmPos = getArmPos();
+        if (currentArmPos != lastArmPos) {
+            lastMeasuredTime = currTime;
+            lastArmPos = currentArmPos;
+        }
+        isArmEncoderConnected = currTime - lastMeasuredTime < DISCONNECTED_ENCODER_TIMEOUT_SEC;
+        SmartDashboard.putBoolean("ArmEncoderConnected", isArmEncoderConnected);
+
+
         // TODO: REMOVE THIS WHEN PID CONSTANTS ARE DONE
         // MAX_FF_VEL[ARM] = SmartDashboard.getNumber("Arm Max Vel", MAX_FF_VEL[ARM]);
         // MAX_FF_VEL[WRIST] = SmartDashboard.getNumber("Wrist Max Vel", MAX_FF_VEL[WRIST]);
-        ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = SmartDashboard.getNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
-        // wristConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL[WRIST], MAX_FF_ACCEL[WRIST]);
-        // armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL[ARM], MAX_FF_ACCEL[ARM]);
+        // ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = SmartDashboard.getNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
+        armConstraints = new TrapezoidProfile.Constraints(
+          RobotContainer.isdriverchild ? MAX_FF_VEL_BABY[0] : MAX_FF_VEL_AUTO[0],
+          MAX_FF_ACCEL[0]);
+        wristConstraints = new TrapezoidProfile.Constraints(
+          RobotContainer.isdriverchild ? MAX_FF_VEL_BABY[1] : MAX_FF_VEL_AUTO[1],
+          MAX_FF_ACCEL[1]);
         armPID.setP(kP[ARM]);
         armPID.setI(kI[ARM]);
         armPID.setD(kD[ARM]);
         wristPID.setP(kP[WRIST]);
         wristPID.setI(kI[WRIST]);
         wristPID.setD(kD[WRIST]);
-        SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID.atSetpoint());
-        SmartDashboard.putBoolean("ArmProfileFinished", armProfile.isFinished(armProfileTimer.get()));
-        SmartDashboard.putBoolean("WristPIDAtSetpoint", wristPID.atSetpoint());
-        SmartDashboard.putBoolean("WristProfileFinished", wristProfile.isFinished(wristProfileTimer.get()));
-        posToleranceRad[ARM] = SmartDashboard.getNumber("Arm Tolerance Pos", posToleranceRad[ARM]);
-        posToleranceRad[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Pos", posToleranceRad[WRIST]);
-        velToleranceRadPSec[ARM] = SmartDashboard.getNumber("Arm Tolerance Vel", velToleranceRadPSec[ARM]);
-        velToleranceRadPSec[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Vel", velToleranceRadPSec[WRIST]);
-
-        SmartDashboard.putNumber("MaxHoldingTorque", maxHoldingTorqueNM());
-        SmartDashboard.putNumber("V_PER_NM", getV_PER_NM());
-        SmartDashboard.putNumber("COMDistance", getCoM().getNorm());
-        SmartDashboard.putNumber("InternalArmVelocity", armRelEncoder.getVelocity());
-        SmartDashboard.putNumber("Arm Current", armMotor.getOutputCurrent());
-        SmartDashboard.putNumber("Wrist Current", wristMotor.getOutputCurrent());
+        // SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID.atSetpoint());
+        // SmartDashboard.putBoolean("ArmProfileFinished", armProfile.isFinished(armProfileTimer.get()));
+        // SmartDashboard.putBoolean("WristPIDAtSetpoint", wristPID.atSetpoint());
+        // SmartDashboard.putBoolean("WristProfileFinished", wristProfile.isFinished(wristProfileTimer.get()));
+        // posToleranceRad[ARM] = SmartDashboard.getNumber("Arm Tolerance Pos", posToleranceRad[ARM]);
+        // posToleranceRad[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Pos", posToleranceRad[WRIST]);
+        // velToleranceRadPSec[ARM] = SmartDashboard.getNumber("Arm Tolerance Vel", velToleranceRadPSec[ARM]);
+        // velToleranceRadPSec[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Vel", velToleranceRadPSec[WRIST]);
+
+        // SmartDashboard.putNumber("MaxHoldingTorque", maxHoldingTorqueNM());
+        // SmartDashboard.putNumber("V_PER_NM", getV_PER_NM());
+        // SmartDashboard.putNumber("COMDistance", getCoM().getNorm());
+        // SmartDashboard.putNumber("InternalArmVelocity", armRelEncoder.getVelocity());
+        // SmartDashboard.putNumber("Arm Current", armMotor.getOutputCurrent());
+        // SmartDashboard.putNumber("Wrist Current", wristMotor.getOutputCurrent());
 
         SmartDashboard.putNumber("ArmPos", getArmPos());
         SmartDashboard.putNumber("WristPos", getWristPos());
 
-        driveArm(armProfile.calculate(armProfileTimer.get()));
+        if (isArmEncoderConnected)
+            driveArm(armProfile.calculate(armProfileTimer.get()));
+        else {
+            armMotor.setVoltage(0);
+            SmartDashboard.putNumber("ArmTotalVolts", 0);
+        }
+
         driveWrist(wristProfile.calculate(wristProfileTimer.get()));
 
         autoCancelArmCommand();
@@ -154,11 +189,9 @@ private void driveArm(TrapezoidProfile.State state) {
         double armPIDVolts = armPID.calculate(getArmPos(), state.position);
         if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) || 
             (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) {
-              forbFlag = true;  
+              forbFlag = true;
             armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0);
         }
-
-
         // TODO: REMOVE WHEN DONE WITH TESTING (ANY CODE REVIEWERS, PLEASE REJECT MERGES
         // TO MASTER IF THIS IS STILL HERE)
         SmartDashboard.putNumber("ArmFeedVolts", armFeedVolts);