From c62ca6bd2fd72170b41ac9050fbcab3554e98180 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 13 Jan 2023 19:39:24 -0700 Subject: [PATCH] SwerveModule done --- src/main/java/frc4388/robot/Constants.java | 23 +++-- .../frc4388/robot/subsystems/DiffDrive.java | 85 ------------------- .../robot/subsystems/SwerveModule.java | 56 +++++++++++- 3 files changed, 69 insertions(+), 95 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/DiffDrive.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8eb768f..7f5b739 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,6 +7,7 @@ package frc4388.robot; +import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; /** @@ -18,12 +19,6 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class DriveConstants { - public static final int DRIVE_PIGEON_ID = 6; - - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - public static final class SwerveDriveConstants { public static final class IDs { public static final int DRIVE_PIGEON_ID = -1; @@ -44,7 +39,21 @@ public final class Constants { public static final int RIGHT_BACK_ENCODER_ID = -1; } - + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + } + + public static final class Conversions { + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + } + + + public static final double MAX_SPEED_FEET_PER_SECOND = 16; // TODO: find the actual value + + public static final int SWERVE_TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index 64861bc..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,85 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; - -/** - * Add your docs here. - */ -public class DiffDrive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private WPI_TalonFX m_leftFrontMotor; - private WPI_TalonFX m_rightFrontMotor; - private WPI_TalonFX m_leftBackMotor; - private WPI_TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor, - WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_driveTrain = driveTrain; - m_gyro = gyro; - } - - @Override - public void periodic() { - m_gyro.updatePigeonDeltas(); - - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - // SmartDashboard.putData(m_gyro); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 9867da5..ee495fe 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,17 +4,27 @@ 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.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.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { private WPI_TalonFX driveMotor; private WPI_TalonFX angleMotor; private CANCoder canCoder; + + public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ @@ -24,11 +34,51 @@ public class SwerveModule extends SubsystemBase { this.canCoder = canCoder; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + angleConfig.slot0.kP = swerveGains.kP; + angleConfig.slot0.kI = swerveGains.kI; + angleConfig.slot0.kD = swerveGains.kD; + // use the CANcoder as the remote sensor for the primary TalonFX PID + angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID(); + angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + angleMotor.configAllSettings(angleConfig); + + CANCoderConfiguration canCoderConfig = new CANCoderConfiguration(); + canCoderConfig.magnetOffsetDegrees = offset; + canCoder.configAllSettings(canCoderConfig); } - @Override - public void periodic() { - // This method will be called once per scheduler run + /** + * Get the angle of a SwerveModule as a Rotation2d + * @return the angle of a SwerveModule as a Rotation2d + */ + public Rotation2d getAngle() { + // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. + return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } + + /** + * Set the speed and rotation of the SwerveModule from a SwerveModuleState object + * @param desiredState a SwerveModuleState representing the desired new state of the module + */ + public void setDesiredState(SwerveModuleState desiredState) { + Rotation2d currentRotation = this.getAngle(); + + SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + + // calculate the difference between our current rotational position and our new rotational position + Rotation2d rotationDelta = state.angle.minus(currentRotation); + + // calculate the new absolute position of the SwerveModule based on the difference in rotation + double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + + // convert the CANCoder from its position reading to ticks + double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + + double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + } + }