From 631958e662ebe96f12e030511d86375f83de3566 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 6 Jan 2025 14:51:44 -0700 Subject: [PATCH 1/8] i love using phonix 6 sweve drivetrain --- src/main/java/frc4388/robot/Constants.java | 20 ++++++++++++++++++-- src/main/java/frc4388/robot/RobotMap.java | 15 +++++++++++++-- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b619905..3c6f252 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -10,6 +10,11 @@ package frc4388.robot; import java.util.ArrayList; import java.util.List; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ParentConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; + import edu.wpi.first.math.trajectory.TrapezoidProfile; import frc4388.utility.CanDevice; import frc4388.utility.Gains; @@ -43,7 +48,7 @@ public final class Constants { public static final double SLOW_SPEED = 0.25; public static final double FAST_SPEED = 0.5; public static final double TURBO_SPEED = 1.0; - + // public static List CAN_DEVICES = new ArrayList<>(); public static final class DefaultSwerveRotOffsets { @@ -127,7 +132,18 @@ public final class Constants { public static final double HEIGHT = 18.5; public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; - + + public static final SwerveModuleConstants FRONT_LEFT = new SwerveModuleConstants<>() + .withLocationX(HALF_WIDTH) + .withLocationX(HALF_HEIGHT) + .withDriveMotorId(2) + .withDriveMotorInverted(false) + .withSteerMotorId(3) + .withSteerMotorInverted(false) + .withEncoderId(10) + .withEncoderInverted(false) + .withEncoderOffset(0.0); + // misc public static final int TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a5e29df..d338630 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,6 +10,10 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; @@ -22,8 +26,8 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { - private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); - public RobotGyro gyro = new RobotGyro(m_pigeon2); + // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); + // public RobotGyro gyro = new RobotGyro(m_pigeon2); public SwerveModule leftFront; public SwerveModule rightFront; @@ -37,6 +41,13 @@ public class RobotMap { /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain( + TalonFX::new, TalonFX::new, CANcoder::new, + new SwerveDrivetrainConstants().withPigeon2Id(SwerveDriveConstants.IDs.DRIVE_PIGEON.id), + new SwerveModuleConstants[] { + } + ) + /* Swreve Drive Subsystem */ public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL.id); public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER.id); From e07bb2fa7b33a13828af06f678bb7bb26a25a31f Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 6 Jan 2025 18:18:43 -0700 Subject: [PATCH 2/8] A lot of constants changes and copy paste example from the docs --- src/main/java/frc4388/robot/Constants.java | 230 ++++++++++++++++++--- 1 file changed, 205 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3c6f252..35dc115 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,15 +7,25 @@ package frc4388.robot; -import java.util.ArrayList; -import java.util.List; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.ParentConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -49,13 +59,48 @@ public final class Constants { public static final double FAST_SPEED = 0.5; public static final double TURBO_SPEED = 1.0; - // public static List CAN_DEVICES = new ArrayList<>(); + // dimensions + public static final double WIDTH = 18.5; + public static final double HEIGHT = 18.5; + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; - public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. + private static final class ModuleSpecificConstants { + //Front Left + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Front Right + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + + //Back Left + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_ENCODER_INVERTED = false; + private static final Distance BACK_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Back Right + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + } + + private static final class DrivetrainSpecificConstants { + } public static final class IDs { @@ -75,7 +120,7 @@ public final class Constants { public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9); public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); - public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 4); + public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14); public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50); } @@ -127,23 +172,158 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - // dimensions - public static final double WIDTH = 18.5; - public static final double HEIGHT = 18.5; - public static final double HALF_WIDTH = WIDTH / 2.d; - public static final double HALF_HEIGHT = HEIGHT / 2.d; + - public static final SwerveModuleConstants FRONT_LEFT = new SwerveModuleConstants<>() - .withLocationX(HALF_WIDTH) - .withLocationX(HALF_HEIGHT) - .withDriveMotorId(2) - .withDriveMotorInverted(false) - .withSteerMotorId(3) - .withSteerMotorInverted(false) - .withEncoderId(10) - .withEncoderInverted(false) - .withEncoderOffset(0.0); + // No mans land + // Beware, there be dragons. + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.91).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final double kSlipCurrent = 120; // TODO: tune??? + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(100) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final double kSpeedAt12Volts = 4.69;// todo: tune??? + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.8181818181818183; //todo: find + + private static final double kDriveGearRatio = 6.12; + private static final double kSteerGearRatio = (150/7); + private static final Distance kWheelRadius = Inches.of(2); // 2 inches in meters + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(IDs.DRIVE_PIGEON.id); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() // holy verbosity batman. + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs); + + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 10; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, Rotations.of(SwerveRotOffsets.FRONT_LEFT_ROT_OFFSET), + Inches.of(HALF_WIDTH), Inches.of(HALF_HEIGHT), false, false, false + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, false, false, false + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, false, false, false + ); + public static final SwerveModuleConstants BackRight = // misc public static final int TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; From 7f93deef6e3e379221a3680ef355f12f8b139346 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Mon, 6 Jan 2025 20:41:44 -0700 Subject: [PATCH 3/8] functional constants, may need revision. sweve drivetrain in robot map --- src/main/java/frc4388/robot/Constants.java | 270 +++++++++------------ src/main/java/frc4388/robot/RobotMap.java | 44 +--- 2 files changed, 120 insertions(+), 194 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 35dc115..0261239 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -11,9 +11,13 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; @@ -53,25 +57,36 @@ public final class Constants { public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - public static final double[] GEARS = {0.25, 0.5, 1.0}; - public static final double SLOW_SPEED = 0.25; public static final double FAST_SPEED = 0.5; public static final double TURBO_SPEED = 1.0; + + public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; // dimensions - public static final double WIDTH = 18.5; - public static final double HEIGHT = 18.5; + public static final double WIDTH = 18.5; // TODO: validate + public static final double HEIGHT = 18.5; // TODO: validate public static final double HALF_WIDTH = WIDTH / 2.d; public static final double HALF_HEIGHT = HEIGHT / 2.d; + // Mechanics + private static final double COUPLE_RATIO = 1; //todo: find + private static final double DRIVE_RATIO = 6.12; + private static final double STEER_RATIO = (150/7); + private static final Distance WHEEL_RADIUS = Inches.of(2); + + public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate + + public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + private static final class ModuleSpecificConstants { //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right @@ -87,8 +102,8 @@ public final class Constants { private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); //Back Right private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); @@ -96,11 +111,7 @@ public final class Constants { private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance BACK_RIGHT_YPOS = Inches.of(HALF_HEIGHT); - } - - private static final class DrivetrainSpecificConstants { - + private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } public static final class IDs { @@ -129,8 +140,23 @@ public final class Constants { public static final int SWERVE_PID_LOOP_IDX = 1; public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0); + public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs() + .withKP(50).withKI(0).withKD(0.32) + .withKS(0).withKV(0).withKA(0); + public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.91).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); } public static final class AutoConstants { @@ -164,166 +190,94 @@ public final class Constants { } public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; + public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. public static final double NEUTRAL_DEADBAND = 0.04; + + // POWER! (limiting) + private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(100) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ).withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ); + private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + ).withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ); + private static final double SLIP_CURRENT = 120; // TODO: Tune??? } - - public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - - - // No mans land // Beware, there be dragons. - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(1.91).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final double kSlipCurrent = 120; // TODO: tune??? - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(100) // TODO: tune??? - .withStatorCurrentLimitEnable(true) - ); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(40) // TODO: tune??? - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final double kSpeedAt12Volts = 4.69;// todo: tune??? - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; //todo: find - - private static final double kDriveGearRatio = 6.12; - private static final double kSteerGearRatio = (150/7); - private static final Distance kWheelRadius = Inches.of(2); // 2 inches in meters - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() .withPigeon2Id(IDs.DRIVE_PIGEON.id); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() // holy verbosity batman. - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs); + .withDriveMotorGearRatio(DRIVE_RATIO) + .withSteerMotorGearRatio(STEER_RATIO) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(PIDConstants.CURRENT_SWERVE_ROT_GAINS) // ? + .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ? + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withSlipCurrent(Configurations.SLIP_CURRENT) + .withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC) + .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) + .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) + .withFeedbackSource(SteerFeedbackType.RemoteCANcoder) + .withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS) + .withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS); - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 10; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - - public static final SwerveModuleConstants FrontLeft = + public static final SwerveModuleConstants FRONT_LEFT = ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, Rotations.of(SwerveRotOffsets.FRONT_LEFT_ROT_OFFSET), - Inches.of(HALF_WIDTH), Inches.of(HALF_HEIGHT), false, false, false - ); - public static final SwerveModuleConstants FrontRight = + IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS, + ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants FRONT_RIGHT = ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, false, false, false - ); - public static final SwerveModuleConstants BackLeft = + IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS, + ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_LEFT = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, false, false, false - ); - public static final SwerveModuleConstants BackRight = + IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS, + ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_RIGHT = + ConstantCreator.createModuleConstants( + IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS, + ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED + ); + // misc public static final int TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d338630..de4c523 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -29,48 +29,20 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public SwerveModule leftFront; - public SwerveModule rightFront; - public SwerveModule leftBack; - public SwerveModule rightBack; - public RobotMap() { configureDriveMotorControllers(); } /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - - public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain( - TalonFX::new, TalonFX::new, CANcoder::new, - new SwerveDrivetrainConstants().withPigeon2Id(SwerveDriveConstants.IDs.DRIVE_PIGEON.id), - new SwerveModuleConstants[] { - } - ) - + /* Swreve Drive Subsystem */ - public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL.id); - public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER.id); - public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER.id); + public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + Constants.SwerveDriveConstants.DrivetrainConstants, + Constants.SwerveDriveConstants.FRONT_LEFT, Constants.SwerveDriveConstants.FRONT_RIGHT, + Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT + ); - - public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL.id); - public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER.id); - public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER.id); - - public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL.id); - public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER.id); - public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER.id); - - public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL.id); - public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER.id); - public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER.id); - - void configureDriveMotorControllers() { - // initialize SwerveModules - this.rightFront = new SwerveModule("Right Front Swerve Module", rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); - this.leftFront = new SwerveModule("Left Front Swerve Module", leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); - this.leftBack = new SwerveModule("Left Back Swerve Module", leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); - this.rightBack = new SwerveModule("Right Back Swerve Module", rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); - } + void configureDriveMotorControllers() {} + } From f27ca5aab76e4cb0e7de48fc76deef019e14967f Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Mon, 6 Jan 2025 21:34:40 -0700 Subject: [PATCH 4/8] start overhauling swerve drive class --- src/main/java/frc4388/robot/Constants.java | 6 +- src/main/java/frc4388/robot/RobotMap.java | 1 + .../frc4388/robot/subsystems/SwerveDrive.java | 91 +++++++++---------- 3 files changed, 47 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0261239..9f6dbcb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -80,6 +80,8 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 + private static final class ModuleSpecificConstants { //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0); @@ -148,13 +150,13 @@ public final class Constants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() + public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() .withKP(100).withKI(0).withKD(0.5) .withKS(0.1).withKV(1.91).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() + public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() .withKP(0.1).withKI(0).withKD(0) .withKS(0).withKV(0.124); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index de4c523..95787cd 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -43,6 +43,7 @@ public class RobotMap { Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT ); + void configureDriveMotorControllers() {} } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 7adf8f8..bc29762 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -6,6 +6,11 @@ package frc4388.robot.subsystems; import java.util.logging.Level; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -30,22 +35,7 @@ import frc4388.utility.Status.Report; import frc4388.utility.Status.ReportLevel; public class SwerveDrive extends Subsystem { - - private SwerveModule leftFront; - private SwerveModule rightFront; - private SwerveModule leftBack; - private SwerveModule rightBack; - - private SwerveModule[] modules; - - private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - - private RobotGyro gyro; + private SwerveDrivetrain swerveDriveTrain; private int gear_index; private boolean stopped = false; @@ -59,16 +49,12 @@ public class SwerveDrive extends Subsystem { public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { super(); - this.leftFront = leftFront; - this.rightFront = rightFront; - this.leftBack = leftBack; - this.rightBack = rightBack; - this.gyro = gyro; + this.swerveDriveTrain = swerveDriveTrain; + reset_index(); - this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ @@ -84,40 +70,47 @@ public class SwerveDrive extends Subsystem { } public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - - double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; - SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); - + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); if (fieldRelative) { - - double rot = 0; + // if (rightStick.getNorm() > 0.05 && + swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + ); + // double rot = 0; // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot_correction = 0; - // rot = rightStick.getX(); - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } + // dependant on if the new odomitry system acounts for rotational drift, this may not be needed. + // if (rightStick.getNorm() > 0.05) { + // rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees(); + // swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + // .withVelocityX(leftStick.getX()*speedAdjust) + // .withVelocityY(leftStick.getY()*speedAdjust) + // .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + // ); + + // // SmartDashboard.putBoolean("drift correction", false); + // stopped = false; + // } else if(leftStick.getNorm() > 0.05) { + // if (!stopped) { + // stopModules(); + // stopped = true; + // } - // SmartDashboard.putBoolean("drift correction", true); + // // SmartDashboard.putBoolean("drift correction", true); - // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + // // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - } + // } - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + // // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - // Convert field-relative speeds to robot-relative speeds. - // chassisSpeeds = chassisSpeeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); + // // Convert field-relative speeds to robot-relative speeds. + // // chassisSpeeds = chassisSpeeds. + // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } From 31dc2d9f8d5b5ec06d48e03bad375937886abbf9 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Tue, 7 Jan 2025 08:38:21 -0700 Subject: [PATCH 5/8] add input checking --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index bc29762..15864ff 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -70,9 +70,15 @@ public class SwerveDrive extends Subsystem { } public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); // stop the swerve + + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput + return; // don't bother doing swerve drive math and return early. + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + if (fieldRelative) { - // if (rightStick.getNorm() > 0.05 && swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(leftStick.getX()*speedAdjust) .withVelocityY(leftStick.getY()*speedAdjust) From da2a1e46fa1a1ce7d70143505fa57718e2540c39 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Tue, 7 Jan 2025 13:45:41 -0700 Subject: [PATCH 6/8] add more to swervedrive subsystem and remove dead code. --- simgui-ds.json | 5 + src/main/java/frc4388/robot/Constants.java | 14 +- .../java/frc4388/robot/RobotContainer.java | 11 +- .../frc4388/robot/subsystems/SwerveDrive.java | 224 +++---------- .../robot/subsystems/SwerveModule.java | 314 ------------------ 5 files changed, 63 insertions(+), 505 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/SwerveModule.java diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..49cf3aa 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000" + } ] } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9f6dbcb..8d74a30 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -62,7 +62,7 @@ public final class Constants { public static final double TURBO_SPEED = 1.0; public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; - + public static final int STARTING_GEAR = 0; // dimensions public static final double WIDTH = 18.5; // TODO: validate public static final double HEIGHT = 18.5; // TODO: validate @@ -192,17 +192,13 @@ public final class Constants { } public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. - public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double OPEN_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help. + public static final double CLOSED_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help. public static final double NEUTRAL_DEADBAND = 0.04; // POWER! (limiting) private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(100) // TODO: tune??? - .withStatorCurrentLimitEnable(true) - ).withMotorOutput( + .withMotorOutput( new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Brake) .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) @@ -229,7 +225,7 @@ public final class Constants { new ClosedLoopRampsConfigs() .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) ); - private static final double SLIP_CURRENT = 120; // TODO: Tune??? + private static final double SLIP_CURRENT = 100; // TODO: Tune??? } // No mans land diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1d5fd95..63f9eec 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -54,12 +54,7 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - m_robotMap.rightFront, - m_robotMap.leftBack, - m_robotMap.rightBack, - - m_robotMap.gyro); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -88,7 +83,7 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); + new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); @@ -148,7 +143,7 @@ public class RobotContainer { // ? /* Driver Buttons */ DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 15864ff..d8ef7b6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,8 +4,6 @@ package frc4388.robot.subsystems; -import java.util.logging.Level; - import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrain; @@ -14,9 +12,7 @@ import com.ctre.phoenix6.swerve.SwerveRequest; 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.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -24,25 +20,22 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants.Conversions; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotUnits; + import frc4388.utility.Status; import frc4388.utility.Subsystem; -import frc4388.utility.Status.Report; import frc4388.utility.Status.ReportLevel; public class SwerveDrive extends Subsystem { private SwerveDrivetrain swerveDriveTrain; - private int gear_index; + private int gear_index = SwerveDriveConstants.STARTING_GEAR; private boolean stopped = false; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25% public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); @@ -53,25 +46,23 @@ public class SwerveDrive extends Subsystem { super(); this.swerveDriveTrain = swerveDriveTrain; - - reset_index(); } - public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ - // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); - // rightStick.getAngle() - double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); - // System.out.println(ang); - // module.go(ang); - // Rotation2d rot = Rotation2d.fromRadians(ang); - Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); - SwerveModuleState state = new SwerveModuleState(speed, rot); - module.setDesiredState(state); - } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ + // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); + // // rightStick.getAngle() + // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); + // // System.out.println(ang); + // // module.go(ang); + // // Rotation2d rot = Rotation2d.fromRadians(ang); + // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); + // SwerveModuleState state = new SwerveModuleState(speed, rot); + // module.setDesiredState(state); + // } public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: - swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); // stop the swerve + stopModules(); // stop the swerve if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput return; // don't bother doing swerve drive math and return early. @@ -118,97 +109,36 @@ public class SwerveDrive extends Subsystem { // // chassisSpeeds = chassisSpeeds. // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + ); } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } - public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + stopModules(); // stop the swerve + + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput + return; // don't bother doing swerve drive math and return early. - double rot = 0; - - // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } - - // SmartDashboard.putBoolean("drift correction", true); - // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - - } - - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - - // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); - } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - - // Translation2d rightStick = new Translation2d(-rightX, rightY); - double rightX = rightStick.getX(); - double rightY = rightStick.getY(); - - double rot_correction = 0; - - // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - if(fieldRelative) { - double rot = 0; - if(rightStick.getNorm() > 0.5) { - orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); - - Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); - double min = tmp.getDegrees(); - min = Math.max(Math.abs(min), 2); - if(tmp.getDegrees() < 0) - min*=-1; - tmp = new Rotation2d(min * Math.PI / 180); - rot = tmp.getRadians(); // x x - y/x - } + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - /** - * 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_SECOND)); - for (int i = 0; i < desiredStates.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); - } + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withTargetDirection(rightStick.getAngle()) + ); } public boolean rotateToTarget(double angle) { - double currentAngle = getGyroAngle(); - double error = angle - currentAngle; - - driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(0) + .withVelocityY(0) + .withTargetDirection(Rotation2d.fromDegrees(angle)) + ); if (Math.abs(angle - getGyroAngle()) < 5.0) { return true; @@ -218,48 +148,15 @@ public class SwerveDrive extends Subsystem { } public double getGyroAngle() { - return -gyro.getAngle(); - } - - public void add180() { - gyro.reset(gyro.getAngle()+180); - rotTarget = gyro.getAngle(); - + return swerveDriveTrain.getRotation3d().getAngle(); } public void resetGyro() { - gyro.reset(); - rotTarget = gyro.getAngle(); + swerveDriveTrain.tareEverything(); } - public void resetGyroFlip() { - gyro.resetFlip(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroRightBlue() { - gyro.resetRightSideBlue(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroRightAmp() { - gyro.resetAmpSide(); - rotTarget = gyro.getAngle(); - } - public void stopModules() { - for (SwerveModule module : this.modules) { - module.stop(); - } - } - - public SwerveDriveKinematics getKinematics() { - return this.kinematics; - } - - public boolean getSpeedState() { - - return false; + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); } @Override @@ -270,7 +167,7 @@ public class SwerveDrive extends Subsystem { } private void reset_index() { - gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?) + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?) } public void shiftDown() { @@ -295,34 +192,18 @@ public class SwerveDrive extends Subsystem { } public void setToSlow() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED; - for(int i=0;i<5;i++){ - Log("SLOW"); - } + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + gear_index = 0; } public void setToFast() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED; - for(int i=0;i<5;i++){ - Log("FAST"); - } + setPercentOutput(SwerveDriveConstants.FAST_SPEED); + gear_index = 1; } public void setToTurbo() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED; - for(int i=0;i<5;i++){ - Log("TURBO"); - } - } - - public void toggleGear(double angle) { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } else { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + gear_index = 2; } public void shiftUpRot() { @@ -354,7 +235,7 @@ public class SwerveDrive extends Subsystem { @Override public void queryStatus() { - sbGyro.setDouble(this.gyro.getAngle()); + sbGyro.setDouble(getGyroAngle()); sbShiftState.setDouble(this.speedAdjust); //TODO: Add more status things @@ -363,13 +244,8 @@ public class SwerveDrive extends Subsystem { @Override public Status diagnosticStatus() { Status status = new Status(); - for (SwerveModule module : modules) { - for (Report moduleDignostic : module.diagnosticStatus().reports) { - status.addReport(moduleDignostic.reportLevel, "[" + module.getSubsystemName() + "] " + moduleDignostic.description); - } - } - status.diagnoseHardwareCTRE("Swerve Gyro", gyro.getPigeon()); + status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); return status; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java deleted file mode 100644 index 471c6ec..0000000 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ /dev/null @@ -1,314 +0,0 @@ -// 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 java.util.logging.Level; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MagnetHealthValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.hardware.CANcoder; - -import edu.wpi.first.math.geometry.Translation2d; -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.util.Units; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.Gains; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.Status.ReportLevel; - -public class SwerveModule extends Subsystem { - private String name = "Null"; - private TalonFX driveMotor; - private TalonFX angleMotor; - private CANcoder encoder; - // private final StatusSignal cc_pos; - // private final StatusSignal cc_vel; - // private int selfid; - // private ConfigurableDouble offsetGetter; - private static int swerveId = 0; - public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; - - - /** Creates a new SwerveModule. */ - public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { - super(); - this.name = name; - this.driveMotor = driveMotor; - this.angleMotor = angleMotor; - this.encoder = encoder; - - var motorCfg = new TalonFXConfiguration() - .withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ).withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(100) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(100) - .withSupplyCurrentLimitEnable(true) - ); - - driveMotor.getConfigurator().apply(motorCfg); - - TalonFXConfiguration angleConfig = new TalonFXConfiguration() - .withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ); - - angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - angleConfig.Slot0.kP = swerveGains.kP; - angleConfig.Slot0.kI = swerveGains.kI; - angleConfig.Slot0.kD = swerveGains.kD; - - angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); - angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - angleMotor.getConfigurator().apply(angleConfig); - - CANcoderConfiguration canconfig = new CANcoderConfiguration(); - canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - canconfig.MagnetSensor.MagnetOffset = offset; - encoder.getConfigurator().apply(canconfig); - - rotateToAngle(0); - } - - // public void go(double ang){ - // // double curang = this.encoder.getAbsolutePosition().getValue(); - // System.out.println(getAngle().getDegrees()); - // rotateToAngle(ang); - // } - - @Override - public void periodic() { - //encoder.configMagnetOffset(offsetGetter.get()); - //SmartDashboard.putString("Error Code: " + selfid, getstuff()); - // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees()); - // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel()); - // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos()); - // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel()); - } - /** - * Get the drive motor of the SwerveModule - * @return the drive motor of the SwerveModule - */ - public TalonFX getDriveMotor() { - return this.driveMotor; - } - - /** - * Get the angle motor of the SwerveModule - * @return the angle motor of the SwerveModule - */ - public TalonFX getAngleMotor() { - return this.angleMotor; - } - - /** - * Get the CANcoder of the SwerveModule - * @return the CANcoder of the SwerveModule - */ - public CANcoder getEncoder() { - return this.encoder; - } - - /** - * 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(encoder.getAbsolutePosition()); - return Rotation2d.fromRotations(encoder.getPosition().getValue().baseUnitMagnitude()); - } - - public double getAngularVel() { - // return this.angleMotor.getSelectedSensorVelocity(); - return angleMotor.getVelocity().getValueAsDouble(); - } - - public double getDrivePos() { - // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; - return driveMotor.getPosition().getValueAsDouble(); - } - - public double getDriveVel() { - // return this.driveMotor.getSelectedSensorVelocity(0); - return driveMotor.getVelocity().getValueAsDouble(); - } - - public void stop() { - driveMotor.set(0); - angleMotor.set(0); - } - - public void rotateToAngle(double angle) { - final PositionVoltage m_request = new PositionVoltage(angle); - angleMotor.setControl(m_request); - } - - /** - * Get state of swerve module - * @return speed in m/s and angle in degrees - */ - public SwerveModuleState getState() { - return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getVelocity().getValue().baseUnitMagnitude() * - SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV * - SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), - getAngle() - ); - } - - // private SwerveModuleState optimizeState(SwerveModuleState desiredState) { - // Rotation2d curRot = this.getAngle(); - - // } - - /** - * Returns the current position of the SwerveModule - * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. - // */ - // public SwerveModulePosition getPosition() { - // return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); - // } - - /** - * 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 state) { - Rotation2d currentRotation = this.getAngle(); - - state.optimize(currentRotation);//SwerveModuleState.optimize(desiredState, currentRotation); - - // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); - - double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND; - - rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations()); - - driveMotor.set(Math.max(Math.min(speed, 1.), -1.)); - } - - public void reset() { - // encoder.setPosition(0); - } - - @Override - public String getSubsystemName() { - return this.name; - } - - ShuffleboardLayout subsystemLayout; - GenericEntry sbSpeed; - GenericEntry sbAngle; - - private void createLayout(){ - - subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - sbSpeed = subsystemLayout - .add("Drive motor speed", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); - - sbAngle = subsystemLayout - .add("Angle motor angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); - } - - - @Override - public void queryStatus() { - if(subsystemLayout == null) - createLayout(); - - // Shuffleboard.getTab("Subsystems").set - - // sbSpeed.setDouble(this.driveMotor.get()); - sbAngle.setDouble(this.angleMotor.getRotorPosition().getValueAsDouble()); - // SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get()); - // SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble()); - //TODO: Add more status things - } - - public boolean motorsAlive() { - return this.driveMotor.isAlive() && this.angleMotor.isAlive(); - } - - @Override - public Status diagnosticStatus() { - Status status = new Status(); - - status.diagnoseHardwareCTRE("Drive", this.driveMotor); - status.diagnoseHardwareCTRE("Angle", this.angleMotor); - status.diagnoseHardwareCTRE("Steer", this.encoder); - - return status; - } - - // public double getCurrent() { - // return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); - // } - - // public double getVoltage() { - // return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); - // } - - // public String getstuff() { - // encoder.getPosition(); - // return "" + encoder.getLastError().value; - // } -} From c1f4829eaf6f110374c60ebd601d32aa9c264497 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 8 Jan 2025 18:32:02 -0700 Subject: [PATCH 7/8] Functional driving of swerve drive, still some orginizational things in constants that need to be fixed --- src/main/java/frc4388/robot/Constants.java | 48 ++++++++++--------- src/main/java/frc4388/robot/RobotMap.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 27 ++++++++--- 3 files changed, 47 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8d74a30..c42cad8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,7 +70,7 @@ public final class Constants { public static final double HALF_HEIGHT = HEIGHT / 2.d; // Mechanics - private static final double COUPLE_RATIO = 1; //todo: find + private static final double COUPLE_RATIO = 3; //todo: find private static final double DRIVE_RATIO = 6.12; private static final double STEER_RATIO = (150/7); private static final Distance WHEEL_RADIUS = Inches.of(2); @@ -80,39 +80,39 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 + public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 private static final class ModuleSpecificConstants { //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } @@ -145,13 +145,17 @@ public final class Constants { public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs() .withKP(50).withKI(0).withKD(0.32) .withKS(0).withKV(0).withKA(0); + + public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs() + .withKP(10).withKI(0).withKD(0.3) + .withKS(0).withKV(0).withKA(0); public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) + .withKP(100).withKI(0).withKD(0.6) .withKS(0.1).withKV(1.91).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -218,12 +222,12 @@ public final class Constants { new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Brake) .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ).withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + // ).withOpenLoopRamps( + // new OpenLoopRampsConfigs() + // .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + // ).withClosedLoopRamps( + // new ClosedLoopRampsConfigs() + // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) ); private static final double SLIP_CURRENT = 100; // TODO: Tune??? } @@ -239,7 +243,7 @@ public final class Constants { .withSteerMotorGearRatio(STEER_RATIO) .withCouplingGearRatio(COUPLE_RATIO) .withWheelRadius(WHEEL_RADIUS) - .withSteerMotorGains(PIDConstants.CURRENT_SWERVE_ROT_GAINS) // ? + .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ? .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ? .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 95787cd..4990efa 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -18,7 +18,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.subsystems.SwerveModule; +// import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d8ef7b6..f127a84 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,11 +4,15 @@ package frc4388.robot.subsystems; +import java.util.Optional; + +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveRequest; +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; @@ -18,6 +22,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.Constants.SwerveDriveConstants; @@ -41,13 +46,16 @@ public class SwerveDrive extends Subsystem { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + private Field2d field = new Field2d(); + /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { super(); - + + SmartDashboard.putData(field); + this.swerveDriveTrain = swerveDriveTrain; } - // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); // // rightStick.getAngle() @@ -67,13 +75,13 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput return; // don't bother doing swerve drive math and return early. - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); if (fieldRelative) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityX(leftStick.getX()*-speedAdjust) .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); // double rot = 0; @@ -110,9 +118,9 @@ public class SwerveDrive extends Subsystem { // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX()*speedAdjust) + .withVelocityX(leftStick.getX()*-speedAdjust) .withVelocityY(leftStick.getY()*speedAdjust) - .withRotationalRate(rightStick.getY()*rotSpeedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); } } @@ -164,6 +172,11 @@ public class SwerveDrive extends Subsystem { // This method will be called once per scheduler run\ SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("RotTartget", rotTarget); + + Optional e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()); + + if(e.isPresent()) + field.setRobotPose(e.get()); } private void reset_index() { From 34efb5698ebeeea882247daa410f99235dda67df Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 8 Jan 2025 18:40:23 -0700 Subject: [PATCH 8/8] remove swervedrive constants conversions --- src/main/java/frc4388/robot/Constants.java | 19 ------------------- .../frc4388/robot/subsystems/SwerveDrive.java | 3 +-- 2 files changed, 1 insertion(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c42cad8..3bfe1d4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,25 +175,6 @@ public final class Constants { public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value } - public static final class Conversions { - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; - - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; - - public static final double TICKS_PER_MOTOR_REV = 0.5; - public static final double WHEEL_DIAMETER_INCHES = 3.9; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; - - public static final double TICK_TIME_TO_SECONDS = 10; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; - } public static final class Configurations { public static final double OPEN_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help. diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f127a84..c685f23 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -26,7 +26,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.SwerveDriveConstants.Conversions; import frc4388.utility.Status; import frc4388.utility.Subsystem; @@ -200,7 +199,7 @@ public class SwerveDrive extends Subsystem { } public void setPercentOutput(double speed) { - speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed; + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; gear_index = -1; }