Merge branch 'master' into sysid

CAD/Robot_RA24/config.json

@@ -0,0 +1,16 @@
+{
+  "name": "2024 Flipside",
+  "rotations": [
+    {
+      "axis": "y",
+      "degrees": 180.0
+    },
+    {
+      "axis": "z",
+      "degrees": 270.0
+    }
+  ],
+  "position": [0.4, 0.0, 0.0],
+  "cameras": [],
+  "components": []
+}

CAD/Robot_RA25/config.json

@@ -0,0 +1,20 @@
+{
+  "name": "2025 RA25",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90.0
+    },
+    {
+      "axis": "y",
+      "degrees": 0.0
+    },
+    {
+      "axis": "z",
+      "degrees": 270.0
+    }
+  ],
+  "position": [0.0, 0.0, 0.0],
+  "cameras": [],
+  "components": []
+}

src/main/java/frc/robot/LaserCanHandler.java

@@ -0,0 +1,59 @@
+package frc.robot;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import frc.robot.constants.RobotConstants;
+
+public class LaserCanHandler {
+  private static LaserCanHandler m_instance;
+
+  private LaserCan m_indexLaser;
+  private LaserCan m_entranceLaser;
+  private LaserCan m_exitLaser;
+
+  public static LaserCanHandler getInstance() {
+    if (m_instance == null) {
+      m_instance = new LaserCanHandler();
+    }
+    return m_instance;
+  }
+
+  private LaserCanHandler() {
+    m_indexLaser = new LaserCan(RobotConstants.robotConfig.LaserCan.k_indexId);
+    m_entranceLaser = new LaserCan(RobotConstants.robotConfig.LaserCan.k_entranceId);
+    m_exitLaser = new LaserCan(RobotConstants.robotConfig.LaserCan.k_exitId);
+
+    try {
+      m_indexLaser.setRangingMode(LaserCan.RangingMode.SHORT);
+      m_indexLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
+      m_indexLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
+
+      m_entranceLaser.setRangingMode(LaserCan.RangingMode.SHORT);
+      m_entranceLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
+      m_entranceLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
+
+      m_exitLaser.setRangingMode(LaserCan.RangingMode.SHORT);
+      m_exitLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
+      m_exitLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
+    } catch (ConfigurationFailedException e) {
+      System.out.println("Configuration failed! " + e);
+    }
+  }
+
+  @AutoLogOutput(key = "LaserCans/Index/seesCoral")
+  public boolean getIndexSeesCoral() {
+    return m_indexLaser.getMeasurement().distance_mm < 75.0; // Value gotten from Cranberry Alarm code
+  }
+
+  @AutoLogOutput(key = "LaserCans/Entrance/seesCoral")
+  public boolean getEntranceSeesCoral() {
+    return m_entranceLaser.getMeasurement().distance_mm < 75.0; // Value gotten from Cranberry Alarm code
+  }
+
+  @AutoLogOutput(key = "LaserCans/Exit/seesCoral")
+  public boolean getExitSeesCoral() {
+    return m_exitLaser.getMeasurement().distance_mm < 75.0; // Value gotten from Cranberry Alarm code
+  }
+}

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

@@ -16,7 +16,11 @@
 import frc.robot.constants.RobotConstants;
 import frc.robot.controls.controllers.DriverController;
 import frc.robot.controls.controllers.FilteredController;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.controls.controllers.OperatorController;
 import frc.robot.controls.controllers.VirtualRobotController;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Elevator.ElevatorState;
 import frc.robot.subsystems.PoseAligner;
 import frc.robot.subsystems.SignalManager;
 import frc.robot.subsystems.Subsystem;
@@ -35,11 +39,15 @@ public class Robot extends LoggedRobot {
   private final ArrayList<Subsystem> m_subsystems;
 
   private final SwerveDrive m_swerve;
+  private final Elevator m_elevator;
+  // private final EndEffector m_endAffector;
   private final RAROdometry m_odometry;
   private final PoseAligner m_poseAligner;
   private final SignalManager m_signalManager = SignalManager.getInstance();
 
   private final DriverController m_driverController;
+  private final OperatorController m_operatorController;
+
   private final VirtualRobotController m_virtualRobotController;
   private final GenericHID m_sysIdController;
 
@@ -55,15 +63,24 @@ public Robot() {
     m_subsystems = new ArrayList<>();
     m_swerve = SwerveDrive.getInstance();
     m_odometry = RAROdometry.getInstance();
+    m_elevator = Elevator.getInstance();
+    // m_endAffector = EndEffector.getInstance();
     m_poseAligner = PoseAligner.getInstance();
 
-    m_driverController = new DriverController(0, true, false, 0.5);
+    m_driverController = new DriverController(0, true, true, 0.5);
+    m_operatorController = new OperatorController(1, true, true, 0.5);
     m_virtualRobotController = new VirtualRobotController(2);
     m_sysIdController = new GenericHID(3);
 
+    // SCARY
+    DriverStation.silenceJoystickConnectionWarning(true);
+
+    m_subsystems.add(m_poseAligner);
     m_subsystems.add(m_swerve);
     m_subsystems.add(m_odometry);
-    m_subsystems.add(m_poseAligner);
+    m_subsystems.add(m_elevator);
+
+    // m_subsystems.add(m_endAffector);
 
     new RobotTelemetry();
 
@@ -121,10 +138,6 @@ public void teleopPeriodic() {
     ySpeed *= slowScaler * boostScaler;
     rot *= slowScaler * boostScaler;
 
-    if (m_driverController.getWantsAutoPositionPressed()) {
-      m_swerve.resetDriveController();
-    }
-
     // Pose2d targetPose =
     // m_poseAligner.getAndCalculateTargetPose(m_virtualRobotController.getCurrentPose());
     // ASPoseHelper.addPose("VirtualRobot/target", targetPose);
@@ -139,9 +152,33 @@ public void teleopPeriodic() {
       m_swerve.drive(xSpeed, ySpeed, rot, true);
     }
 
-    if(m_driverController.getWantsResetOdometry()) {
+    if (m_driverController.getWantsResetOdometry()) {
       m_odometry.reset();
     }
+
+    if (m_operatorController.getWantsGoToStow()) {
+      m_elevator.goToElevatorPosition(ElevatorState.STOW);
+    } else if (m_operatorController.getWantsGoToL1()) {
+      m_elevator.goToElevatorPosition(ElevatorState.L1);
+    } else if (m_operatorController.getWantsGoToL2()) {
+      m_elevator.goToElevatorPosition(ElevatorState.L2);
+    } else if (m_operatorController.getWantsGoToL3()) {
+      m_elevator.goToElevatorPosition(ElevatorState.L3);
+    } else if (m_operatorController.getWantsGoToL4()) {
+      m_elevator.goToElevatorPosition(ElevatorState.L4);
+    } else if (m_operatorController.getWantsResetElevator()) {
+      m_elevator.reset();
+    }
+
+    // if (m_operatorController.getWantsScore() > 0) {
+    // m_endAffector.setState(EndEffectorState.SCORE_BRANCHES);
+    // } else if (m_operatorController.getWantsScore() < 0) {
+    // m_endAffector.setState(EndEffectorState.SCORE_TROUGH);
+    // } else {
+    // m_endAffector.setState(EndEffectorState.OFF);
+    // if (m_driverController.getWantsAutoPositionPressed()) {
+    // m_swerve.resetDriveController();
+    // }
   }
 
   @Override
@@ -151,6 +188,10 @@ public void disabledInit() {
   @Override
   public void disabledPeriodic() {
     m_odometry.setAllianceGyroAngleAdjustment();
+
+    if(m_operatorController.getWantsResetElevator()) {
+      m_elevator.reset();
+    }
   }
 
   @Override

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

@@ -8,6 +8,9 @@ public class Constants {
   public RobotConstants Robot = new RobotConstants();
   public FieldConstants Field = new FieldConstants();
   public SwerveDriveConstants SwerveDrive = new SwerveDriveConstants();
+  public ElevatorConstants Elevator = new ElevatorConstants();
+  public LaserCanConstants LaserCan = new LaserCanConstants();
+  public EndEffectorConstants EndEffector = new EndEffectorConstants();
   public OdometryConstants Odometry = new OdometryConstants();
 
   public static class RobotConstants {
@@ -151,4 +154,52 @@ public class TurnConstants {
   public class PoseAlignerConstants {
 
   }
+
+  public static class ElevatorConstants {
+    public final int k_elevatorLeftMotorId = 20;
+    public final int k_elevatorRightMotorId = 21;
+
+    public final double k_P = 0.15;
+    public final double k_I = 0;
+    public final double k_D = 0.0;
+    public final double k_IZone = 0.0;
+    public final double k_FF = 0.50;
+
+    public final double k_maxVelocity = 65;
+    public final double k_maxAcceleration = 200;
+
+    public final int k_maxCurrent = 30;
+
+    public final double k_stowHeight = 0.0; // TODO Confirm units
+    public final double k_L1Height = 0.0; // TODO Get height
+    public final double k_L2Height = 9.0;
+    public final double k_L3Height = 25.14;
+    public final double k_L4Height = 52.0;
+    public final double k_maxHeight = 60.5;
+    public final double k_groundAlgaeHeight = 0.0;
+    // public final double k_lowAlgaeHeight = 24.8;
+    // public final double k_highAlgaeHeight = 42.5;
+  }
+
+  public static class LaserCanConstants {
+    public final int k_indexId = 32;
+    public final int k_entranceId = 33;
+    public final int k_exitId = 34;
+  }
+
+  public static class EndEffectorConstants {
+    public final int k_leftMotorId = 30;
+    public final int k_rightMotorId = 31;
+    public final double k_P = 1.0;
+    public final double k_I = 0.0;
+    public final double k_D = 0.0;
+    public final double k_FF = 0.0;
+    public final double k_minOutput = 0.0;
+    public final double k_maxOutput = 0.0;
+    public final double k_indexSpeed = 0.0;
+    public final double k_branchesSpeed = 0.0;
+    public final double k_troughSpeed = 0.0;
+  }
+
+  // TODO: add Gamepiece class for Coral- and Algae-related constants
 }

src/main/java/frc/robot/controls/controllers/OperatorController.java

@@ -8,4 +8,38 @@ public OperatorController(int port) {
   public OperatorController(int port, boolean useDeadband, boolean useSquaredInput, double triggerActivationThreshold) {
     super(port, useDeadband, useSquaredInput, triggerActivationThreshold);
   }
+
+  public boolean getWantsGoToStow() {
+    return this.getRawButtonPressed(Button.A);
+  }
+
+  public boolean getWantsGoToL1() {
+    return this.getRawButtonPressed(Button.X);
+  }
+
+  public boolean getWantsGoToL2() {
+    return this.getRawButtonPressed(Button.B);
+  }
+
+  public boolean getWantsGoToL3() {
+    return this.getRawButtonPressed(Button.Y);
+  }
+
+  public boolean getWantsGoToL4() {
+    return this.getRawButtonPressed(Button.RIGHT_BUMPER);
+  }
+
+  public boolean getWantsResetElevator() {
+    return this.getRawButtonPressed(Button.BACK);
+  }
+
+  public int getWantsScore() {
+    if (this.getRawAxis(Axis.RIGHT_TRIGGER) > this.getRawAxis(Axis.LEFT_TRIGGER)) {
+      return 1;
+    } else if (this.getRawAxis(Axis.RIGHT_TRIGGER) < this.getRawAxis(Axis.LEFT_TRIGGER)) {
+      return -1;
+    } else {
+      return 0;
+    }
+  }
 }

src/main/java/frc/robot/simulation/Field.java

@@ -0,0 +1,18 @@
+package frc.robot.simulation;
+
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+
+public class Field extends Field2d {
+  private static Field m_field;
+
+  public static Field getInstance() {
+    if (m_field == null) {
+      m_field = new Field();
+    }
+    return m_field;
+  }
+
+  private Field() {
+    super();
+  }
+}

src/main/java/frc/robot/simulation/SimulatableCANSparkMax.java

@@ -0,0 +1,29 @@
+package frc.robot.simulation;
+
+import com.revrobotics.spark.SparkMax;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+
+public class SimulatableCANSparkMax extends SparkMax {
+  SimDeviceSim mCANSparkMaxSim;
+
+  SimDouble mCANSparkMaxSimAppliedOutput;
+
+  public SimulatableCANSparkMax(int deviceId, MotorType type) {
+    super(deviceId, type);
+
+    // mCANSparkMaxSim = new SimDeviceSim("SPARK MAX ", deviceId);
+    // mCANSparkMaxSimAppliedOutput = mCANSparkMaxSim.getDouble("Applied Output");
+
+    // TODO: Add other simulation fields
+  }
+
+  // @Override
+  // public void set(double speed) {
+  //   super.set(speed);
+
+  //   // TODO: Figure out why this is mad when running on a real robot
+  //   // mCANSparkMaxSimAppliedOutput.set(speed);
+  // }
+}