diff --git a/Robot/src/main/java/com/swrobotics/robot/Robot.java b/Robot/src/main/java/com/swrobotics/robot/Robot.java index 3035465..4194209 100644 --- 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 index ed0b737..3bd38b2 100644 --- 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 autoSelector; private final LoggedDashboardChooser musicSelector; private final LoggedDashboardChooser 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 index 3f1530b..edd01f9 100644 --- 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 index 5c475a3..aa39824 100644 --- 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 index 39bb17e..3c01105 100644 --- 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(