From 01deef05a9a5030ee04b932ace6d560f9de70549 Mon Sep 17 00:00:00 2001 From: DrollerBubble59 <114955979+DrollerBubble59@users.noreply.github.com> Date: Wed, 19 Nov 2025 19:18:54 -0500 Subject: [PATCH] adds sim --- Test_17.0_Base_Rewrite/simgui-ds.json | 5 ++++ .../src/main/java/frc/robot/Robot.java | 12 ++++----- .../main/java/frc/robot/RobotContainer.java | 8 +++++- .../frc/robot/subsystems/SwerveSubsystem.java | 27 ++++++++++++------- 4 files changed, 35 insertions(+), 17 deletions(-) diff --git a/Test_17.0_Base_Rewrite/simgui-ds.json b/Test_17.0_Base_Rewrite/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/Test_17.0_Base_Rewrite/simgui-ds.json +++ b/Test_17.0_Base_Rewrite/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/Test_17.0_Base_Rewrite/src/main/java/frc/robot/Robot.java b/Test_17.0_Base_Rewrite/src/main/java/frc/robot/Robot.java index ea16e2b..3dceae6 100644 --- a/Test_17.0_Base_Rewrite/src/main/java/frc/robot/Robot.java +++ b/Test_17.0_Base_Rewrite/src/main/java/frc/robot/Robot.java @@ -1,6 +1,8 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -11,21 +13,21 @@ public class Robot extends TimedRobot { @Override public void robotInit() { - // Create RobotContainer, which sets up the swerve + controller bindings + // Start WPILib data logging (creates .wpilog files) + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); + m_robotContainer = new RobotContainer(); } @Override public void robotPeriodic() { - // Run the command scheduler every loop CommandScheduler.getInstance().run(); } @Override public void autonomousInit() { - // Get whatever auto command RobotContainer exposes (currently just a stub) m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } @@ -41,7 +43,6 @@ public void autonomousExit() { @Override public void teleopInit() { - // Cancel auto when teleop starts if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); m_autonomousCommand = null; @@ -56,7 +57,6 @@ public void disabledPeriodic() {} @Override public void testInit() { - // Cancel all commands when entering test mode CommandScheduler.getInstance().cancelAll(); } diff --git a/Test_17.0_Base_Rewrite/src/main/java/frc/robot/RobotContainer.java b/Test_17.0_Base_Rewrite/src/main/java/frc/robot/RobotContainer.java index 940e7c3..f1f2fe7 100644 --- a/Test_17.0_Base_Rewrite/src/main/java/frc/robot/RobotContainer.java +++ b/Test_17.0_Base_Rewrite/src/main/java/frc/robot/RobotContainer.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -17,6 +18,10 @@ public class RobotContainer { private final CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); + // Rotation scale: 30% in sim, 100% on real robot + private static final double ROTATION_SCALE = + RobotBase.isSimulation() ? 0.05 : 1.0; + // YAGSL drive input stream private final SwerveInputStream driveAngularVelocity = SwerveInputStream.of( @@ -25,7 +30,8 @@ public class RobotContainer { () -> -m_driverController.getLeftX()) // strafe .withControllerRotationAxis(() -> -m_driverController.getRightX()) // rotation .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(1.0) // full speed; lower if you want a cap + .scaleTranslation(1.0) // full translation speed + .scaleRotation(ROTATION_SCALE) // 30% in sim, 100% on real .allianceRelativeControl(true); // always field-relative // You can copy() and rescale driveRegular later for slow mode, etc. diff --git a/Test_17.0_Base_Rewrite/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/Test_17.0_Base_Rewrite/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index c863e3f..7136d2b 100644 --- a/Test_17.0_Base_Rewrite/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/Test_17.0_Base_Rewrite/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -17,31 +19,33 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; public class SwerveSubsystem extends SubsystemBase { + private final SwerveDrive swerveDrive; + // Added: Field2d object for AdvantageScope / dashboards + private final Field2d field = new Field2d(); + public SwerveSubsystem() { - // Directory containing swervedrive.json and module configs File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "swerve"); - // Keep telemetry light by default SwerveDriveTelemetry.verbosity = TelemetryVerbosity.LOW; try { - // Create the swerve drive from YAGSL configs swerveDrive = new SwerveParser(swerveJsonDirectory) .createSwerveDrive(Constants.Drive.MAX_LINEAR_SPEED_MPS); } catch (Exception e) { throw new RuntimeException("Failed to create SwerveDrive from configuration", e); } + + // Publish the field to NetworkTables so AdvantageScope can see it + SmartDashboard.putData("Field", field); } - /** Direct drive using translation and rotation (radians/sec). */ public void drive(Translation2d translation, double rotationRadPerSec, boolean fieldRelative) { swerveDrive.drive(translation, rotationRadPerSec, fieldRelative, false); } - /** Drive directly from chassis speeds (robot- or field-relative chosen by caller). */ public void drive(ChassisSpeeds speeds, boolean fieldRelative) { if (fieldRelative) { swerveDrive.driveFieldOriented(speeds); @@ -50,34 +54,37 @@ public void drive(ChassisSpeeds speeds, boolean fieldRelative) { } } - /** Build a Command that continuously drives field-relative from a ChassisSpeeds supplier. */ public Command driveFieldOriented(Supplier velocity) { return run(() -> swerveDrive.driveFieldOriented(velocity.get())); } - /** Current robot pose from YAGSL's internal odometry. */ public Pose2d getPose() { return swerveDrive.getPose(); } - /** Heading from the swerve pose. */ public Rotation2d getHeading() { return swerveDrive.getPose().getRotation(); } - /** Reset gyro heading to 0. */ public void zeroGyro() { swerveDrive.zeroGyro(); } - /** Expose underlying YAGSL SwerveDrive (used by SwerveInputStream). */ public SwerveDrive getSwerveDrive() { return swerveDrive; } + // Optional getter if you ever want direct access to the field + public Field2d getField() { + return field; + } + @Override public void periodic() { // Keep YAGSL odometry updated swerveDrive.updateOdometry(); + + // Update the Field2d pose so AdvantageScope can draw the robot + field.setRobotPose(getPose()); } }