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 b619905..3bfe1d4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,10 +7,29 @@ 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.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; +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; @@ -38,19 +57,63 @@ 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 List CAN_DEVICES = new ArrayList<>(); + + 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 + 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. + // Mechanics + 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); + + 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; + + 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.36328125); + private static final boolean FRONT_LEFT_DRIVE_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_YPOS = Inches.of(HALF_HEIGHT); + + //Front Right + 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 = 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); + + //Back Left + 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 = 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); + + //Back Right + 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 = 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_YPOS = Inches.of(-HALF_HEIGHT); } public static final class IDs { @@ -70,7 +133,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); } @@ -79,8 +142,27 @@ 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 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.6) + .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 + public 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 { @@ -93,41 +175,92 @@ 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.2; - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; + 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() + .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 = 100; // 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; - - // 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; - + + // No mans land + // Beware, there be dragons. + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(IDs.DRIVE_PIGEON.id); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() // holy verbosity batman. + .withDriveMotorGearRatio(DRIVE_RATIO) + .withSteerMotorGearRatio(STEER_RATIO) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_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); + + public static final SwerveModuleConstants FRONT_LEFT = + ConstantCreator.createModuleConstants( + 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( + 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( + 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/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/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a5e29df..4990efa 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,11 +10,15 @@ 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; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.subsystems.SwerveModule; +// import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; /** @@ -22,44 +26,24 @@ 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; - public SwerveModule leftBack; - public SwerveModule rightBack; - public RobotMap() { configureDriveMotorControllers(); } /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - + /* 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() {} + } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 7adf8f8..c685f23 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,212 +4,148 @@ package frc4388.robot.subsystems; -import java.util.logging.Level; +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; -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; 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 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 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 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(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { - super(); - this.leftFront = leftFront; - this.rightFront = rightFront; - this.leftBack = leftBack; - this.rightBack = rightBack; - - this.gyro = gyro; - reset_index(); - this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; - } + private Field2d field = new Field2d(); - 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); + /** 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() + // 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) { - - double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; - SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); - - if (fieldRelative) { - - 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; - } - - // SmartDashboard.putBoolean("drift correction", true); - - // 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)); - - // 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); - } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { - - 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 - } + 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 - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput + return; // don't bother doing swerve drive math and return early. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1); + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + if (fieldRelative) { + swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) + ); + // double rot = 0; + + // ! drift correction + // 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); + + // // 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)); + + // // 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); - } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withRotationalRate(rightStick.getX()*rotSpeedAdjust) + ); } + } - /** - * 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); - } + 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. + + leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + 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; @@ -219,48 +155,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 @@ -268,10 +171,15 @@ 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() { - 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() { @@ -291,39 +199,23 @@ 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; } 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() { @@ -355,7 +247,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 @@ -364,13 +256,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; - // } -}