From 6bf96455e9ca458c920ff635fed368290c468215 Mon Sep 17 00:00:00 2001 From: PGgit08 <60079012+PGgit08@users.noreply.github.com> Date: Mon, 22 Jan 2024 21:29:02 -0500 Subject: [PATCH] some cleanup and utilization of talonfx config class invert option --- .../{RotateWrist.java => AngleShooter.java} | 6 ++--- .../robot/subsystems/ShooterSubsystem.java | 3 --- .../subsystems/SwerveDriveSubsystem.java | 12 +-------- .../java/frc/robot/utils/SwerveModule.java | 27 +++++++++---------- src/main/java/frc/robot/utils/UtilFuncs.java | 2 +- 5 files changed, 18 insertions(+), 32 deletions(-) rename src/main/java/frc/robot/commands/shooter/{RotateWrist.java => AngleShooter.java} (87%) diff --git a/src/main/java/frc/robot/commands/shooter/RotateWrist.java b/src/main/java/frc/robot/commands/shooter/AngleShooter.java similarity index 87% rename from src/main/java/frc/robot/commands/shooter/RotateWrist.java rename to src/main/java/frc/robot/commands/shooter/AngleShooter.java index 1758b68..2c45957 100644 --- a/src/main/java/frc/robot/commands/shooter/RotateWrist.java +++ b/src/main/java/frc/robot/commands/shooter/AngleShooter.java @@ -7,11 +7,11 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; -public class RotateWrist extends Command { - /** Creates a new ShooterWrist. */ +public class AngleShooter extends Command { + /** Creates a new AngleShooter. */ private ShooterSubsystem _shooter; - public RotateWrist(ShooterSubsystem shooter) { + public AngleShooter(ShooterSubsystem shooter) { // Use addRequirements() here to declare subsystem dependencies. _shooter = shooter; addRequirements(_shooter); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 52e10d8..cdfc1ed 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -63,8 +63,5 @@ public void setVelocity(double velocity) { // a similar controller setup can be found in SwerveModule _leftMotor.set(flywheel_output + flywheel_pid); } - - - } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index be7f6d6..c5739c2 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -15,10 +15,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics.SwerveDriveWheelStates; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.wpilibj.BuiltInAccelerometer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -89,9 +85,6 @@ public class SwerveDriveSubsystem extends SubsystemBase { private Field2d _field = new Field2d(); - private final StructArrayPublisher _scopePublisher = NetworkTableInstance.getDefault().getStructArrayTopic( - "ScopeState", SwerveModuleState.struct).publish(); - /** A boolean for whether the swerve is field oriented or not. */ public boolean fieldOriented = false; @@ -117,8 +110,7 @@ public Pose2d getPose() { /** Get the drive's chassis speeds (robot relative). */ public ChassisSpeeds getRobotRelativeSpeeds() { - return Constants.Physical.SWERVE_KINEMATICS.toChassisSpeeds( - _frontLeft.getState(), _frontRight.getState(), _backRight.getState(), _backLeft.getState()); + return Constants.Physical.SWERVE_KINEMATICS.toChassisSpeeds(getStates()); } /** Creates a new SwerveDriveSubsystem. */ @@ -188,8 +180,6 @@ public void periodic() { _field.setRobotPose(_pose); SmartDashboard.putData("FIELD", _field); - _scopePublisher.set(getStates()); - _robotSpeed = Math.sqrt( Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2) diff --git a/src/main/java/frc/robot/utils/SwerveModule.java b/src/main/java/frc/robot/utils/SwerveModule.java index 8c779cf..f0a861b 100644 --- a/src/main/java/frc/robot/utils/SwerveModule.java +++ b/src/main/java/frc/robot/utils/SwerveModule.java @@ -50,10 +50,11 @@ public SwerveModule( _driveMotor = new TalonFX(driveMotorId); _rotationMotor = new TalonFX(rotationMotorId); + // NO NEED FOR THIS CODE ANYMORE, FIGURED HOW TO DO OFFSETS IN PHOENIX TUNER // new stuff because CTRE update - MagnetSensorConfigs encoderConfig = new MagnetSensorConfigs(); - encoderConfig.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - encoderConfig.MagnetOffset = (angleOffset / 180) / 2; + // MagnetSensorConfigs encoderConfig = new MagnetSensorConfigs(); + // encoderConfig.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + // encoderConfig.MagnetOffset = (angleOffset / 180) / 2; _encoder = new CANcoder(encoderId); // _encoder.getConfigurator().apply(encoderConfig); @@ -64,7 +65,7 @@ public SwerveModule( _rotationController.enableContinuousInput(-180, 180); TalonFXConfig.configureFalcon(_driveMotor, false); - TalonFXConfig.configureFalcon(_rotationMotor, false); + TalonFXConfig.configureFalcon(_rotationMotor, true); } /** Set the percentage output of the drive motor. */ @@ -77,7 +78,7 @@ public void rotate(double speed) { _rotationMotor.set(speed); } - /** Get the absolute angle of the module (-180 to 180 degrees). */ + /** Get the absolute angle of the module as an int (-180 to 180 degrees). */ public int getAngle() { return Double.valueOf(_encoder.getAbsolutePosition().getValueAsDouble() * 2 * 180).intValue(); // ctre update } @@ -119,20 +120,18 @@ public void setState(SwerveModuleState state) { SmartDashboard.putNumber("DESIRED ANGLE", state.angle.getDegrees()); state = SwerveModuleState.optimize(state, new Rotation2d(Math.toRadians(getAngle()))); - double speed = - MathUtil.clamp( - state.speedMetersPerSecond, - -Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, - Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); + double speed = MathUtil.clamp( + state.speedMetersPerSecond, + -Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, + Constants.Speeds.SWERVE_DRIVE_MAX_SPEED + ); - double rotation_volts = - -MathUtil.clamp( - _rotationController.calculate(getAngle(), state.angle.getDegrees()), -1.5, 1.5); + double rotation_volts = MathUtil.clamp(_rotationController.calculate(getAngle(), state.angle.getDegrees()), -1.5, 1.5); double drive_pid = _driveController.calculate(getDriveVelocity(), speed); double drive_output = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); - rotate(-(rotation_volts / RobotController.getBatteryVoltage())); + rotate(rotation_volts / RobotController.getBatteryVoltage()); // drive(drive_output + drive_pid); // drive(-0.08); } diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java index de8e7dd..eddafb7 100644 --- a/src/main/java/frc/robot/utils/UtilFuncs.java +++ b/src/main/java/frc/robot/utils/UtilFuncs.java @@ -3,7 +3,7 @@ package frc.robot.utils; -/** Any utility functions for anything. */ +/** Any utility functions are here. */ public final class UtilFuncs { /** * Applies deadband to a certain value.