diff --git a/build.gradle b/build.gradle index 98fe57c0..0243b91a 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:14b09294500182f67060d8608ace9e710bb0a004" + implementation "com.github.deepbluerobotics:lib199:0e5932a4219f3439ef2be3981a9189baf777f61e" } 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 99dcaf69..9b3270e4 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]": { + "CANCoder[2]": { "header": { "open": true } }, - "SparkMax[11]": { + "SparkMax[15]": { "header": { "open": true } }, - "SparkMax[14]": { - "header": { - "open": true - } - }, - "SparkMax[18]": { - "header": { - "open": true - } - }, - "SparkMax[19]": { + "navX-Sensor[0]": { "header": { "open": true } @@ -60,13 +37,44 @@ "/Shuffleboard/arm SysID/quasistatic backward": "Command", "/Shuffleboard/arm SysID/quasistatic forward": "Command", "/SmartDashboard/Arm": "Subsystem", + "/SmartDashboard/Field": "Field2d", "/SmartDashboard/SendableChooser[0]": "String Chooser", "/SmartDashboard/moveClimber": "Command" + }, + "windows": { + "/Shuffleboard/Auto Chooser Tab/SendableChooser[0]": { + "window": { + "visible": true + } + }, + "/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 } } @@ -87,9 +95,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 b3dda1ed..7ca9a331 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -218,7 +218,7 @@ public static final class Drivetrainc { public static final boolean[] reversed = { false, false, false, false }; // 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 } + public static final double[] turnZeroDeg = RobotBase.isSimulation() ? 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 diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 22d8474a..3eb0d172 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -19,12 +19,16 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; +import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; 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; 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; @@ -40,7 +44,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.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; 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.)); @@ -48,7 +53,6 @@ public class Drivetrain extends SubsystemBase { private SwerveDriveKinematics kinematics = null; private SwerveDriveOdometry odometry = null; - private Field2d field = new Field2d(); private SwerveModule modules[]; private boolean fieldOriented = true; @@ -68,6 +72,12 @@ public class Drivetrain extends SubsystemBase { private SwerveModule moduleBL; private SwerveModule moduleBR; + private final Field2d field = new Field2d(); + + private SwerveModuleSim[] moduleSims; + private SimDouble gyroYawSim; + private Timer simTimer = new Timer(); + public Drivetrain() { // SmartDashboard.putNumber("set x", 0); // SmartDashboard.putNumber("set y", 0); @@ -142,6 +152,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); @@ -163,6 +181,8 @@ public Drivetrain() { } + SmartDashboard.putData("Field", field); + // for(CANSparkMax driveMotor : driveMotors) // driveMotor.setSmartCurrentLimit(80); @@ -179,6 +199,36 @@ public Drivetrain() { // SmartDashboard.putNumber("chassis speeds theta", 0); } + @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 = field.getRobotPose(); + simPose = simPose.exp( + new Twist2d( + speeds.vxMetersPerSecond * dtSecs, + speeds.vyMetersPerSecond * dtSecs, + speeds.omegaRadiansPerSecond * dtSecs)); + 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 // frontorback) { // switch(frontorback) { @@ -251,7 +301,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()); - } @Override @@ -465,8 +514,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); }