diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index d74ce65e..0a995b34 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotContainer { @@ -33,6 +34,10 @@ public class RobotContainer { public final Arm arm = Arm.getInstance(); public final SwerveDrive swerve = SwerveDrive.getInstance(); + // Controller + // private final CommandXboxController driver = new CommandXboxController(Ports.Gamepad.DRIVER); + // private final CommandXboxController operator = new CommandXboxController(Ports.Gamepad.OPERATOR); + // Autons private static SendableChooser autonChooser = new SendableChooser<>(); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 367583c8..2c5279cf 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -1,8 +1,5 @@ package com.stuypulse.robot.constants; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.util.PIDConstants; - import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.network.SmartBoolean; @@ -10,6 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; /*- @@ -146,49 +144,51 @@ public interface FF { } public interface Swerve { - // between wheel centers - double WIDTH = Units.inchesToMeters(20.75); - double LENGTH = Units.inchesToMeters(20.75); + double WIDTH = Units.inchesToMeters(32.5); + double LENGTH = Units.inchesToMeters(27.375); double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(13.0); double MAX_MODULE_SPEED = 4.9; double MAX_MODULE_ACCEL = 15.0; + double MAX_LINEAR_VELOCITY = 15.0; + double MAX_ANGULAR_VELOCITY = 12.0; + double MODULE_VELOCITY_DEADBAND = 0.05; SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); // TODO: make 0.25 and test - public interface Assist { - SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change + // public interface Assist { + // SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change - double BUZZ_INTENSITY = 1; + // double BUZZ_INTENSITY = 1; - // angle PID - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); - SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); - SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); + // // angle PID + // SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); + // SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); + // SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); - double ANGLE_DERIV_RC = 0.05; - double REDUCED_FF_DIST = 0.75; - } + // double ANGLE_DERIV_RC = 0.05; + // double REDUCED_FF_DIST = 0.75; + // } - // TODO: Tune these values - public interface Motion { - SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0); - SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0); - SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540)); - SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720)); - - PathConstraints DEFAULT_CONSTRAINTS = - new PathConstraints( - MAX_VELOCITY.get(), - MAX_ACCELERATION.get(), - MAX_ANGULAR_VELOCITY.get(), - MAX_ANGULAR_ACCELERATION.get()); - - PIDConstants XY = new PIDConstants(2.5, 0, 0.02); - PIDConstants THETA = new PIDConstants(4, 0, 0.1); - } + // // TODO: Tune these values + // public interface Motion { + // SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0); + // SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0); + // SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540)); + // SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720)); + + // PathConstraints DEFAULT_CONSTRAINTS = + // new PathConstraints( + // MAX_VELOCITY.get(), + // MAX_ACCELERATION.get(), + // MAX_ANGULAR_VELOCITY.get(), + // MAX_ANGULAR_ACCELERATION.get()); + + // PIDConstants XY = new PIDConstants(2.5, 0, 0.02); + // PIDConstants THETA = new PIDConstants(4, 0, 0.1); + // } public interface Encoder { public interface Drive { @@ -214,6 +214,7 @@ public interface Turn { SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", 0.25582); SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 0.00205); SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", 0.00020123); + double kT = 1.0 / DCMotor.getKrakenX60Foc(1).KtNMPerAmp; SmartBoolean INVERTED = new SmartBoolean("Swerve/Turn/INVERTED", true); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 2cb4ff48..0b616509 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft; import com.stuypulse.robot.constants.Settings.Swerve.FrontRight; import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.controllers.DriveController; import com.stuypulse.robot.subsystems.swerve.modules.KrakenSwerveModule; import com.stuypulse.robot.subsystems.swerve.modules.SwerveModule; @@ -22,61 +23,27 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -/* - * Fields: - * - swerveModules: AbstractSwerveModule... - * - kinematics: SwerveDriveKinematics - * - gyro: AHRS - * - modules2D: FieldObject2D[] - * - * Tasks: - * - drive - * - followDrive - * - trackingDrive - * - aligning (Trap, Speaker, Amp) - * - GTADrive (shooting while driving) - * - * Methods: - * + singleton - * + initFieldObject(Field2D field): void - * + getModulePositions(): Translation2D[] - * + getModuleStates(): SwerveModuleStates[] - * + getModuleOffsets(): Rotation2D[] - * + getChassisSpeeds(): ChassisSpeed[] - * + setModuleStates(SwerveModuleState... states): void - * + setChassisSpeed(ChassisSpeed): void - * + drive(double, Rotation2D) - * + stop(double, Rotation2D) - * - * SwerveDrive.java - * Methods: - * - getGyroAngle(): Rotation2D - * - getGyroYaw(): Rotation2D - * - getGyroPitch(): Rotation2D - * - getGyroRoll(): Rotation2D - * - getKinematics(): SwerveDriveKinematics - * + periodic(): void - * - * - */ public class SwerveDrive extends SubsystemBase { private static final SwerveDrive instance; static { instance = new SwerveDrive( - new KrakenSwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.ENCODER), - new KrakenSwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.ENCODER), - new KrakenSwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.ENCODER), - new KrakenSwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.ENCODER) + new KrakenSwerveModule(FrontRight.ID, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.ENCODER), + new KrakenSwerveModule(FrontLeft.ID, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.ENCODER), + new KrakenSwerveModule(BackLeft.ID, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.ENCODER), + new KrakenSwerveModule(BackRight.ID, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.ENCODER) ); } @@ -89,8 +56,16 @@ public static SwerveDrive getInstance() { private final Pigeon2 gyro; private final FieldObject2d[] modules2D; + private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); + // TODO: Add SSG + private final DriveController driveController; + private final StructArrayPublisher statesPub; + // Status Signals + private final StatusSignal yaw; + private final StatusSignal yawVelocity; + /** * Creates a new Swerve Drive using the provided modules * @@ -102,8 +77,18 @@ protected SwerveDrive(SwerveModule... modules) { gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*"); modules2D = new FieldObject2d[modules.length]; + driveController = new DriveController(); + statesPub = NetworkTableInstance.getDefault() .getStructArrayTopic("SmartDashboard/Swerve/States", SwerveModuleState.struct).publish(); + + yaw = gyro.getYaw(); + yawVelocity = gyro.getAngularVelocityZWorld(); + gyro.getConfigurator().apply(new Pigeon2Configuration()); + gyro.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(250); + yawVelocity.setUpdateFrequency(100); + gyro.optimizeBusUtilization(); } public void initFieldObject(Field2d field) { @@ -133,12 +118,14 @@ public SwerveModulePosition[] getModulePositions() { return offsets; } + // TODO: Offset should be to the center of wheels, not corner of robot public Translation2d[] getModuleOffsets() { - Translation2d[] offsets = new Translation2d[modules.length]; - for (int i = 0; i < modules.length; i++) { - offsets[i] = modules[i].getModuleOffset(); - } - return offsets; + return new Translation2d[] { + new Translation2d(Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0), + new Translation2d(-Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0), + new Translation2d(-Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0), + new Translation2d(Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0) + }; } public ChassisSpeeds getChassisSpeeds() { @@ -207,26 +194,38 @@ public void stop() { setChassisSpeeds(new ChassisSpeeds()); } + public void acceptTeleopInput( + double controllerX, double controllerY, double controllerOmega) { + if(DriverStation.isTeleopEnabled()) { + driveController.acceptDriveInput(controllerX, controllerY, controllerOmega); + } + + } + /** Gyro **/ public Rotation2d getGyroAngle() { return gyro.getRotation2d(); } public StatusSignal getGyroYaw() { - return gyro.getYaw(); + return yaw; } public StatusSignal getGyroYawVelocity() { - return gyro.getAngularVelocityZWorld(); + return yawVelocity; } @Override public void periodic() { statesPub.set(getModuleStates()); + desiredSpeeds = driveController.update(); + + BaseStatusSignal.refreshAll(yaw, yawVelocity); + SmartDashboard.putNumber("Swerve/Gyro/Angle (deg)", getGyroAngle().getDegrees()); - SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", getGyroYaw().getValueAsDouble()); - SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", getGyroYawVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", yaw.getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", yawVelocity.getValueAsDouble()); SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getAccelerationX().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getAccelerationY().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java new file mode 100644 index 00000000..c54c86c1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java @@ -0,0 +1,68 @@ +package com.stuypulse.robot.subsystems.swerve.controllers; + +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.stuylib.network.SmartNumber; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class DriveController { + private static final SmartNumber controllerDeadband = + new SmartNumber("Controller/DriveController/Deadband", 0.1); + private static final SmartNumber maxAngularVelocityScalar = + new SmartNumber("Controller/DriveController/MaxAngularVelocityScalar", 0.65); + + + private double controllerX = 0; + private double controllerY = 0; + private double controllerOmega = 0; + + /** + * Accepts new drive input from joysticks. + * + * @param x Desired x velocity scalar, -1..1 + * @param y Desired y velocity scalar, -1..1 + * @param omega Desired omega velocity scalar, -1..1 + */ + public void acceptDriveInput(double x, double y, double omega) { + controllerX = x; + controllerY = y; + controllerOmega = omega; + } + + public ChassisSpeeds update() { + Translation2d linearVelocity = calcLinearVelocity(controllerX, controllerY); + double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get()); + omega = Math.copySign(omega * omega, omega); + + final double maxLinearVelocity = Swerve.MAX_LINEAR_VELOCITY; + final double maxAngularVelocity = + Swerve.MAX_ANGULAR_VELOCITY * (maxAngularVelocityScalar.get()); + + return new ChassisSpeeds( + linearVelocity.getX() * maxLinearVelocity, + linearVelocity.getY() * maxLinearVelocity, + omega * maxAngularVelocity); + } + + + public static Translation2d calcLinearVelocity(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), controllerDeadband.get()); + Rotation2d linearDirection = new Rotation2d(x, y); + + // Square magnitude + linearMagnitude = linearMagnitude * linearMagnitude; + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + return linearVelocity; + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index 16e03ce9..8502ac83 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -2,7 +2,10 @@ import java.util.concurrent.Executor; import java.util.concurrent.Executors; +import java.util.function.Supplier; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; @@ -27,7 +30,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -44,6 +46,20 @@ public class KrakenSwerveModule extends SwerveModule { private final IFilter targetAcceleration; + // Status Signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveTorqueCurrent; + + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnSupplyCurrent; + private final StatusSignal turnTorqueCurrent; + private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration turnConfig = new TalonFXConfiguration(); private static final Executor brakeModeExecutor = Executors.newFixedThreadPool(8); // TODO: Choose n threaqds @@ -59,13 +75,12 @@ public class KrakenSwerveModule extends SwerveModule { public KrakenSwerveModule( String id, - Translation2d location, Rotation2d angleOffset, int driveMotorID, int turnMotorID, int turnEncoderID ) { - super(id, location); + super(id); this.angleOffset = angleOffset; @@ -107,7 +122,37 @@ public KrakenSwerveModule( // driveConfig.CurrentLimits.StatorCurrentLimit = 65; // 65A stator current limit // driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Enable stator current limiting - driveMotor.setPosition(0); + // 250 hz signals + drivePosition = driveMotor.getPosition(); + turnPosition = turnMotor.getPosition(); + BaseStatusSignal.setUpdateFrequencyForAll(250, drivePosition, turnPosition); + + // 100 hz signals + driveVelocity = driveMotor.getVelocity(); + driveAppliedVolts = driveMotor.getMotorVoltage(); + driveSupplyCurrent = driveMotor.getSupplyCurrent(); + driveTorqueCurrent = driveMotor.getTorqueCurrent(); + turnAbsolutePosition = + () -> + Rotation2d.fromRotations(turnEncoder.getAbsolutePosition().getValueAsDouble()) + .minus(angleOffset); + turnVelocity = turnMotor.getVelocity(); + turnAppliedVolts = turnMotor.getMotorVoltage(); + turnSupplyCurrent = turnMotor.getSupplyCurrent(); + turnTorqueCurrent = turnMotor.getTorqueCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 100, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + // driveMotor.setPosition(0); + turnMotor.setPosition(turnAbsolutePosition.get().getRotations(), 1.0); pivotController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD) .setOutputFilter(x -> -x); @@ -120,7 +165,7 @@ public KrakenSwerveModule( driveMotor.optimizeBusUtilization(); turnMotor.optimizeBusUtilization(); - // turnMotor is a TalonFX, so we need to configure it as such + // turnMotor is a TalonFX, so we do not need to configure it as such // Motors.disableStatusFrames(turnMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY); // Motors.Swerve.TURN_CONFIG.configure(turnMotor); } @@ -130,25 +175,25 @@ public KrakenSwerveModule( /****************/ public double getPosition() { - return driveMotor.getPosition().getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; + return drivePosition.getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; } public double getTurnPosition() { - return turnMotor.getPosition().getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; + return turnPosition.getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; } @Override public double getVelocity() { - return driveMotor.getVelocity().getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; + return driveVelocity.getValueAsDouble() * Encoder.Drive.POSITION_CONVERSION; } public double getTurnVelocity() { - return turnMotor.getVelocity().getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; + return turnVelocity.getValueAsDouble() * Encoder.Turn.POSITION_CONVERSION; } @Override public Rotation2d getAngle() { - return Rotation2d.fromRotations(turnEncoder.getAbsolutePosition().getValueAsDouble()) + return Rotation2d.fromRotations(turnAbsolutePosition.get().getRotations()) .minus(angleOffset); } @@ -187,6 +232,11 @@ public void setCharacterization(double input) { driveMotor.setControl(currentControl.withOutput(input)); } + public void setCharacterization(double turnSetpointRads, double input) { + setTurnPosition(turnSetpointRads); + setCharacterization(input); + } + public void setDriveVelocity(double velocityRadsPerSec, double feedForward) { driveMotor.setControl( velocityTorqueCurrentFOC @@ -194,7 +244,7 @@ public void setDriveVelocity(double velocityRadsPerSec, double feedForward) { .withFeedForward(feedForward)); } - public void runTurnPositionSetpoint(double angleRads) { + public void setTurnPosition(double angleRads) { turnMotor.setControl(positionControl.withPosition(Units.radiansToRotations(angleRads))); } @@ -220,6 +270,11 @@ public void setTurnBrakeMode(boolean enable) { }); } + public void setBrakeMode(boolean enable) { + setDriveBrakeMode(enable); + setTurnBrakeMode(enable); + } + public void stop() { driveMotor.setControl(neutralControl); turnMotor.setControl(neutralControl); @@ -229,10 +284,25 @@ private double convertDriveVel(double speedMetersPerSecond) { return speedMetersPerSecond / Encoder.Drive.POSITION_CONVERSION; } + // TODO: FIX PERIODIC @Override public void periodic() { super.periodic(); + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent); + + BaseStatusSignal.refreshAll( + turnPosition, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + final boolean USE_ACCEL = true; double velocity = convertDriveVel(getTargetState().speedMetersPerSecond); @@ -265,7 +335,7 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Turn Voltage", pivotController.getOutput()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Turn Current", turnMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Angle Error", pivotController.getError().toDegrees()); - SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Raw Encoder Angle", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble())); + SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Raw Encoder Angle", Units.rotationsToDegrees(turnAbsolutePosition.get().getRotations())); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java index 59aa4c9e..af04c728 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/SwerveModule.java @@ -1,7 +1,6 @@ package com.stuypulse.robot.subsystems.swerve.modules; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -10,14 +9,12 @@ public abstract class SwerveModule extends SubsystemBase { private final String id; - private final Translation2d offset; private SwerveModuleState targetState; - public SwerveModule(String id, Translation2d offset) { + public SwerveModule(String id) { this.id = id; - this.offset = offset; - + targetState = new SwerveModuleState(); } @@ -25,10 +22,6 @@ public final String getId() { return this.id; } - public final Translation2d getModuleOffset() { - return this.offset; - } - public abstract double getVelocity(); public abstract Rotation2d getAngle();