Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4 from Team4388/Overhaul-Swerve

Browse Source 
Overhaul swerve
This commit is contained in:
Michael Mikovsky
2025-01-09 02:25:10 +00:00
committed by GitHub
parent 5348cafeea 34efb5698e
commit bdef8352d4
6 changed files with 320 additions and 630 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simgui-ds.json
+5
View File
@@ -88,5 +88,10 @@
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "78696e70757401000000000000000000"
}
]
}
src/main/java/frc4388/robot/Constants.java
+172 -39
View File
@@ -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<CanDevice> 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,40 +175,91 @@ 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;
// No mans land
// Beware, there be dragons.
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
.withPigeon2Id(IDs.DRIVE_PIGEON.id);
// 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;
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // 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<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> 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<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> 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<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> 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<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> 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;
src/main/java/frc4388/robot/RobotContainer.java
+3 -8
View File
@@ -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
src/main/java/frc4388/robot/RobotMap.java
+13 -29
View File
@@ -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,13 +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);
public SwerveModule leftFront;
public SwerveModule rightFront;
public SwerveModule leftBack;
public SwerveModule rightBack;
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
public RobotMap() {
configureDriveMotorControllers();
@@ -38,28 +37,13 @@ public class RobotMap {
// 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<TalonFX, TalonFX, CANcoder> swerveDrivetrain = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (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);
void configureDriveMotorControllers() {}
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);
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+106 -219
View File
@@ -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<TalonFX, TalonFX, CANcoder> 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();
private Field2d field = new Field2d();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
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};
}
SmartDashboard.putData(field);
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);
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) {
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
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
if (fieldRelative) {
double rot = 0;
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX()*-speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*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", true);
// // SmartDashboard.putBoolean("drift correction", false);
// stopped = false;
// } else if(leftStick.getNorm() > 0.05) {
// if (!stopped) {
// stopModules();
// stopped = true;
// }
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
// // 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));
// // 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);
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX()*-speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*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
double rot = 0;
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
return; // don't bother doing swerve drive math and return early.
// ! 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;
}
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
// 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
}
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;
@@ -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();
}
public void resetGyroFlip() {
gyro.resetFlip();
rotTarget = gyro.getAngle();
}
public void resetGyroRightBlue() {
gyro.resetRightSideBlue();
rotTarget = gyro.getAngle();
}
public void resetGyroRightAmp() {
gyro.resetAmpSide();
rotTarget = gyro.getAngle();
swerveDriveTrain.tareEverything();
}
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<Pose2d> 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;
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
-314
View File
@@ -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<Double> cc_pos;
// private final StatusSignal<Double> 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;
// }
}