From 9dc17b5c84d31f150131e25599ff0c4ef5613ee7 Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Mon, 28 Oct 2024 11:44:49 -0400 Subject: [PATCH] bruh --- .../swerve/moduleIO/PhysicalModule.java | 24 ++++++- .../swerve/moduleIO/SimulatedModule.java | 71 ++++++++++++++++--- 2 files changed, 81 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java index f097652..854f892 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java @@ -202,13 +202,13 @@ public double getDriveVoltage() { @Override public void setDesiredState(SwerveModuleState desiredState) { - double turnRadians = getTurnRotations(); + double turnRotations = getTurnRotations(); // Optimize the reference state to avoid spinning further than 90 degrees SwerveModuleState setpoint = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); - setpoint.optimize(Rotation2d.fromRotations(turnRadians)); - setpoint.cosineScale(Rotation2d.fromRotations(turnRadians)); + setpoint.optimize(Rotation2d.fromRotations(turnRotations)); + setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { driveMotor.set(0); @@ -252,4 +252,22 @@ public void stopModule() { public double getDrivePosition() { return driveMotor.getPosition().refresh().getValueAsDouble(); } + + @Override + public String getCANBus() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getCANBus'"); + } + + @Override + public void setDriveSpeed(double speedPercent) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setDriveSpeed'"); + } + + @Override + public void setTurnSpeed(double powerPercent) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setTurnSpeed'"); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java index 26e4e8c..7817ae4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java @@ -3,10 +3,14 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swervePhysicsSim.SwerveModuleSimulation; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; + import java.util.Arrays; /** Wrapper class around {@link SwerveModuleSimulation} that implements ModuleIO */ @@ -84,12 +88,15 @@ public double getDrivePosition() { } public void setDesiredState(SwerveModuleState desiredState) { - Rotation2d turnRotations = getTurnRotations(); + double turnRotations = getTurnRotations(); // Optimize the reference state to avoid spinning further than 90 degrees - SwerveModuleState optimizedDesiredState = - SwerveModuleState.optimize(desiredState, turnRotations); + SwerveModuleState setpoint = + new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); + + setpoint.optimize(Rotation2d.fromRotations(turnRotations)); + setpoint.cosineScale(Rotation2d.fromRotations(turnRotations)); - if (Math.abs(optimizedDesiredState.speedMetersPerSecond) < 0.01) { + if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) { moduleSimulation.requestDriveVoltageOut(0); moduleSimulation.requestTurnVoltageOut(0); return; @@ -97,23 +104,65 @@ public void setDesiredState(SwerveModuleState desiredState) { // Converts meters per second to rotations per second double desiredDriveRPS = - optimizedDesiredState.speedMetersPerSecond + setpoint.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; moduleSimulation.requestDriveVoltageOut( drivePID.calculate( Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()), - desiredDriveRPS) - + driveFF.calculate(desiredDriveRPS)); + desiredDriveRPS)); + // + driveFF.calculate(desiredDriveRPS)); moduleSimulation.requestTurnVoltageOut( turnPID.calculate( moduleSimulation.getTurnAbsolutePosition().getRotations(), - desiredState.angle.getRotations()) - + turnFF.calculate(turnPID.getSetpoint().velocity)); + desiredState.angle.getRotations())); + // + turnFF.calculate(turnPID.getSetpoint().velocity)); + } + + public double getTurnRotations() { + return moduleSimulation.getTurnAbsolutePosition().getRotations(); + } + + @Override + public String getCANBus() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getCANBus'"); + } + + @Override + public void setDriveSpeed(double speedPercent) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setDriveSpeed'"); + } + + @Override + public void setTurnSpeed(double powerPercent) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setTurnSpeed'"); + } + + @Override + public void setDriveBrake(boolean enable) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setDriveBrake'"); } - public Rotation2d getTurnRotations() { - return Rotation2d.fromRotations(moduleSimulation.getTurnAbsolutePosition()); + @Override + public void setTurnBrake(boolean enable) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setTurnBrake'"); + } + + @Override + public void stopModule() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'stopModule'"); + } + + @Override + public double getDriveVoltage() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getDriveVoltage'"); } }