From 18dbf67dc03d4011ece74621dcc56661b07d557f Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 1 Nov 2022 17:43:01 -0600
Subject: [PATCH] Churro
---
.idea/.gitignore | 3 +
.idea/gradle.xml | 17 +
.idea/misc.xml | 4 +
.idea/vcs.xml | 6 +
src/main/java/frc4388/robot/Constants.java | 46 +++
.../frc4388/robot/subsystems/SwerveDrive.java | 315 ++++++++++++++++++
.../robot/subsystems/SwerveModule.java | 181 ++++++++++
7 files changed, 572 insertions(+)
create mode 100644 .idea/.gitignore
create mode 100644 .idea/gradle.xml
create mode 100644 .idea/misc.xml
create mode 100644 .idea/vcs.xml
create mode 100644 src/main/java/frc4388/robot/subsystems/SwerveDrive.java
create mode 100644 src/main/java/frc4388/robot/subsystems/SwerveModule.java
diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..26d3352
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml
diff --git a/.idea/gradle.xml b/.idea/gradle.xml
new file mode 100644
index 0000000..4edc9d5
--- /dev/null
+++ b/.idea/gradle.xml
@@ -0,0 +1,17 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..6ed36dd
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..35eb1dd
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index d92d66b..84bd57a 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -18,6 +18,52 @@ import frc4388.utility.LEDPatterns;
* constants are needed, to reduce verbosity.
*/
public final class Constants {
+ public static final class SwerveDriveConstants {
+ public static final double ROTATION_SPEED = 0.1;
+ public static final double WHEEL_SPEED = 0.1;
+ public static final double WIDTH = 22;
+ public static final double HEIGHT = 22;
+ public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
+ public static final double MAX_SPEED_FEET_PER_SEC = 16;
+ public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
+ public static final int LEFT_FRONT_STEER_CAN_ID = 2;
+ public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
+ public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
+ public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
+ public static final int LEFT_BACK_STEER_CAN_ID = 6;
+ public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
+ public static final int RIGHT_BACK_STEER_CAN_ID = 8;
+ public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
+ public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
+ public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
+ public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
+ public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
+ // ofsets are in degrees
+ //ofsets are in degrees
+ // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
+ // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
+ // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
+ // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
+ public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
+ public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
+ public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
+ public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
+
+ // swerve PID constants
+ public static final int SWERVE_SLOT_IDX = 0;
+ public static final int SWERVE_PID_LOOP_IDX = 1;
+ public static final int SWERVE_TIMEOUT_MS = 30;
+ public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
+
+ // swerve configuration
+ public static final double NEUTRAL_DEADBAND = 0.04;
+ public static final double OPEN_LOOP_RAMP_RATE = 0.2;
+ public static final int REMOTE_0 = 0;
+
+ // misc
+ public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+ }
+
public static final class DriveConstants {
public static final int DRIVE_LEFT_FRONT_CAN_ID = 2;
public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4;
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
new file mode 100644
index 0000000..34687d9
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -0,0 +1,315 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+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;
+import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+
+public class SwerveDrive extends SubsystemBase {
+
+ private SwerveModule m_leftFront;
+ private SwerveModule m_leftBack;
+ private SwerveModule m_rightFront;
+ private SwerveModule m_rightBack;
+
+ double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
+ double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
+
+ public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
+
+ Translation2d m_frontLeftLocation =
+ new Translation2d(
+ Units.inchesToMeters(halfHeight),
+ Units.inchesToMeters(halfWidth));
+ Translation2d m_frontRightLocation =
+ new Translation2d(
+ Units.inchesToMeters(halfHeight),
+ Units.inchesToMeters(-halfWidth));
+ Translation2d m_backLeftLocation =
+ new Translation2d(
+ Units.inchesToMeters(-halfHeight),
+ Units.inchesToMeters(halfWidth));
+ Translation2d m_backRightLocation =
+ new Translation2d(
+ Units.inchesToMeters(-halfHeight),
+ Units.inchesToMeters(-halfWidth));
+
+ public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
+ m_backLeftLocation, m_backRightLocation);
+
+ public SwerveModule[] modules;
+ public WPI_Pigeon2 m_gyro;
+
+ public SwerveDriveOdometry m_odometry;
+ // public SwerveDriveOdometry m_odometry;
+
+ public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+ public boolean ignoreAngles;
+ public Rotation2d rotTarget = new Rotation2d();
+ private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+
+ private final Field2d m_field = new Field2d();
+
+ public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
+ WPI_Pigeon2 gyro) {
+
+ m_leftFront = leftFront;
+ m_leftBack = leftBack;
+ m_rightFront = rightFront;
+ m_rightBack = rightBack;
+ m_gyro = gyro;
+
+ modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
+
+ // m_poseEstimator = new SwerveDrivePoseEstimator(
+ // getRegGyro(),//m_gyro.getRotation2d(),
+ // new Pose2d(),
+ // m_kinematics,
+ // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune
+ // VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
+ // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
+
+ m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
+
+ m_gyro.reset();
+ SmartDashboard.putData("Field", m_field);
+ }
+
+ public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
+ Translation2d speed = new Translation2d(speedX, speedY);
+ driveWithInput(speed, rot, fieldRelative);
+ }
+
+ /**
+ * Method to drive the robot using joystick info.
+ * @link https://github.com/ZachOrr/MK3-Swerve-Example
+ * @param speeds[0] Speed of the robot in the x direction (forward).
+ * @param speeds[1] Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the
+ * field.
+ */
+ public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) {
+ ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
+
+ double mag = speed.getNorm();
+ speed = speed.times(mag * speedAdjust);
+
+ double xSpeedMetersPerSecond = speed.getX();
+ double ySpeedMetersPerSecond = speed.getY();
+ chassisSpeeds = fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+ -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
+ : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
+ -rot * SwerveDriveConstants.ROTATION_SPEED * 2);
+ SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
+ setModuleStates(states);
+ }
+
+ public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
+ Translation2d speed = new Translation2d(leftX, leftY);
+ Translation2d head = new Translation2d(rightX, rightY);
+ driveWithInput(speed, head, fieldRelative);
+ }
+
+ // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
+ public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+ ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0;
+ leftStick = leftStick.times(leftStick.getNorm() * speedAdjust);
+ if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND)
+ rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0,1));
+ double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
+ if (ignoreAngles) {
+ rot = 0;
+ }
+ double xSpeedMetersPerSecond = leftStick.getX();
+ double ySpeedMetersPerSecond = leftStick.getY();
+ chassisSpeeds = fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+ rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
+ : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
+ SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
+ chassisSpeeds);
+ // if (ignoreAngles) {
+ // SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
+ // for (int i = 0; i < states.length; i ++) {
+ // SwerveModuleState state = states[i];
+ // lockedStates[i]= new SwerveModuleState(0, state.angle);
+ // }
+ // setModuleStates(lockedStates);
+ // }
+ setModuleStates(states);
+ // SmartDashboard.putNumber("rot", rot);
+ // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
+ }
+
+ /**
+ * Set each module of the swerve drive to the corresponding desired state.
+ *
+ * @param desiredStates Array of module states to set.
+ */
+ public void setModuleStates(SwerveModuleState[] desiredStates) {
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
+ Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
+ // int i = 2; {
+ for (int i = 0; i < desiredStates.length; i++) {
+ SwerveModule module = modules[i];
+ SwerveModuleState state = desiredStates[i];
+ module.setDesiredState(state, ignoreAngles);
+ }
+ // modules[0].setDesiredState(desiredStates[0], false);
+ }
+
+ public void setModuleRotationsToAngle(double angle) {
+ for (int i = 0; i < modules.length; i++) {
+ SwerveModule module = modules[i];
+ module.rotateToAngle(angle);
+ }
+ }
+
+ @Override
+ public void periodic() {
+ updateOdometry();
+ updateSmartDash();
+
+ // SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
+ // SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
+ // SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
+ // SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
+
+ m_field.setRobotPose(getOdometry());
+ super.periodic();
+ }
+
+ private void updateSmartDash() {
+ // odometry
+ SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
+ SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
+ SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
+
+ // chassis speeds
+ // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
+ // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
+ // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
+ // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
+ }
+
+ /**
+ * Gets the current chassis speeds in m/s and rad/s.
+ * @return Current chassis speeds (vx, vy, ω)
+ */
+ public ChassisSpeeds getChassisSpeeds() {
+ return chassisSpeeds;
+ }
+
+ /**
+ * Gets the current pose of the robot.
+ *
+ * @return Robot's current pose.
+ */
+ public Pose2d getOdometry() {
+ // return m_odometry.getPoseMeters();
+ return m_odometry.getPoseMeters();
+ // return m_poseEstimator.getEstimatedPosition();
+ }
+
+ public Pose2d getAutoOdo() {
+ Pose2d workingPose = getOdometry();
+ return new Pose2d(-workingPose.getX(), workingPose.getY(), workingPose.getRotation());
+ }
+
+ /**
+ * Gets the current gyro using regression formula.
+ *
+ * @return Rotation2d object holding current gyro in radians
+ */
+ public Rotation2d getRegGyro() {
+ // * test chassis regression
+ // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
+ // * new robot regression
+ double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743;
+ return new Rotation2d(Math.toRadians(regCur));
+ }
+
+ /**
+ * Resets the odometry of the robot to the given pose.
+ */
+ public void resetOdometry(Pose2d pose) {
+ m_odometry.resetPosition(pose, m_gyro.getRotation2d());
+ }
+
+ /**
+ * Updates the field relative position of the robot.
+ */
+ public void updateOdometry() {
+ Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)); //+ (Math.PI/2));
+ Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians());
+
+ SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
+ SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
+
+ m_odometry.update( actual,//m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
+ modules[0].getState(),
+ modules[1].getState(),
+ modules[2].getState(),
+ modules[3].getState());
+ }
+
+
+ /**
+ * Resets pigeon.
+ */
+ public void resetGyro() {
+ m_gyro.reset();
+ rotTarget = new Rotation2d(0);
+ }
+
+ /**
+ * Stop all four swerve modules.
+ */
+ public void stopModules() {
+ modules[0].stop();
+ modules[1].stop();
+ modules[2].stop();
+ modules[3].stop();
+ }
+
+ /**
+ * Switches speed modes.
+ *
+ * @param shift True if fast mode, false if slow mode.
+ */
+ public void highSpeed(boolean shift) {
+ if (shift) {
+ speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+ } else {
+ speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+ }
+ }
+
+ public double getCurrent(){
+ return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
+ }
+
+ public double getVoltage(){
+ return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
new file mode 100644
index 0000000..68c383d
--- /dev/null
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -0,0 +1,181 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+
+public class SwerveModule extends SubsystemBase {
+ public WPI_TalonFX angleMotor;
+ public WPI_TalonFX driveMotor;
+ private CANCoder canCoder;
+ public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
+
+ private static double kEncoderTicksPerRotation = 4096;
+ private SwerveModuleState state;
+ private double canCoderFeedbackCoefficient;
+
+ public long m_currentTime;
+ public long m_lastTime;
+ public double m_deltaTime;
+
+ public double m_currentPos;
+ public double m_lastPos;
+
+ public SwerveModuleState lastState = new SwerveModuleState();
+ public SwerveModuleState currentState;
+
+ /** Creates a new SwerveModule. */
+ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
+ this.driveMotor = driveMotor;
+ this.angleMotor = angleMotor;
+ this.canCoder = canCoder;
+ canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
+
+ TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
+
+ angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
+ angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
+ angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
+
+ // Use the CANCoder as the remote sensor for the primary TalonFX PID
+ angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
+ angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+ angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+ angleMotor.configAllSettings(angleTalonFXConfiguration);
+ // angleMotor.setInverted(true);
+ // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
+ // driveTalonFXConfiguration.slot0.kP = 0.05;
+ // driveTalonFXConfiguration.slot0.kI = 0.0;
+ // driveTalonFXConfiguration.slot0.kD = 0.0;
+ // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
+ // FeedbackDevice.IntegratedSensor;
+ driveMotor.configFactoryDefault();
+ driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
+ driveMotor.configNominalOutputForward(0, 30);
+ driveMotor.configNominalOutputReverse(0, 30);
+ driveMotor.configPeakOutputForward(1, 30);
+ driveMotor.configPeakOutputReverse(-1, 30);
+ driveMotor.configAllowableClosedloopError(0, 0, 30);
+ // driveMotor.setInverted(true);
+ driveMotor.config_kP(0, 0, 30);
+ driveMotor.config_kI(0, 0, 30);
+ driveMotor.config_kD(0, 0, 30);
+
+ // driveMotor.configAllSettings(driveTalonFXConfiguration);
+
+ CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
+ canCoderConfiguration.sensorCoefficient = 0.087890625;
+ canCoderConfiguration.magnetOffsetDegrees = offset;
+ canCoderConfiguration.sensorDirection = true;
+ canCoder.configAllSettings(canCoderConfiguration);
+
+ m_currentTime = System.currentTimeMillis();
+ m_lastTime = System.currentTimeMillis();
+
+ m_lastPos = driveMotor.getSelectedSensorPosition();
+ }
+
+ private Rotation2d getAngle() {
+ // ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
+ return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
+ }
+
+ /**
+ * Set the speed + rotation of the swerve module from a SwerveModuleState object
+ *
+ * @param desiredState - A SwerveModuleState representing the desired new state
+ * of the module
+ */
+ public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
+ Rotation2d currentRotation = getAngle();
+ // currentRotation.getDegrees());
+ state = SwerveModuleState.optimize(desiredState, currentRotation);
+
+ // Find the difference between our current rotational position + our new
+ // rotational position
+ Rotation2d rotationDelta = state.angle.minus(currentRotation);
+
+ // Find the new absolute position of the module based on the difference in
+ // rotation
+ double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
+ // Convert the CANCoder from it's position reading back to ticks
+ double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
+ double desiredTicks = currentTicks + deltaTicks;
+
+ if (!ignoreAngle) {
+ angleMotor.set(TalonFXControlMode.Position, desiredTicks);
+ }
+
+ // Please work
+ double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
+ double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
+ // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
+ driveMotor.set(normFtPerSec);// - angleMotor.get());
+ // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
+ // between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
+ }
+
+ /**
+ * Get current module state.
+ *
+ * @return The current state of the module in m/s.
+ */
+ public SwerveModuleState getState() {
+ // return state;
+ return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
+ * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
+ }
+
+ /**
+ * Stop the drive and steer motors of current module.
+ */
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public void rotateToAngle(double angle) {
+ this.angleMotor.set(TalonFXControlMode.Position, angle);
+ }
+
+ @Override
+ public void periodic() {
+ currentState = this.getState();
+
+ Rotation2d currentRotation = getAngle();
+ SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+ SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
+ ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
+
+ lastState = currentState;
+ }
+
+ public void reset() {
+ canCoder.setPositionToAbsolute();
+ // canCoder.configSensorInitializationStrategy(initializationStrategy)
+ }
+ public double getCurrent(){
+ return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
+ }
+
+ public double getVoltage(){
+ return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
+ }
+}
\ No newline at end of file