From 2cd0433f51b4042462094d68e183dfaea3afd6ce Mon Sep 17 00:00:00 2001 From: Ishan Venkatesh <242120@students.srvusd.net> Date: Mon, 30 Sep 2024 16:59:38 -0700 Subject: [PATCH 1/2] Adding simulation files --- 1458TeamCode/build.gradle | 4 +- .../main/java/frc/robot/RobotContainer.java | 19 +- .../main/java/frc/robot/subsystems/Drive.java | 75 ++++++- .../java/frc/robot/subsystems/Module.java | 189 +++++++++++++++++- 4 files changed, 269 insertions(+), 18 deletions(-) diff --git a/1458TeamCode/build.gradle b/1458TeamCode/build.gradle index c73a804..b4ffe57 100644 --- a/1458TeamCode/build.gradle +++ b/1458TeamCode/build.gradle @@ -45,7 +45,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. @@ -98,4 +98,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' -} +} \ No newline at end of file diff --git a/1458TeamCode/src/main/java/frc/robot/RobotContainer.java b/1458TeamCode/src/main/java/frc/robot/RobotContainer.java index 1d22cdf..3e4626f 100644 --- a/1458TeamCode/src/main/java/frc/robot/RobotContainer.java +++ b/1458TeamCode/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.autos.*; import frc.robot.commands.*; @@ -33,18 +34,18 @@ public class RobotContainer { /* Subsystems */ private final Drive s_Swerve = new Drive(); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - s_Swerve.setDefaultCommand( - new TeleopSwerve( - s_Swerve, - () -> -driver.getRawAxis(translationAxis), - () -> -driver.getRawAxis(strafeAxis), - () -> -driver.getRawAxis(rotationAxis), - () -> robotCentric.getAsBoolean() - ) + TeleopSwerve teleopSwerveCommand = new TeleopSwerve( + s_Swerve, + () -> -driver.getRawAxis(translationAxis), + () -> -driver.getRawAxis(strafeAxis), + () -> -driver.getRawAxis(rotationAxis), + () -> robotCentric.getAsBoolean() ); + s_Swerve.setDefaultCommand(teleopSwerveCommand); + + SmartDashboard.putData("TeleopSwerveCmd", teleopSwerveCommand); // Configure the button bindings configureButtonBindings(); diff --git a/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java b/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java index 6bddb71..d30229b 100644 --- a/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java +++ b/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java @@ -8,13 +8,28 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import java.util.List; + import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.sim.Pigeon2SimState; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; 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.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,6 +38,16 @@ public class Drive extends SubsystemBase { public Module[] mSwerveMods; public Pigeon2 gyro; + private Trajectory trajectory; + private final Field2d field = new Field2d(); + private Pose2d simPose = new Pose2d(); + + // // Simulation + // private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); + // private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); + // private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); + private final Pigeon2SimState gyroSim; + public Drive() { gyro = new Pigeon2(Constants.Swerve.pigeonID, "CV"); gyro.getConfigurator().apply(new Pigeon2Configuration()); @@ -35,8 +60,25 @@ public Drive() { new Module(3, Constants.Swerve.BackRightMod.constants) }; - swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions()); + + // Simulation + // Create and push Field2d to SmartDashboard. + SmartDashboard.putData("Field", field); + + // Create the trajectory to follow in autonomous. + trajectory = + TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + List.of(new Translation2d(1, 1), new Translation2d(2, -1)), + new Pose2d(3, 0, Rotation2d.fromDegrees(0)), + new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)) + ); + + // Push the trajectory to Field2d. + field.getObject("traj").setTrajectory(trajectory); + + gyroSim = gyro.getSimState(); } public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { @@ -127,10 +169,39 @@ public void periodic() { for (Module mod : mSwerveMods) { SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", (mod.getCANcoder().getDegrees()+180)%360); - // SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue()%22.0)*360/22-180)); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue() * 360) % 360 + 360) % 360); // This is super specific, don't break this pls // SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } + + field.setRobotPose(swerveOdometry.getPoseMeters()); + } + + @Override + public void simulationPeriodic() { + // TODO: Sim jointly as one system + for (Module mod : mSwerveMods) { + mod.updateSimPeriodic(); + + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", (mod.getCANcoder().getDegrees()+180)%360); + SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue() * 360) % 360 + 360) % 360); // This is super specific, don't break this pls + } + + // final ChassisSpeeds chassisSpeeds = Constants.Swerve.swerveKinematics.toChassisSpeeds(getModuleStates()); + // simPose = simPose.transformBy(( + // new Transform2d( + // new Translation2d( + // chassisSpeeds.vxMetersPerSecond * TimedRobot.kDefaultPeriod * 100000, + // chassisSpeeds.vyMetersPerSecond * TimedRobot.kDefaultPeriod * 100000), + // // new Translation2d( + // // 20, + // // 20), + // new Rotation2d(chassisSpeeds.omegaRadiansPerSecond * TimedRobot.kDefaultPeriod) + // ) + // )); + // field.setRobotPose(simPose); + + swerveOdometry.update(getGyroYaw(), getModulePositions()); + field.setRobotPose(swerveOdometry.getPoseMeters()); } } \ No newline at end of file diff --git a/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java b/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java index d1465e6..b3f43e0 100644 --- a/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java +++ b/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java @@ -5,16 +5,30 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.mechanisms.swerve.SimSwerveDrivetrain; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.Pigeon2SimState; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + import frc.robot.lib.math.Conversions; import frc.robot.lib.util.SwerveModuleConstants; import frc.robot.Constants; -import frc.robot.CTREConfigs; import frc.robot.Robot; public class Module { @@ -23,6 +37,7 @@ public class Module { TalonFX mAngleMotor; private TalonFX mDriveMotor; + private CANcoder angleEncoder; private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); @@ -34,10 +49,20 @@ public class Module { /* angle motor control requests */ private final PositionVoltage anglePosition = new PositionVoltage(0); + /* simulation */ + // private final DifferentialDrivetrainSim mDriveSim; + private final FlywheelSim mDriveSim; + private final SingleJointedArmSim mSteeringSim; + + // private final EncoderSim encoderSim; + private final TalonFXSimState mDriveMotorSimState; + private final TalonFXSimState mAngleMotorSimState; + private CANcoderSimState angleEncoderSimState; + public Module(int moduleNumber, SwerveModuleConstants moduleConstants){ this.moduleNumber = moduleNumber; this.angleOffset = moduleConstants.angleOffset; - + /* Angle Encoder Config */ angleEncoder = new CANcoder(moduleConstants.cancoderID, "CV"); angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCANcoderConfig); @@ -59,6 +84,52 @@ public Module(int moduleNumber, SwerveModuleConstants moduleConstants){ mAngleMotor.setInverted(true); } mDriveMotor.getConfigurator().setPosition(0.0); + + // For simulation + // encoderSim = new EncoderSim(mAngleMotor); + mDriveMotorSimState = mDriveMotor.getSimState(); + mAngleMotorSimState = mAngleMotor.getSimState(); + angleEncoderSimState = angleEncoder.getSimState(); + + final double kEncoderRadiansPerPulse = 2.0 * Math.PI / 2048; + + // mDriveSim = new DifferentialDrivetrainSim( + // DCMotor.getFalcon500Foc(2), // 2 CIMS on each side of the drivetrain. + // Constants.Swerve.driveGearRatio, // Standard AndyMark Gearing reduction. + // 2.1, // MOI of 2.1 kg m^2 (from CAD model). + // 26.5, // Mass of the robot is 26.5 kg. + // Constants.Swerve.wheelCircumference, // Robot uses 3" radius (6" diameter) wheels. + // 0.546, // Distance between wheels is _ meters. + + // /* + // * The standard deviations for measurement noise: + // * x and y: 0.001 m + // * heading: 0.001 rad + // * l and r velocity: 0.1 m/s + // * l and r position: 0.005 m + // */ + // /* Uncomment the following line to add measurement noise. */ + // null // VecBuilder.fill(0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005) + // ); + + mDriveSim = new FlywheelSim( + DCMotor.getFalcon500(1), + Constants.Swerve.driveGearRatio, + 0.01, + VecBuilder.fill(kEncoderRadiansPerPulse) // Add noise with a std-dev of 1 tick + ); + + mSteeringSim = new SingleJointedArmSim( + LinearSystemId.createSingleJointedArmSystem(DCMotor.getFalcon500(1), 0.001, Constants.Swerve.angleGearRatio), + DCMotor.getFalcon500(1), + Constants.Swerve.angleGearRatio, + 0.0, // Length (m) + Double.NEGATIVE_INFINITY, // Min angle + Double.POSITIVE_INFINITY, // Max angle + false, // Simulate gravity + 0.0, + VecBuilder.fill(kEncoderRadiansPerPulse) // Add noise with a std-dev of 1 tick + ); } public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { @@ -94,15 +165,123 @@ public void resetToAbsolute() { public SwerveModuleState getState(){ return new SwerveModuleState( - Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference), + Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference), Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()) ); } public SwerveModulePosition getPosition(){ return new SwerveModulePosition( - Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), Constants.Swerve.wheelCircumference), + Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), Constants.Swerve.wheelCircumference), Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()) ); } + + // /** Simulate one module with naive physics model. */ + // public void updateSimPeriodic() { + // // Pass the robot battery voltage to the simulated devices + // mDriveMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + // mAngleMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + // angleEncoderSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + // // Simulate drive + // mDriveSim.setInput(mDriveMotorSimState.getMotorVoltage() * RobotController.getBatteryVoltage()); + // // mDriveSim.setInputs(mDriveMotorSimState.getVelocity().getValue() * RobotController.getBatteryVoltage()); + // mDriveSim.update(TimedRobot.kDefaultPeriod); + + // double velocity = mDriveSim.getAngularVelocityRadPerSec() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI); + // mDriveMotorSimState.setRawRotorPosition(velocity * Constants.Swerve.driveGearRatio); + // // angleEncoderSimState.setVelocity(velocity); + + // driveDutyCycle.Output = velocity / Constants.Swerve.maxSpeed; + // mDriveMotor.setControl(driveDutyCycle); + // // else { + // // driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); + // // driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); + // // mDriveMotor.setControl(driveVelocity); + // // } + + // // // Simulate steering + // // mSteeringSim.setInput( + // // mAngleMotor.getPosition().getValue() * RobotController.getBatteryVoltage()); + // // mSteeringSim.update(TimedRobot.kDefaultPeriod); + // // mAngleMotor.setSimSensorPositionAndVelocity( + // // mSteeringSim.getAngleRads(), + // // mSteeringSim.getVelocityRadPerSec(), + // // TimedRobot.kDefaultPeriod, + // // Constants.Swerve.angleGearRatio); + + // // // Update all of our sensors + // // final double leftPos = metersToRotations(mDriveSim.; + // // // This is OK, since the time base is the same + // // final double leftVel = metersToRotations(mDriveMotorSimState.getLeftVelocityMetersPerSecond()); + // // final double rightPos = metersToRotations(mDriveMotorSimState.getRightPositionMeters()); + // // // This is OK, since the time base is the same + // // final double rightVel = metersToRotations(mDriveMotorSimState.getRightVelocityMetersPerSecond()); + // // leftSensSim.setRawPosition(leftPos); + // // leftSensSim.setVelocity(leftVel); + // // rightSensSim.setRawPosition(rightPos); + // // rightSensSim.setVelocity(rightVel); + // // leftSim.setRawRotorPosition(leftPos * this.kGearRatio); + // // leftSim.setRotorVelocity(leftVel * this.kGearRatio); + // // rightSim.setRawRotorPosition(rightPos * this.kGearRatio); + // // rightSim.setRotorVelocity(rightVel * this.kGearRatio); + // } + + /** Simulate one module with naive physics model. */ + public void updateSimPeriodic() { + // Pass the robot battery voltage to the simulated devices + mDriveMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + mAngleMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + angleEncoderSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + // Simulate drive + mDriveSim.setInput(mDriveMotorSimState.getMotorVoltage() * RobotController.getBatteryVoltage()); + // mDriveSim.setInputs(mDriveMotorSimState.getVelocity().getValue() * RobotController.getBatteryVoltage()); + mDriveSim.update(TimedRobot.kDefaultPeriod); + + double driveVelocity = mDriveSim.getAngularVelocityRadPerSec() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI); + mDriveMotorSimState.setRawRotorPosition(driveVelocity * Constants.Swerve.driveGearRatio); + // mDriveMotorSimState.setRotorVelocity(driveVelocity * Constants.Swerve.driveGearRatio); + // angleEncoderSimState.setVelocity(velocity); + + // driveDutyCycle.Output = driveVelocity / Constants.Swerve.maxSpeed; + // mDriveMotor.setControl(driveDutyCycle); + // else { + // driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); + // driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); + // mDriveMotor.setControl(driveVelocity); + // } + + // Simulate steering + mSteeringSim.setInput(mAngleMotorSimState.getMotorVoltage() * RobotController.getBatteryVoltage()); + mSteeringSim.update(TimedRobot.kDefaultPeriod); + + double angle = mSteeringSim.getAngleRads() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI); + double steeringVelocity = mSteeringSim.getVelocityRadPerSec() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI); + + mAngleMotorSimState.setRawRotorPosition(angle); + mAngleMotorSimState.setRotorVelocity(steeringVelocity * Constants.Swerve.driveGearRatio); + // mAngleMotor.setSimSensorPositionAndVelocity( + // mSteeringSim.getAngleRads(), + // mSteeringSim.getVelocityRadPerSec(), + // TimedRobot.kDefaultPeriod, + // Constants.Swerve.angleGearRatio); + + // // Update all of our sensors + // final double leftPos = metersToRotations(mDriveSim.; + // // This is OK, since the time base is the same + // final double leftVel = metersToRotations(mDriveMotorSimState.getLeftVelocityMetersPerSecond()); + // final double rightPos = metersToRotations(mDriveMotorSimState.getRightPositionMeters()); + // // This is OK, since the time base is the same + // final double rightVel = metersToRotations(mDriveMotorSimState.getRightVelocityMetersPerSecond()); + // leftSensSim.setRawPosition(leftPos); + // leftSensSim.setVelocity(leftVel); + // rightSensSim.setRawPosition(rightPos); + // rightSensSim.setVelocity(rightVel); + // leftSim.setRawRotorPosition(leftPos * this.kGearRatio); + // leftSim.setRotorVelocity(leftVel * this.kGearRatio); + // rightSim.setRawRotorPosition(rightPos * this.kGearRatio); + // rightSim.setRotorVelocity(rightVel * this.kGearRatio); + } } \ No newline at end of file From 2135deeff03495f2e9ac562e0a28a29738ec8ed4 Mon Sep 17 00:00:00 2001 From: Ishan Venkatesh <242120@students.srvusd.net> Date: Mon, 30 Sep 2024 17:10:18 -0700 Subject: [PATCH 2/2] Deleting commented code --- .../main/java/frc/robot/subsystems/Drive.java | 31 ----- .../java/frc/robot/subsystems/Module.java | 110 +----------------- 2 files changed, 1 insertion(+), 140 deletions(-) diff --git a/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java b/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java index d30229b..df10cf4 100644 --- a/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java +++ b/1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java @@ -12,17 +12,9 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.sim.Pigeon2SimState; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; -import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; -import edu.wpi.first.wpilibj.simulation.EncoderSim; 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.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrajectoryConfig; @@ -40,13 +32,6 @@ public class Drive extends SubsystemBase { private Trajectory trajectory; private final Field2d field = new Field2d(); - private Pose2d simPose = new Pose2d(); - - // // Simulation - // private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); - // private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); - // private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); - private final Pigeon2SimState gyroSim; public Drive() { gyro = new Pigeon2(Constants.Swerve.pigeonID, "CV"); @@ -77,8 +62,6 @@ public Drive() { // Push the trajectory to Field2d. field.getObject("traj").setTrajectory(trajectory); - - gyroSim = gyro.getSimState(); } public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { @@ -187,20 +170,6 @@ public void simulationPeriodic() { SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue() * 360) % 360 + 360) % 360); // This is super specific, don't break this pls } - // final ChassisSpeeds chassisSpeeds = Constants.Swerve.swerveKinematics.toChassisSpeeds(getModuleStates()); - // simPose = simPose.transformBy(( - // new Transform2d( - // new Translation2d( - // chassisSpeeds.vxMetersPerSecond * TimedRobot.kDefaultPeriod * 100000, - // chassisSpeeds.vyMetersPerSecond * TimedRobot.kDefaultPeriod * 100000), - // // new Translation2d( - // // 20, - // // 20), - // new Rotation2d(chassisSpeeds.omegaRadiansPerSecond * TimedRobot.kDefaultPeriod) - // ) - // )); - // field.setRobotPose(simPose); - swerveOdometry.update(getGyroYaw(), getModulePositions()); field.setRobotPose(swerveOdometry.getPoseMeters()); } diff --git a/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java b/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java index b3f43e0..41d7cdf 100644 --- a/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java +++ b/1458TeamCode/src/main/java/frc/robot/subsystems/Module.java @@ -5,10 +5,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.mechanisms.swerve.SimSwerveDrivetrain; import com.ctre.phoenix6.sim.CANcoderSimState; -import com.ctre.phoenix6.sim.ChassisReference; -import com.ctre.phoenix6.sim.Pigeon2SimState; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.VecBuilder; @@ -16,13 +13,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; -import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; @@ -86,32 +80,12 @@ public Module(int moduleNumber, SwerveModuleConstants moduleConstants){ mDriveMotor.getConfigurator().setPosition(0.0); // For simulation - // encoderSim = new EncoderSim(mAngleMotor); mDriveMotorSimState = mDriveMotor.getSimState(); mAngleMotorSimState = mAngleMotor.getSimState(); angleEncoderSimState = angleEncoder.getSimState(); final double kEncoderRadiansPerPulse = 2.0 * Math.PI / 2048; - // mDriveSim = new DifferentialDrivetrainSim( - // DCMotor.getFalcon500Foc(2), // 2 CIMS on each side of the drivetrain. - // Constants.Swerve.driveGearRatio, // Standard AndyMark Gearing reduction. - // 2.1, // MOI of 2.1 kg m^2 (from CAD model). - // 26.5, // Mass of the robot is 26.5 kg. - // Constants.Swerve.wheelCircumference, // Robot uses 3" radius (6" diameter) wheels. - // 0.546, // Distance between wheels is _ meters. - - // /* - // * The standard deviations for measurement noise: - // * x and y: 0.001 m - // * heading: 0.001 rad - // * l and r velocity: 0.1 m/s - // * l and r position: 0.005 m - // */ - // /* Uncomment the following line to add measurement noise. */ - // null // VecBuilder.fill(0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005) - // ); - mDriveSim = new FlywheelSim( DCMotor.getFalcon500(1), Constants.Swerve.driveGearRatio, @@ -176,58 +150,7 @@ public SwerveModulePosition getPosition(){ Rotation2d.fromRotations(mAngleMotor.getPosition().getValue()) ); } - - // /** Simulate one module with naive physics model. */ - // public void updateSimPeriodic() { - // // Pass the robot battery voltage to the simulated devices - // mDriveMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - // mAngleMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - // angleEncoderSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - - // // Simulate drive - // mDriveSim.setInput(mDriveMotorSimState.getMotorVoltage() * RobotController.getBatteryVoltage()); - // // mDriveSim.setInputs(mDriveMotorSimState.getVelocity().getValue() * RobotController.getBatteryVoltage()); - // mDriveSim.update(TimedRobot.kDefaultPeriod); - - // double velocity = mDriveSim.getAngularVelocityRadPerSec() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI); - // mDriveMotorSimState.setRawRotorPosition(velocity * Constants.Swerve.driveGearRatio); - // // angleEncoderSimState.setVelocity(velocity); - - // driveDutyCycle.Output = velocity / Constants.Swerve.maxSpeed; - // mDriveMotor.setControl(driveDutyCycle); - // // else { - // // driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); - // // driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); - // // mDriveMotor.setControl(driveVelocity); - // // } - - // // // Simulate steering - // // mSteeringSim.setInput( - // // mAngleMotor.getPosition().getValue() * RobotController.getBatteryVoltage()); - // // mSteeringSim.update(TimedRobot.kDefaultPeriod); - // // mAngleMotor.setSimSensorPositionAndVelocity( - // // mSteeringSim.getAngleRads(), - // // mSteeringSim.getVelocityRadPerSec(), - // // TimedRobot.kDefaultPeriod, - // // Constants.Swerve.angleGearRatio); - - // // // Update all of our sensors - // // final double leftPos = metersToRotations(mDriveSim.; - // // // This is OK, since the time base is the same - // // final double leftVel = metersToRotations(mDriveMotorSimState.getLeftVelocityMetersPerSecond()); - // // final double rightPos = metersToRotations(mDriveMotorSimState.getRightPositionMeters()); - // // // This is OK, since the time base is the same - // // final double rightVel = metersToRotations(mDriveMotorSimState.getRightVelocityMetersPerSecond()); - // // leftSensSim.setRawPosition(leftPos); - // // leftSensSim.setVelocity(leftVel); - // // rightSensSim.setRawPosition(rightPos); - // // rightSensSim.setVelocity(rightVel); - // // leftSim.setRawRotorPosition(leftPos * this.kGearRatio); - // // leftSim.setRotorVelocity(leftVel * this.kGearRatio); - // // rightSim.setRawRotorPosition(rightPos * this.kGearRatio); - // // rightSim.setRotorVelocity(rightVel * this.kGearRatio); - // } - + /** Simulate one module with naive physics model. */ public void updateSimPeriodic() { // Pass the robot battery voltage to the simulated devices @@ -242,16 +165,6 @@ public void updateSimPeriodic() { double driveVelocity = mDriveSim.getAngularVelocityRadPerSec() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI); mDriveMotorSimState.setRawRotorPosition(driveVelocity * Constants.Swerve.driveGearRatio); - // mDriveMotorSimState.setRotorVelocity(driveVelocity * Constants.Swerve.driveGearRatio); - // angleEncoderSimState.setVelocity(velocity); - - // driveDutyCycle.Output = driveVelocity / Constants.Swerve.maxSpeed; - // mDriveMotor.setControl(driveDutyCycle); - // else { - // driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); - // driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); - // mDriveMotor.setControl(driveVelocity); - // } // Simulate steering mSteeringSim.setInput(mAngleMotorSimState.getMotorVoltage() * RobotController.getBatteryVoltage()); @@ -262,26 +175,5 @@ public void updateSimPeriodic() { mAngleMotorSimState.setRawRotorPosition(angle); mAngleMotorSimState.setRotorVelocity(steeringVelocity * Constants.Swerve.driveGearRatio); - // mAngleMotor.setSimSensorPositionAndVelocity( - // mSteeringSim.getAngleRads(), - // mSteeringSim.getVelocityRadPerSec(), - // TimedRobot.kDefaultPeriod, - // Constants.Swerve.angleGearRatio); - - // // Update all of our sensors - // final double leftPos = metersToRotations(mDriveSim.; - // // This is OK, since the time base is the same - // final double leftVel = metersToRotations(mDriveMotorSimState.getLeftVelocityMetersPerSecond()); - // final double rightPos = metersToRotations(mDriveMotorSimState.getRightPositionMeters()); - // // This is OK, since the time base is the same - // final double rightVel = metersToRotations(mDriveMotorSimState.getRightVelocityMetersPerSecond()); - // leftSensSim.setRawPosition(leftPos); - // leftSensSim.setVelocity(leftVel); - // rightSensSim.setRawPosition(rightPos); - // rightSensSim.setVelocity(rightVel); - // leftSim.setRawRotorPosition(leftPos * this.kGearRatio); - // leftSim.setRotorVelocity(leftVel * this.kGearRatio); - // rightSim.setRawRotorPosition(rightPos * this.kGearRatio); - // rightSim.setRotorVelocity(rightVel * this.kGearRatio); } } \ No newline at end of file