From 7091720ee05b30917f11cb2057918f059e0cf224 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 27 Mar 2024 09:33:42 -0700 Subject: [PATCH 1/6] Add Field2D. --- .../java/org/carlmontrobotics/subsystems/Drivetrain.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 823ec56e..fa36ae92 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -51,6 +51,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; @@ -106,6 +107,9 @@ public class Drivetrain extends SubsystemBase { private SwerveModule moduleBL; private SwerveModule moduleBR; + private final Field2d field = new Field2d(); + + public Drivetrain() { // Calibrate Gyro { @@ -210,6 +214,8 @@ public Drivetrain() { // Setup autopath builder configurePPLAutoBuilder(); + + SmartDashboard.putData("Field", field); } // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int frontorback) { @@ -278,6 +284,8 @@ public void periodic() { // SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); // SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + odometry.update(Rotation2d.fromDegrees(getHeadingDeg()), getModulePositions()); + field.setRobotPose(odometry.getPoseMeters()); } @Override From f30c5b9611bcfd3a08303a2ea60913364992ebba Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Mon, 1 Apr 2024 12:00:33 -0700 Subject: [PATCH 2/6] Use swerve sim support from lib199. --- build.gradle | 2 +- simgui-ds.json | 17 ++--- simgui.json | 65 ++++++++++--------- .../java/org/carlmontrobotics/Constants.java | 2 +- .../subsystems/Drivetrain.java | 42 +++++++++++- 5 files changed, 87 insertions(+), 41 deletions(-) diff --git a/build.gradle b/build.gradle index ff1b55fe..7833bd07 100644 --- a/build.gradle +++ b/build.gradle @@ -79,7 +79,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - implementation "com.github.deepbluerobotics:lib199:cc93080eab462da161d0d47eda253ac8b31495ee" + implementation "com.github.deepbluerobotics:lib199:f8bf1ab981be8888a960ea746c17ba4dc1b1d941" } test { diff --git a/simgui-ds.json b/simgui-ds.json index b16ea5cf..9c0ce4ad 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -15,9 +15,14 @@ "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 74, + "incKey": 76 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -41,10 +46,7 @@ }, { "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, + {}, { "decKey": 73, "incKey": 75 @@ -91,11 +93,10 @@ ], "robotJoysticks": [ { - "useGamepad": true + "guid": "Keyboard0" }, { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard1" } ] } diff --git a/simgui.json b/simgui.json index 0ca0c20b..968efbf2 100644 --- a/simgui.json +++ b/simgui.json @@ -3,48 +3,25 @@ "Addressable LEDs": { "0": { "columns": 32 - }, - "window": { - "visible": true } }, "Other Devices": { - "CANCoder (v6)[0]": { - "header": { - "open": true - } - }, - "CANCoder (v6)[1]": { - "header": { - "open": true - } - }, "CANCoder (v6)[2]": { "header": { "open": true } }, - "CANCoder (v6)[3]": { - "header": { - "open": true - } - }, - "SparkMax[11]": { + "CANCoder[2]": { "header": { "open": true } }, - "SparkMax[14]": { + "SparkMax[15]": { "header": { "open": true } }, - "SparkMax[18]": { - "header": { - "open": true - } - }, - "SparkMax[19]": { + "navX-Sensor[0]": { "header": { "open": true } @@ -60,12 +37,39 @@ "/Shuffleboard/arm SysID/quasistatic backward": "Command", "/Shuffleboard/arm SysID/quasistatic forward": "Command", "/SmartDashboard/Arm": "Subsystem", - "/SmartDashboard/SendableChooser[0]": "String Chooser" + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/SendableChooser[0]": "String Chooser", + "/SmartDashboard/moveClimber": "Command" + }, + "windows": { + "/SmartDashboard/Field": { + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } + } } }, "NetworkTables": { "transitory": { "SmartDashboard": { + "BL": { + "open": true + }, + "BR": { + "open": true + }, + "FL": { + "open": true + }, + "FR": { + "open": true + }, "open": true } } @@ -86,9 +90,12 @@ 0.0, 0.8500000238418579 ], - "height": 177 + "height": 263 } - ] + ], + "window": { + "visible": false + } } } } diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 66c874c7..7d82dd0a 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -232,7 +232,7 @@ public static final class Drivetrainc { // public static final boolean[] reversed = {true, true, true, true}; // Determine correct turnZero constants (FL, FR, BL, BR) public static final double[] turnZeroDeg = RobotBase.isSimulation() ? - new double[] {0, 0, 0, 0} : + new double[] {-90.0, -90.0, -90.0, -90.0} : new double[] {-48.6914, 63.3691, 94.1309, -6.7676};/*real values here*/ // kP, kI, and kD constants for turn motor controllers in the order of front-left, front-right, back-left, back-right. diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index fa36ae92..03f59f48 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -19,6 +19,7 @@ import org.carlmontrobotics.lib199.SensorFactory; import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.swerve.SwerveModule; +import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; import org.carlmontrobotics.Constants.Drivetrainc.Autoc; import org.carlmontrobotics.Robot; import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; @@ -27,10 +28,12 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder.Type; +import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; @@ -74,11 +77,14 @@ import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.drive.DifferentialDrive; // import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + import java.util.function.DoubleSupplier; import java.util.function.Function; @@ -109,6 +115,9 @@ public class Drivetrain extends SubsystemBase { private final Field2d field = new Field2d(); + private SwerveModuleSim[] moduleSims; + private SimDouble gyroYawSim; + private Timer simTimer = new Timer(); public Drivetrain() { // Calibrate Gyro @@ -182,6 +191,14 @@ public Drivetrain() { turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + + if (RobotBase.isSimulation()) { + moduleSims = new SwerveModuleSim[] { + moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() + }; + gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + } + for (CANSparkMax driveMotor : driveMotors) { driveMotor.setOpenLoopRampRate(secsPer12Volts); driveMotor.getEncoder().setPositionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); @@ -200,9 +217,9 @@ public Drivetrain() { coder.getAbsolutePosition().setUpdateFrequency(500); coder.getPosition().setUpdateFrequency(500); coder.getVelocity().setUpdateFrequency(500); - } + SmartDashboard.putData("Field", field); // for(CANSparkMax driveMotor : driveMotors) // driveMotor.setSmartCurrentLimit(80); @@ -214,8 +231,29 @@ public Drivetrain() { // Setup autopath builder configurePPLAutoBuilder(); + } - SmartDashboard.putData("Field", field); + @Override + public void simulationPeriodic() { + for (var moduleSim : moduleSims) { + moduleSim.update(); + } + SwerveModuleState[] measuredStates = + new SwerveModuleState[] { + moduleFL.getCurrentState(), moduleFR.getCurrentState(), moduleBL.getCurrentState(), moduleBR.getCurrentState() + }; + ChassisSpeeds speeds = kinematics.toChassisSpeeds(measuredStates); + + double dtSecs = simTimer.get(); + simTimer.restart(); + + Pose2d simPose = + getPose().exp( + new Twist2d( + speeds.vxMetersPerSecond * dtSecs, + speeds.vyMetersPerSecond * dtSecs, + speeds.omegaRadiansPerSecond * dtSecs)); + gyroYawSim.set((isGyroReversed ? -1.0 : 1.0) * simPose.getRotation().getDegrees()); } // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int frontorback) { From 42ca1843071b98950a20d67efc9dd00089e640c2 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Mon, 1 Apr 2024 13:54:16 -0700 Subject: [PATCH 3/6] Remove duplicate moveClimber command from simgui.json. --- simgui.json | 1 - 1 file changed, 1 deletion(-) diff --git a/simgui.json b/simgui.json index c52347a2..968efbf2 100644 --- a/simgui.json +++ b/simgui.json @@ -39,7 +39,6 @@ "/SmartDashboard/Arm": "Subsystem", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/SendableChooser[0]": "String Chooser", - "/SmartDashboard/moveClimber": "Command", "/SmartDashboard/moveClimber": "Command" }, "windows": { From 1becfbe178aa8c8571ebd31c9e824631006a3b25 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Mon, 1 Apr 2024 14:02:16 -0700 Subject: [PATCH 4/6] Minimize changes vs bangbot branch. --- .../subsystems/Drivetrain.java | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index ba3c7be7..2fe546e3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -42,23 +42,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import static edu.wpi.first.units.Units.Volts; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Meters; - -import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; - -import java.util.function.DoubleSupplier; -import java.util.function.Function; - public class Drivetrain extends SubsystemBase { private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2 private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); @@ -309,9 +294,6 @@ public void periodic() { // SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); // SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); // SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); - - odometry.update(Rotation2d.fromDegrees(getHeadingDeg()), getModulePositions()); - field.setRobotPose(odometry.getPoseMeters()); } @Override From 1292a288ce8e450e2eb4cd5e35f10d1b5adf09f2 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Mon, 1 Apr 2024 20:47:30 -0700 Subject: [PATCH 5/6] Fix bug where swerve sim would not work if setPose() was called with a non-zero angle. --- .../subsystems/Drivetrain.java | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 2fe546e3..1cdb3668 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -23,6 +23,7 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -211,13 +212,20 @@ public void simulationPeriodic() { double dtSecs = simTimer.get(); simTimer.restart(); - Pose2d simPose = - getPose().exp( + Pose2d simPose = field.getRobotPose(); + simPose = simPose.exp( new Twist2d( speeds.vxMetersPerSecond * dtSecs, speeds.vyMetersPerSecond * dtSecs, speeds.omegaRadiansPerSecond * dtSecs)); - gyroYawSim.set((isGyroReversed ? -1.0 : 1.0) * simPose.getRotation().getDegrees()); + double newAngleDeg = simPose.getRotation().getDegrees(); + // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back. + newAngleDeg -= simGyroOffset.getDegrees(); + newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); + gyroYawSim.set(newAngleDeg); + while (Math.abs(MathUtil.inputModulus(gyro.getAngle() - newAngleDeg, -180.0, 180.0)) > 0.1) { + Timer.delay(1.0/gyro.getActualUpdateRate()); + } } // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int @@ -507,8 +515,13 @@ public Pose2d getPose() { return odometry.getPoseMeters(); } + private Rotation2d simGyroOffset = new Rotation2d(); public void setPose(Pose2d initialPose) { - odometry.resetPosition(gyro.getRotation2d(), getModulePositions(), initialPose); + Rotation2d gyroRotation = gyro.getRotation2d(); + odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); + // Remember the offset that the above call to resetPosition() will cause the odometry.update() will add to the gyro rotation in the future + // We need the offset so that we can compensate for it during simulationPeriodic(). + simGyroOffset = initialPose.getRotation().minus(gyroRotation); //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); } From 72304a296903c6d65a139e794397296aeb4dc8ff Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 2 Apr 2024 08:02:21 -0700 Subject: [PATCH 6/6] Display the auto chooser in the sim gui. --- simgui.json | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/simgui.json b/simgui.json index 968efbf2..9b3270e4 100644 --- a/simgui.json +++ b/simgui.json @@ -42,6 +42,11 @@ "/SmartDashboard/moveClimber": "Command" }, "windows": { + "/Shuffleboard/Auto Chooser Tab/SendableChooser[0]": { + "window": { + "visible": true + } + }, "/SmartDashboard/Field": { "bottom": 1476, "height": 8.210550308227539,