[Robot] Thursday CMP changes

SouthwestRoboticsProgramming · Apr 19, 2024 · bb8fbc1 · bb8fbc1
1 parent ce0df0a
commit bb8fbc1
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 13 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/Robot.java b/Robot/src/main/java/com/swrobotics/robot/Robot.java
@@ -124,7 +124,7 @@ public void robotInit() {
  public void robotPeriodic() {
  Threads.setCurrentThreadPriority(true, 99);
 
- robotContainer.messenger.readMessages();
+ // robotContainer.messenger.readMessages();
  ThreadUtils.runMainThreadOperations();
  CommandScheduler.getInstance().run(); // Leave this alone
  }

diff --git a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java
@@ -47,17 +47,17 @@
  */
 public class RobotContainer {
  // Configuration for our Raspberry Pi communication service
- private static final String MESSENGER_HOST_ROBOT = "10.21.29.3";
- private static final String MESSENGER_HOST_SIM = "localhost";
- private static final int MESSENGER_PORT = 5805;
- private static final String MESSENGER_NAME = "Robot";
+ // private static final String MESSENGER_HOST_ROBOT = "10.21.29.3";
+ // private static final String MESSENGER_HOST_SIM = "localhost";
+ // private static final int MESSENGER_PORT = 5805;
+ // private static final String MESSENGER_NAME = "Robot";
 
  // Create a way to choose between autonomous sequences
  private final LoggedDashboardChooser<Command> autoSelector;
  private final LoggedDashboardChooser<String> musicSelector;
  private final LoggedDashboardChooser<Double> autoDelaySelector;
 
- public final MessengerClient messenger;
+ // public final MessengerClient messenger;
  public final ControlBoard controlboard;
 
  public final PowerDistribution pdp;
@@ -87,13 +87,13 @@ public RobotContainer() {
  DriverStation.silenceJoystickConnectionWarning(RobotBase.isSimulation());
 
  // Initialize Messenger
- String host = RobotBase.isSimulation() ? MESSENGER_HOST_SIM : MESSENGER_HOST_ROBOT;
- messenger = new MessengerClient(host, MESSENGER_PORT, MESSENGER_NAME);
- new FileSystemAPI(messenger, "RoboRIO", Filesystem.getOperatingDirectory());
+ // String host = RobotBase.isSimulation() ? MESSENGER_HOST_SIM : MESSENGER_HOST_ROBOT;
+ // messenger = new MessengerClient(host, MESSENGER_PORT, MESSENGER_NAME);
+ // new FileSystemAPI(messenger, "RoboRIO", Filesystem.getOperatingDirectory());
 
  pdp = new PowerDistribution(IOAllocation.CAN.PDP.id(), PowerDistribution.ModuleType.kRev);
 
- drive = new SwerveDrive(FieldInfo.CRESCENDO_2024, messenger);
+ drive = new SwerveDrive(FieldInfo.CRESCENDO_2024/*, messenger*/);
 
  intake = new IntakeSubsystem(pdp);
  indexer = new IndexerSubsystem(intake);

diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
@@ -347,6 +347,11 @@ public void periodic() {
  } else {
  waitingForArmRetract = false;
  }
+
+ if (ampArmPos == AmpArm2Subsystem.Position.AMP) {
+ NTData.AMP_ARM_EXTEND_POS.set(NTData.AMP_ARM_EXTEND_POS.get() + -operator.leftStickY.get() * 10 / 20);
+ }
+
  robot.ampArm2.setPosition(ampArmPos);
  robot.climber.setState(climbState.climberState);
 

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/amp/AmpArm2Subsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/amp/AmpArm2Subsystem.java
@@ -29,7 +29,7 @@ public final class AmpArm2Subsystem extends SubsystemBase {
  private static final double motorToArmRatio = 50;
  private static final double encoderToArmRatio = 2;
 
- private static final double cancoderOffset = 0.045410;
+ private static final double cancoderOffset = 0.317627;
 
  private Position targetPos;
 

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java
@@ -90,7 +90,7 @@ public static record TurnRequest(int priority, Rotation2d turn) {
  private TurnRequest currentTurnRequest;
  private int lastSelectedPriority;
 
- public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) {
+ public SwerveDrive(FieldInfo fieldInfo/*, MessengerClient msg*/) {
  this.fieldInfo = fieldInfo;
  gyro = new AHRS(SPI.Port.kMXP);
 
@@ -138,7 +138,7 @@ public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) {
 
  // Pathfinding.setPathfinder(new LocalADStar());
 // Pathfinding.setPathfinder(new ThetaStarPathfinder(msg));
- Pathfinding.setPathfinder(new ArcPathfinder(msg));
+ // Pathfinding.setPathfinder(new ArcPathfinder(msg));
  PathPlannerLogging.setLogActivePathCallback(
  (activePath) -> {
  Logger.recordOutput(