From bdcd3f02bf8fa0292b5fcdc12ed2f8e7105977cf Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Wed, 10 Dec 2025 20:49:41 -0800 Subject: [PATCH 1/3] added simulation autos --- src/main/deploy/pathplanner/paths/Mobility.path | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 7 ++++++- .../java/frc/robot/subsystems/CommandSwerveDrivetrain.java | 7 +++++-- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path index fac81f9..a06f646 100755 --- a/src/main/deploy/pathplanner/paths/Mobility.path +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 7.0, + "x": 6.212301136363636, "y": 4.0 }, "prevControl": { - "x": 7.255655952782719, + "x": 6.467957089146355, "y": 4.003114592795971 }, "nextControl": null, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8fee634..6b2bea4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -238,6 +238,11 @@ public static double ExponentialConvert(double controllerValue, double exponent) } public Command getAutonomousCommand() { - return autoChooser.getSelected(); + if (RobotBase.isReal()){ + return autoChooser.getSelected(); // Selected auto on driver station (for real robot) + } + else{ + return AutoBuilder.buildAuto("MobilityAuto"); // Change the autoName to the desired auto in pathplanner (for sim) + } } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index f3b53ae..953500b 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -139,9 +139,12 @@ private void startSimThread() { /* Run simulation at a faster rate so PID gains behave more reasonably */ m_simNotifier = new Notifier(mapleSimSwerveDrivetrain::update); m_simNotifier.startPeriodic(kSimLoopPeriod); + + // Initialize simulation pose to inside the field on black line for red alliance + double redAllianceInitialSimX = 10.2; + int redAllianceInitialSimY = 4; - // Initialize simulation pose to inside the field - Pose2d initialSimPose = new Pose2d(16, 5, new Rotation2d(0)); + Pose2d initialSimPose = new Pose2d(redAllianceInitialSimX, redAllianceInitialSimY, new Rotation2d(0)); mapleSimSwerveDrivetrain.mapleSimDrive.setSimulationWorldPose(initialSimPose); } From 5a248ce88fb6d12aaa69d421cd4b4d1a59a9e5d7 Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Sat, 20 Dec 2025 14:12:48 -0800 Subject: [PATCH 2/3] add algae autos to sim, add more print statements --- src/main/java/frc/robot/RobotContainer.java | 4 +++- src/main/java/frc/robot/Superstructure.java | 5 ++++- .../java/frc/robot/subsystems/AutoCommands/AutoCommands.java | 1 + 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b2bea4..3c369d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,6 +70,8 @@ public class RobotContainer extends TimedRobot { public RobotContainer() { NamedCommands.registerCommand("Eject Coral", superstructure.AutoCoral()); + NamedCommands.registerCommand("Intake Algae", autoCommands.IntakeAlgae()); + NamedCommands.registerCommand("Eject Algae", autoCommands.EjectAlgae()); autoChooser = AutoBuilder.buildAutoChooser("Tests"); SmartDashboard.putData("Auto Mode", autoChooser); @@ -242,7 +244,7 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); // Selected auto on driver station (for real robot) } else{ - return AutoBuilder.buildAuto("MobilityAuto"); // Change the autoName to the desired auto in pathplanner (for sim) + return AutoBuilder.buildAuto("RightAuto (Coral, Algae x2)"); // Change the autoName to the desired auto in pathplanner (for sim) } } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 841a1dd..f8b1e29 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -100,7 +100,7 @@ public void PrintPosition() { public Command AutoCoral() { return Commands.sequence( Commands.runOnce(() -> { - System.out.println("============ CoralEjectPressed\nmoving rollers forward//\n"); + System.out.println("============ CoralEjectPressed\nmoving rollers forward\n"); armMotor.set(TalonSRXControlMode.Position, m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) ); @@ -124,6 +124,7 @@ public Command AutoCoral() { }), Commands.waitSeconds(0.5), Commands.runOnce(() -> { + System.out.println("Reset arm and roller"); rollerMotor.set(VictorSPXControlMode.PercentOutput, 0); armMotor.set(ControlMode.Position, m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) @@ -222,6 +223,7 @@ public Command AutoIntakeCoral() { public Command AlgaeEjectPressed() { return Commands.runOnce(() -> { System.out.println("============= AlgaeEjectPressed"); + System.out.println("Set rollers to algae eject velocity"); rollerMotor.set(VictorSPXControlMode.PercentOutput, m_networkTables.getDoubleValue(ConstantId.RollerMovementAlgaeEjectVelocity) ); @@ -231,6 +233,7 @@ public Command AlgaeEjectPressed() { public Command AlgaeEjectReleased() { return Commands.runOnce(() -> { System.out.println("========== AlgaeEjectReleased"); + System.out.println("Reset rollers"); armMotor.set(TalonSRXControlMode.Position, m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition) ); diff --git a/src/main/java/frc/robot/subsystems/AutoCommands/AutoCommands.java b/src/main/java/frc/robot/subsystems/AutoCommands/AutoCommands.java index 1bf76ab..57898fc 100644 --- a/src/main/java/frc/robot/subsystems/AutoCommands/AutoCommands.java +++ b/src/main/java/frc/robot/subsystems/AutoCommands/AutoCommands.java @@ -33,6 +33,7 @@ public Command IntakeAlgae() { public Command EjectAlgae() { return Commands.sequence( // Implement algae eject + superstructure.AlgaeEjectPressed(), Commands.waitSeconds(networkTables.getTimeValue(NetworkTables.ConstantId.AutoEjectAlgaeWait).in(edu.wpi.first.units.Units.Seconds)), superstructure.AlgaeEjectReleased() ); From 8e646e719b277ace5ae69c682b8d3f4b93fcdfbe Mon Sep 17 00:00:00 2001 From: Aydin-Firoozshahian Date: Sat, 20 Dec 2025 14:24:11 -0800 Subject: [PATCH 3/3] removed position readings in simulation --- src/main/java/frc/robot/Superstructure.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f8b1e29..e4658fb 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -15,6 +15,8 @@ import frc.robot.NetworkTables; import frc.robot.NetworkTables.ConstantId; +import edu.wpi.first.wpilibj.RobotBase; + public class Superstructure extends edu.wpi.first.wpilibj2.command.SubsystemBase { private final DigitalInput m_coralBeamBreak = new DigitalInput(0); private final NetworkTables m_networkTables; @@ -138,8 +140,7 @@ public Command AlgaeIntakePressed() { Commands.runOnce(() -> { System.out.println("============ AlgaeIntakePressed"); System.out.println("lowering arm"); - System.out.println("Position1: " + armMotor.getSelectedSensorPosition(0)); - + if (RobotBase.isReal()) System.out.println("Position1: " + armMotor.getSelectedSensorPosition(0)); armMotor.set(ControlMode.Position, m_networkTables.getDoubleValue(ConstantId.ArmIntakePosition) ); @@ -149,7 +150,7 @@ public Command AlgaeIntakePressed() { ), Commands.runOnce(() -> { System.out.println("stopping the lowering of arm"); - System.out.println("Position2: " + armMotor.getSelectedSensorPosition()); + if (RobotBase.isReal()) System.out.println("Position2: " + armMotor.getSelectedSensorPosition()); rollerMotor.set(VictorSPXControlMode.PercentOutput, m_networkTables.getDoubleValue(ConstantId.RollerMovementAlgaeIntakeVelocity) @@ -162,7 +163,7 @@ public Command AlgaeIntakeReleased() { return Commands.sequence( Commands.runOnce(() -> { System.out.println("============ AlgaeIntakeReleased"); - System.out.println("Position3:" + armMotor.getSelectedSensorPosition()); + if (RobotBase.isReal()) System.out.println("Position3:" + armMotor.getSelectedSensorPosition()); armMotor.set(ControlMode.Position, m_networkTables.getDoubleValue(ConstantId.ArmHoldPosition) @@ -173,7 +174,7 @@ public Command AlgaeIntakeReleased() { ), Commands.runOnce(() -> { System.out.println("============ AlgaeIntakeReleased"); - System.out.println("Position4:" + armMotor.getSelectedSensorPosition(0)); + if (RobotBase.isReal()) System.out.println("Position4:" + armMotor.getSelectedSensorPosition(0)); rollerMotor.set(VictorSPXControlMode.PercentOutput, m_networkTables.getDoubleValue(ConstantId.RollerMovementHoldVelocity)); })