From 810885ad9ac9e7ca3f51639c432ab4a861f6a229 Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Wed, 24 Jan 2024 15:15:28 -0500 Subject: [PATCH] Merge SwerveDrive into Vision/Odometry (#8) * use CANcoders Co-authored-by: simon Co-authored-by: Tmanxyz Co-authored-by: dchen60 Co-authored-by: alex Co-authored-by: urnotkool Co-authored-by: rzheng287 * remove used imports in swerve * Finish periodic after getting odometry logic * add SimModules * Refactor identifyPositionVelocitySystem into PositionVelocitySystem.java --------- Co-authored-by: Keobkeig Co-authored-by: simon Co-authored-by: Tmanxyz Co-authored-by: dchen60 Co-authored-by: alex Co-authored-by: urnotkool Co-authored-by: rzheng287 Co-authored-by: Richie_Xue --- .../stuypulse/robot/constants/Settings.java | 46 +++++-- .../swerve/AbstractSwerveDrive.java | 25 +++- .../robot/subsystems/swerve/SwerveDrive.java | 47 +++++-- .../{ => modules}/AbstractSwerveModule.java | 3 +- .../subsystems/swerve/modules/SimModule.java | 128 ++++++++++++++++++ .../swerve/{ => modules}/SwerveModule.java | 34 ++--- .../robot/util/PositionVelocitySystem.java | 69 ++++++++++ 7 files changed, 304 insertions(+), 48 deletions(-) rename src/main/java/com/stuypulse/robot/subsystems/swerve/{ => modules}/AbstractSwerveModule.java (89%) create mode 100644 src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java rename src/main/java/com/stuypulse/robot/subsystems/swerve/{ => modules}/SwerveModule.java (72%) create mode 100644 src/main/java/com/stuypulse/robot/util/PositionVelocitySystem.java diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3f97088d..8cb9cbd8 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -36,37 +36,35 @@ public interface Drive { double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; double GEAR_RATIO = 1.0 / 4.71; //TODO: Find Gear Ratio - //TODO: CHECK THESE double POSITION_CONVERSION = WHEEL_CIRCUMFERENCE * GEAR_RATIO; double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; } public interface Turn { - //TODO: CHECK THESE double POSITION_CONVERSION = 1; double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; } } public interface Controller { - //TODO: CHECK THESE public interface Turn { SmartNumber kP = new SmartNumber("Swerve/Turn/kP", 1.0); SmartNumber kI = new SmartNumber("Swerve/Turn/kI", 0.0); SmartNumber kD = new SmartNumber("Swerve/Turn/kD", 0.0); - SmartNumber kS = new SmartNumber("Swerve/Turn/kS", 0.25); - SmartNumber kV = new SmartNumber("Swerve/Turn/kV", 0.00); + SmartNumber kS = new SmartNumber("Swerve/Turn/kS", 0.01); + SmartNumber kV = new SmartNumber("Swerve/Turn/kV", 0.25); + SmartNumber kA = new SmartNumber("Swerve/Turn/kA", 0.01); } public interface Drive { - SmartNumber kP = new SmartNumber("Swerve/Drive/kP", 0.10); + SmartNumber kP = new SmartNumber("Swerve/Drive/kP", 1.0); SmartNumber kI = new SmartNumber("Swerve/Drive/kI", 0.00); SmartNumber kD = new SmartNumber("Swerve/Drive/kD", 0.00); - SmartNumber kS = new SmartNumber("Swerve/Drive/kS", 0.25); - SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 0.00); - SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0.00); + SmartNumber kS = new SmartNumber("Swerve/Drive/kS", 0.01); + SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 0.25); + SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0.01); } } @@ -121,4 +119,34 @@ public interface Rotation { SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.1); } } + + public interface Driver { + public interface Drive { + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); + + SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.01); + SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); + + SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_MODULE_SPEED.get()); + SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 15); + } + + public interface Turn { + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); + + SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.1); + SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); + + SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 6.0); + + public interface GyroFeedback { + SmartBoolean GYRO_FEEDBACK_ENABLED = new SmartBoolean("Driver Settings/Gyro Feedback/Enabled", true); + + SmartNumber P = new SmartNumber("Driver Settings/Gyro Feedback/kP", 0.5); + SmartNumber I = new SmartNumber("Driver Settings/Gyro Feedback/kI", 0.0); + SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1); + } + } + + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveDrive.java index 52ab631e..a6cac7f8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveDrive.java @@ -5,6 +5,8 @@ import com.stuypulse.robot.constants.Settings.Swerve.BackRight; import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft; import com.stuypulse.robot.constants.Settings.Swerve.FrontRight; +import com.stuypulse.robot.subsystems.swerve.modules.SimModule; +import com.stuypulse.robot.subsystems.swerve.modules.SwerveModule; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -12,6 +14,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,12 +23,22 @@ public abstract class AbstractSwerveDrive extends SubsystemBase { private static final AbstractSwerveDrive instance; static { - instance = new SwerveDrive( - new SwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, Ports.Swerve.FrontRight.TURN, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE), - new SwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, Ports.Swerve.FrontLeft.TURN, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE), - new SwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, Ports.Swerve.BackLeft.TURN, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE), - new SwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, Ports.Swerve.BackRight.TURN, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE) - ); + if (RobotBase.isSimulation()) { + instance = new SwerveDrive( + new SimModule(FrontRight.ID, FrontRight.MODULE_OFFSET), + new SimModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET), + new SimModule(BackLeft.ID, BackLeft.MODULE_OFFSET), + new SimModule(BackRight.ID, BackRight.MODULE_OFFSET) + ); + } + else { + instance = new SwerveDrive( + new SwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, Ports.Swerve.FrontRight.TURN, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE), + new SwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, Ports.Swerve.FrontLeft.TURN, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE), + new SwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, Ports.Swerve.BackLeft.TURN, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE), + new SwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, Ports.Swerve.BackRight.TURN, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE) + ); + } } public static AbstractSwerveDrive getInstance() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 45ef63dd..a6d25801 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -1,9 +1,10 @@ package com.stuypulse.robot.subsystems.swerve; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; -import com.ctre.phoenix6.sim.ChassisReference; import com.kauailabs.navx.frc.AHRS; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.modules.AbstractSwerveModule; import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.geometry.Pose2d; @@ -63,6 +64,11 @@ public class SwerveDrive extends AbstractSwerveDrive { private AHRS gyro; private FieldObject2d[] modules2D; + + /** + * Creates a new Swerve Drive using the provided modules + * @param modules the modules to use + */ public SwerveDrive(AbstractSwerveModule... modules){ this.modules = modules; this.kinematics = new SwerveDriveKinematics(getModuleOffsets()); @@ -76,7 +82,9 @@ public void initFieldObject(Field2d field) { modules2D[i] = field.getObject(modules[i].getId() + "-2d"); } } - + + + /*Getters */ @Override public SwerveDriveKinematics getKinematics() { return kinematics; @@ -117,7 +125,7 @@ public ChassisSpeeds getChassisSpeeds() { /*Setters */ private static SwerveModuleState filterModuleState(SwerveModuleState state) { - if (Math.abs(state.speedMetersPerSecond) > Settings.Swerve.MODULE_VELOCITY_DEADBAND.get()) + if (Math.abs(state.speedMetersPerSecond) > Swerve.MODULE_VELOCITY_DEADBAND.get()) return state; return new SwerveModuleState(0, state.angle); @@ -129,7 +137,7 @@ public void setModuleStates(SwerveModuleState[] states) { throw new IllegalArgumentException("Provided incorrect number of states for swerve drive modules"); } - SwerveDriveKinematics.desaturateWheelSpeeds(states, Settings.Swerve.MAX_MODULE_SPEED.get()); + SwerveDriveKinematics.desaturateWheelSpeeds(states, Swerve.MAX_MODULE_SPEED.get()); for (int i = 0; i < modules.length; i++){ modules[i].setState(filterModuleState(states[i])); @@ -141,12 +149,13 @@ public void setChassisSpeed(ChassisSpeeds robotSpeeds) { setModuleStates(kinematics.toSwerveModuleStates(robotSpeeds)); } + /*Drive Functions*/ @Override public void drive(Vector2D velocity, double rotation) { ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( velocity.x, -velocity.y, rotation, - getGyroAngle() + Odometry.getInstance().getPose().getRotation() ); Pose2d pose = new Pose2d( @@ -189,15 +198,31 @@ public double getForwardAccelerationGs() { @Override public void periodic() { - /*XXX: WAITING FOR ODOMETRY TO WRITE PERIODIC */ + Odometry odometry = Odometry.getInstance(); + Pose2d pose = odometry.getPose(); + Rotation2d angle = pose.getRotation(); - SmartDashboard.putNumber("Swerve/Gyro Angle (deg)", getGyroAngle().getDegrees()); - SmartDashboard.putNumber("Swerve/Gyro Pitch", getGyroPitch().getDegrees()); - SmartDashboard.putNumber("Swerve/Gyro Roll", getGyroRoll().getDegrees()); + for (int i = 0; i < modules.length; i++) { + modules2D[i].setPose(new Pose2d( + pose.getTranslation().plus(modules[i].getModuleOffset().rotateBy(angle)), + modules[i].getAngle().plus(angle) + )); + } + + SmartDashboard.putNumber("Swerve/Gyro/Angle (deg)", getGyroAngle().getDegrees()); + SmartDashboard.putNumber("Swerve/Gyro/Pitch (deg)", getGyroPitch().getDegrees()); + SmartDashboard.putNumber("Swerve/Gyro/Roll (deg)", getGyroRoll().getDegrees()); - SmartDashboard.putNumber("Swerve/Forward Acceleration (Gs)", getForwardAccelerationGs()); + SmartDashboard.putNumber("Swerve/Forward Acceleration (Gs)", getForwardAccelerationGs()); SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getWorldLinearAccelX()); SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getWorldLinearAccelY()); SmartDashboard.putNumber("Swerve/Z Acceleration (Gs)", gyro.getWorldLinearAccelZ()); } + + @Override + public void simulationPeriodic() { + //show gyro angle in simulation + ChassisSpeeds speeds = kinematics.toChassisSpeeds(getModuleStates()); + gyro.setAngleAdjustment(speeds.omegaRadiansPerSecond * Settings.DT); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/AbstractSwerveModule.java similarity index 89% rename from src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveModule.java rename to src/main/java/com/stuypulse/robot/subsystems/swerve/modules/AbstractSwerveModule.java index 995a1a2a..b7ced329 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/AbstractSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/AbstractSwerveModule.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.subsystems.swerve; +package com.stuypulse.robot.subsystems.swerve.modules; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -14,7 +14,6 @@ public abstract class AbstractSwerveModule extends SubsystemBase { public abstract Translation2d getModuleOffset(); public abstract SwerveModulePosition getModulePosition(); public abstract void setState(SwerveModuleState state); - public abstract void periodic(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java new file mode 100644 index 00000000..be7d8f16 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SimModule.java @@ -0,0 +1,128 @@ +package com.stuypulse.robot.subsystems.swerve.modules;import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Swerve.Controller.Drive; +import com.stuypulse.robot.constants.Settings.Swerve.Controller.Turn; +import com.stuypulse.robot.util.PositionVelocitySystem; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.ARateLimit; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; + +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SimModule extends AbstractSwerveModule { + private String id; + + private Translation2d offset; + + private SwerveModuleState state; + + private Controller driveController; + private AngleController angleController; + + private LinearSystemSim driveSim; + private LinearSystemSim turnSim; + + public SimModule(String id, Translation2d offset) { + this.id = id; + this.offset = offset; + + this.state = new SwerveModuleState(); + + this.driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD) + .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity()); + + this.angleController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD) + .setSetpointFilter(new ARateLimit(Swerve.MAX_TURNING)); + + this.driveSim = new PositionVelocitySystem(Drive.kV, Drive.kA).getSim(); + this.turnSim = new LinearSystemSim(LinearSystemId.identifyPositionSystem(Turn.kV.get(), Turn.kA.get())); + } + + @Override + public double getVelocity() { + return driveSim.getOutput(1); + } + + public double getDistance() { + return driveSim.getOutput(0); + } + + @Override + public Rotation2d getAngle() { + return Rotation2d.fromRadians(turnSim.getOutput(0)); + } + + @Override + public String getId() { + return this.id; + } + + @Override + public SwerveModuleState getState() { + return state; + } + + @Override + public Translation2d getModuleOffset() { + return offset; + } + + @Override + public SwerveModulePosition getModulePosition() { + return new SwerveModulePosition(getDistance(), getAngle()); + } + + @Override + public void setState(SwerveModuleState state) { + this.state = state; + } + + @Override + public void periodic() { + driveController.update( + state.speedMetersPerSecond, + getVelocity() + ); + + angleController.update( + Angle.fromRotation2d(state.angle), + Angle.fromRotation2d(getAngle()) + ); + + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Drive Voltage", driveController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Turn Voltage", angleController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Target Angle", state.angle.getDegrees()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Angle", getAngle().getDegrees()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Target Speed", state.speedMetersPerSecond); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Speed", getVelocity()); + } + + @Override + public void simulationPeriodic() { + driveSim.setInput(driveController.getOutput()); + driveSim.update(Settings.DT); + + turnSim.setInput(angleController.getOutput()); + turnSim.update(Settings.DT); + + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage( + turnSim.getCurrentDrawAmps() + driveSim.getCurrentDrawAmps() + )); + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java similarity index 72% rename from src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModule.java rename to src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java index 2e09a8e1..abf313ab 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java @@ -1,10 +1,9 @@ -package com.stuypulse.robot.subsystems.swerve; +package com.stuypulse.robot.subsystems.swerve.modules; +import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.SparkAbsoluteEncoder.Type; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.control.Controller; @@ -57,7 +56,7 @@ public class SwerveModule extends AbstractSwerveModule { private CANSparkMax driveMotor; private RelativeEncoder driveEncoder; - private SparkAbsoluteEncoder turnEncoder; + private CANcoder turnEncoder; private Controller driveController; private AngleController angleController; @@ -84,9 +83,7 @@ public SwerveModule(String id, Translation2d offset, int driveId, Rotation2d ang driveEncoder.setPositionConversionFactor(Settings.Swerve.Encoder.Drive.POSITION_CONVERSION); driveEncoder.setVelocityConversionFactor(Settings.Swerve.Encoder.Drive.VELOCITY_CONVERSION); - this.turnEncoder = turnMotor.getAbsoluteEncoder(Type.kDutyCycle); - turnEncoder.setPositionConversionFactor(Settings.Swerve.Encoder.Turn.POSITION_CONVERSION); - turnEncoder.setVelocityConversionFactor(Settings.Swerve.Encoder.Turn.VELOCITY_CONVERSION); + this.turnEncoder = new CANcoder(turnId); this.driveController = new PIDController(Settings.Swerve.Controller.Drive.kP, Settings.Swerve.Controller.Drive.kI, Settings.Swerve.Controller.Drive.kD) .add(new MotorFeedforward(Settings.Swerve.Controller.Drive.kS, Settings.Swerve.Controller.Drive.kV, Settings.Swerve.Controller.Drive.kA).velocity()); @@ -94,7 +91,6 @@ public SwerveModule(String id, Translation2d offset, int driveId, Rotation2d ang this.angleController = new AnglePIDController(Settings.Swerve.Controller.Turn.kP, Settings.Swerve.Controller.Turn.kI, Settings.Swerve.Controller.Turn.kD); this.driveEncoder.setPosition(0); - this.turnEncoder.setZeroOffset(this.angleOffset.getRotations()); Motors.Swerve.DRIVE_CONFIG.configure(driveMotor); Motors.Swerve.TURN_CONFIG.configure(turnMotor); @@ -112,7 +108,7 @@ public double getVelocity() { @Override public Rotation2d getAngle() { - return Rotation2d.fromRotations(turnEncoder.getPosition()); + return Rotation2d.fromRotations(turnEncoder.getAbsolutePosition().getValueAsDouble()).minus(angleOffset); } @Override @@ -139,19 +135,17 @@ public void setState(SwerveModuleState state) { public void periodic() { // Update Controllers driveMotor.setVoltage(driveController.update(state.speedMetersPerSecond, getVelocity())); + turnMotor.setVoltage(angleController.update(Angle.fromRotation2d(state.angle), Angle.fromRotation2d(getAngle()))); //Logging - SmartDashboard.putNumber("Swerve/" + id + "/Raw Angle (deg)", Units.rotationsToDegrees(turnEncoder.getPosition())); - SmartDashboard.putNumber("Swerve/" + id + "/Target Angle", state.angle.getDegrees()); - SmartDashboard.putNumber("Swerve/" + id + "/Angle", getAngle().getDegrees()); - SmartDashboard.putNumber("Swerve/" + id + "/Angle Error", angleController.getError().toDegrees()); - SmartDashboard.putNumber("Swerve/" + id + "/Angle Voltage", angleController.getOutput()); - SmartDashboard.putNumber("Swerve/" + id + "/Angle Current", turnMotor.getOutputCurrent()); - SmartDashboard.putNumber("Swerve/" + id + "/Target Velocity", state.speedMetersPerSecond); - SmartDashboard.putNumber("Swerve/" + id + "/Velocity", getVelocity()); - SmartDashboard.putNumber("Swerve/" + id + "/Velocity Error", driveController.getError()); - SmartDashboard.putNumber("Swerve/" + id + "/Velocity Voltage", driveController.getOutput()); - SmartDashboard.putNumber("Swerve/" + id + "/Velocity Current", driveMotor.getOutputCurrent()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Drive Voltage", driveController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Turn Voltage", angleController.getOutput()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Angle Error", angleController.getError().toDegrees()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Target Angle", state.angle.getDegrees()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Angle", getAngle().getDegrees()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Target Speed", state.speedMetersPerSecond); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Speed", getVelocity()); + SmartDashboard.putNumber("Swerve/Modules/" + id + "/Raw Encoder Angle", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble())); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/PositionVelocitySystem.java b/src/main/java/com/stuypulse/robot/util/PositionVelocitySystem.java new file mode 100644 index 00000000..5bee8531 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/PositionVelocitySystem.java @@ -0,0 +1,69 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; + +public class PositionVelocitySystem { + private double kV; + private double kA; + + private LinearSystem system; + private LinearSystemSim sim; + + /** + * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA (volts/(unit/sec²). + *
  • States: [position, velocity]ᵀ + *
  • Inputs [voltage] + *
  • Outputs [position, velocity] + * + *

    The distance unit MUST either meters or radians. + *

    The parameters provided by the user are from this feedforward model: + *

    u = K_v v + K_a a + * + * @param kV The velocity gain, in volts/(unit/sec) + * @param kA The acceleration gain, in volts/(unit/sec²) + * @throws IllegalArgumentException if kV <= 0 or kA <= 0. + */ + public PositionVelocitySystem(SmartNumber kV, SmartNumber kA) { + if (kV.get() <= 0.0) { + throw new IllegalArgumentException("kV must greater than zero"); + } + if (kA.get() <= 0.0) { + throw new IllegalArgumentException("kA must be greater than zero"); + } + + this.kV = kV.get(); + this.kA = kA.get(); + + this.system = new LinearSystem ( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -this.kV / this.kA), + MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / this.kA), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), + MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0) + ); + + this.sim = new LinearSystemSim(this.system); + } + + /** + * Gets the linear system. + * @return A LinearSystem representing the given characterized constants. + */ + public LinearSystem getSystem() { + return this.system; + } + + /** + * Gets the simulation of the system. + * @return A LinearSystemSim representing the simulation of the system. + */ + public LinearSystemSim getSim() { + return this.sim; + } +}