diff --git a/.gitignore b/.gitignore index 38f63d9..238ae65 100644 --- a/.gitignore +++ b/.gitignore @@ -186,4 +186,9 @@ compile_commands.json # Eclipse generated file for annotation processors .factorypath -src/main/java/frc/robot/BuildConstants.java \ No newline at end of file +# Manually added +src/main/java/frc/robot/BuildConstants.java +simgui-ds.json +simgui.json +/simlogs +networktables.json diff --git a/build.gradle b/build.gradle index d35a8f9..9e08a39 100644 --- a/build.gradle +++ b/build.gradle @@ -50,6 +50,10 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true +<<<<<<< HEAD + +======= +>>>>>>> main task(replayWatch, type: JavaExec) { mainClass = "org.littletonrobotics.junction.ReplayWatch" classpath = sourceSets.main.runtimeClasspath diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 98a5492..69b364f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,6 +67,8 @@ private void configureDrive() { driverController::getLeftY, driverController::getLeftX, driverController::getRightX, + () -> 0.0, + driverController.y(), driverController.leftBumper().negate()::getAsBoolean, driverController.rightBumper()::getAsBoolean); diff --git a/src/main/java/frc/robot/subsystems/swerve/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swerve/PoseEstimator.java new file mode 100644 index 0000000..2771df6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/PoseEstimator.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.swerve; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import team2679.atlantiskit.logfields.LogFieldsTable; + +public class PoseEstimator { + private static final PoseEstimator instance = new PoseEstimator(); + + private Pose2d odomertryPose = Pose2d.kZero; + private Pose2d estimatedPose = Pose2d.kZero; + + private LogFieldsTable fieldsTable = new LogFieldsTable("PoseEstimator"); + + private SwerveModulePosition[] lastModulePositions = new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + }; + + private PoseEstimator() { } + + public static PoseEstimator getInstance() { + return instance; + } + + public void update(SwerveDriveKinematics kinematics, SwerveModulePosition[] modulePositions, Optional gyroAngle) { + Twist2d twist2d = kinematics.toTwist2d(lastModulePositions, modulePositions); + lastModulePositions = modulePositions; + Pose2d lastOdometryPose = odomertryPose; + odomertryPose = odomertryPose.exp(twist2d); + if (gyroAngle.isPresent()) { + odomertryPose = new Pose2d(odomertryPose.getTranslation(), gyroAngle.get()); + } + fieldsTable.recordOutput("Current Odomertry Pose", odomertryPose); + Twist2d twistFromLastPose = lastOdometryPose.log(odomertryPose); + estimatedPose = estimatedPose.exp(twistFromLastPose); + fieldsTable.recordOutput("Current Estimated Pose", estimatedPose); + } + + public void resetPose(Pose2d newPose) { + odomertryPose = newPose; + estimatedPose = newPose; + fieldsTable.recordOutput("Current Odomertry Pose", odomertryPose); + fieldsTable.recordOutput("Current Estimated Pose", estimatedPose); + } + + public void resetYaw(Rotation2d newYaw) { + resetPose(new Pose2d(estimatedPose.getTranslation(), newYaw)); + } + + public Pose2d getEstimatedPose() { + return estimatedPose; + } + + public Pose2d getOdometryPose() { + return odomertryPose; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index b701cb7..9b9c2a8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -66,7 +66,9 @@ public Swerve() { isRedAlliance.addOption("blue", false); isRedAlliance.getSendableChooser().onChange((str) -> { - resetYaw(str == "red" ? 0 : 180); + double newAngleDegreesCCW = str == "red" ? 0 : 180; + resetYaw(newAngleDegreesCCW); + PoseEstimator.getInstance().resetYaw(Rotation2d.fromDegrees(newAngleDegreesCCW)); }); gyroYawDegreesCCW = new RotationalSensorHelper(gyroIO.angleDegreesCCW.getAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/robot/subsystems/swerve/SwerveCommands.java index f82f588..5fd8ad9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveCommands.java @@ -21,9 +21,11 @@ public SwerveCommands(Swerve swerve) { } public TunableCommand driverController(DoubleSupplier forwardSupplier, DoubleSupplier sidewaysSupplier, - DoubleSupplier rotationSupplier, BooleanSupplier isFieldRelativeSupplier, BooleanSupplier isSensetiveMode) { + DoubleSupplier rotationSupplier, DoubleSupplier yawAutoRotationSupplier, + BooleanSupplier autoRotationMode, BooleanSupplier isFieldRelativeSupplier, BooleanSupplier isSensetiveMode) { return new SwerveDriverController(swerve, forwardSupplier, sidewaysSupplier, rotationSupplier, + yawAutoRotationSupplier, autoRotationMode, isFieldRelativeSupplier, isSensetiveMode); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index eff7fb1..3320ba8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -55,7 +55,7 @@ public final class Sim { public static final double DRIVE_MOTOR_MOMENT_OF_INERTIA = 0.025; public static final double TURN_MOTOR_MOMENT_OF_INERTIA = 0.004; - public static final double SIM_TURN_MOTOR_KP = 0; + public static final double SIM_TURN_MOTOR_KP = 1.8 * 12; public static final double SIM_TURN_MOTOR_KI = 0; public static final double SIM_TURN_MOTOR_KD = 0; } @@ -78,5 +78,8 @@ public static final class DriverController { public static final double DRIVER_ANGULAR_ACCELERATION_LIMIT_RPS = 4.5; public static final double SENSETIVE_TRANSLATION_MULTIPLIER = 0.3; public static final double SENSETIVE_ROTATION_MULTIPLIER = 0.3; + public static final double ROTATION_KP = 1.8 * 12; + public static final double ROTATION_KI = 0; + public static final double ROTATION_KD = 0; } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDriverController.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDriverController.java index d470ffa..f0843da 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDriverController.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDriverController.java @@ -3,6 +3,7 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import team2679.atlantiskit.tunables.SendableType; @@ -16,30 +17,37 @@ public class SwerveDriverController extends TunableCommand { private final Swerve swerve; - private TunablesTable tuneablesTable = new TunablesTable(SendableType.LIST); + private TunablesTable tunablesTable = new TunablesTable(SendableType.LIST); private DoubleSupplier sidewaysSupplier; private DoubleSupplier forwardSupplier; private DoubleSupplier rotationsSupplier; + private DoubleSupplier yawAutoRotationSupplier; + private BooleanSupplier autoRotationMode; private BooleanSupplier isFieldRelative; private BooleanSupplier isSensetiveMode; - private DoubleHolder maxAngularVelocityRPS = tuneablesTable.addNumber("Max Angular Velocity RPS", - DRIVER_MAX_ANGULAR_VELOCITY_RPS); + private DoubleHolder maxAngularVelocityRPS = tunablesTable.addNumber("Max Angular Velocity RPS", + DRIVER_MAX_ANGULAR_VELOCITY_RPS); private SendableChooser velocityMultiplierChooser = new SendableChooser<>(); private final SlewRateLimiter forwardSlewRateLimiter = new SlewRateLimiter(DRIVER_ACCELERATION_LIMIT_MPS); private final SlewRateLimiter sidewaysSlewRateLimiter = new SlewRateLimiter(DRIVER_ACCELERATION_LIMIT_MPS); private final SlewRateLimiter rotationSlewRateLimiter = new SlewRateLimiter(DRIVER_ANGULAR_ACCELERATION_LIMIT_RPS); + private final PIDController autoRotationPID = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); + public SwerveDriverController(Swerve swerve, DoubleSupplier forwardSupplier, DoubleSupplier sidewaysSupplier, - DoubleSupplier rotationsSupplier, BooleanSupplier isFieldRelative, BooleanSupplier isSensetiveMode) { + DoubleSupplier rotationsSupplier, DoubleSupplier yawAutoRotationSupplier, BooleanSupplier autoRotationMode, + BooleanSupplier isFieldRelative, BooleanSupplier isSensetiveMode) { this.swerve = swerve; addRequirements(swerve); this.sidewaysSupplier = sidewaysSupplier; this.forwardSupplier = forwardSupplier; this.rotationsSupplier = rotationsSupplier; + this.yawAutoRotationSupplier = yawAutoRotationSupplier; + this.autoRotationMode = autoRotationMode; this.isFieldRelative = isFieldRelative; this.isSensetiveMode = isSensetiveMode; @@ -48,7 +56,10 @@ public SwerveDriverController(Swerve swerve, DoubleSupplier forwardSupplier, Dou velocityMultiplierChooser.addOption("BABY (30%)", 0.3); velocityMultiplierChooser.addOption("EGG (10%)", 0.1); - tuneablesTable.addChild("velocity chooser", velocityMultiplierChooser); + tunablesTable.addChild("velocity chooser", velocityMultiplierChooser); + + autoRotationPID.enableContinuousInput(-Math.PI, Math.PI); + tunablesTable.addChild("Auto Rotation PID Controller", autoRotationPID); } @Override @@ -61,24 +72,33 @@ public void execute() { double precentageForward = forwardSupplier.getAsDouble() * velocityMultiplier; double precentageSideways = sidewaysSupplier.getAsDouble() * velocityMultiplier; - double precentageRotation = rotationsSupplier.getAsDouble() * velocityMultiplier; + double precentageRotation; + + if (!autoRotationMode.getAsBoolean()) { + precentageRotation = rotationsSupplier.getAsDouble() * velocityMultiplier; + } else { + double currentYawAngle = PoseEstimator.getInstance().getEstimatedPose().getRotation().getDegrees(); + precentageRotation = autoRotationPID.calculate(currentYawAngle, yawAutoRotationSupplier.getAsDouble()) + * velocityMultiplier; + } if (isSensetiveMode.getAsBoolean()) { precentageForward *= SENSETIVE_TRANSLATION_MULTIPLIER; precentageSideways *= SENSETIVE_TRANSLATION_MULTIPLIER; precentageRotation *= SENSETIVE_ROTATION_MULTIPLIER; } - + swerve.drive( - forwardSlewRateLimiter.calculate(precentageForward * MAX_VOLTAGE), - sidewaysSlewRateLimiter.calculate(precentageSideways * MAX_VOLTAGE), - rotationSlewRateLimiter.calculate(precentageRotation * maxAngularVelocityRPS.get()), - isFieldRelative.getAsBoolean(), - false); + forwardSlewRateLimiter.calculate(precentageForward * MAX_VOLTAGE), + sidewaysSlewRateLimiter.calculate(precentageSideways * MAX_VOLTAGE), + rotationSlewRateLimiter.calculate(precentageRotation * maxAngularVelocityRPS.get()), + isFieldRelative.getAsBoolean(), + false); } @Override public void end(boolean interrupted) { + autoRotationPID.reset(); } @Override @@ -88,6 +108,6 @@ public boolean isFinished() { @Override public void initTunable(TunableBuilder builder) { - tuneablesTable.initTunable(builder); + tunablesTable.initTunable(builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/io/SwerveModuleSim.java b/src/main/java/frc/robot/subsystems/swerve/io/SwerveModuleSim.java index aa05d58..ef2301d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/io/SwerveModuleSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/io/SwerveModuleSim.java @@ -35,17 +35,19 @@ protected void periodicBeforeFields() { driveMotor.update(0.2); turnMotor.update(0.2); - angleRotaions += (turnMotor.getAngularVelocityRPM() / 60 * 0.02); - angleRotaions = warpAngle(angleRotaions); + driveMotorRotations += driveMotor.getAngularVelocityRPM() / 60 * 0.02; + + angleRotaions += turnMotor.getAngularVelocityRPM() / 60 * 0.02; + turnMotor.setInputVoltage(turnPIDController.calculate(getAbsoluteTurnAngleRotations())); } - private double warpAngle(double angle) { + private double wrapAngle(double angle) { return ((angle + 1) % 2 + 2) % 2 - 1; } @Override protected double getAbsoluteTurnAngleRotations() { - return angleRotaions; + return wrapAngle(angleRotaions); } @Override @@ -65,7 +67,7 @@ public void setDrivePercentageSpeed(double speed) { @Override public void setTurnAngleRotations(double rotations) { - turnMotor.setInputVoltage(rotations); + turnPIDController.setSetpoint(wrapAngle(rotations)); } @Override @@ -75,10 +77,12 @@ protected double getDriveDistanceRotations() { @Override public void setCoast() { + driveMotor.setInputVoltage(0); } @Override public void resetIntegratedAngleRotations(double newAngle) { + angleRotaions = newAngle; } @Override