some cleanup and utilization of talonfx config class invert option

src/main/java/frc/robot/commands/shooter/RotateWrist.java → 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);

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);
   }
-
-
-
 }
 

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<SwerveModuleState> _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)

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);
   }

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.