Make code work

Update basic functions for apriltags and outline for integration

src/main/java/frc4388/robot/Constants.java

@@ -24,7 +24,7 @@ import frc4388.utility.LEDPatterns;
 public final class Constants {
   public static final class SwerveDriveConstants {
 
-    public static final double ROTATION_SPEED = -0.7;
+    public static final double ROTATION_SPEED = 2.0;
 
     public static final class IDs {
       public static final int LEFT_FRONT_WHEEL_ID = 2;
@@ -65,13 +65,13 @@ public final class Constants {
       public static final int CANCODER_TICKS_PER_ROTATION = 4096;
 
       public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value
-      public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value
 
-      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 MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value
+      public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value
 
       public static final double TICKS_PER_MOTOR_REV = 2048;
-      public static final double WHEEL_DIAMETER_INCHES = 3.9;
+      public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value
       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;
@@ -88,6 +88,7 @@ public final class Constants {
       public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
       public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
 
+
       // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values
       // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values
       // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values
@@ -105,11 +106,11 @@ public final class Constants {
       
     }
 
-    public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
+    public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value
 
     // dimensions
-    public static final double WIDTH = 18.5;
-    public static final double HEIGHT = 18.5;
+    public static final double WIDTH = 18.5; // TODO: find the actual value
+    public static final double HEIGHT = 18.5; // TODO: find the actual value
     public static final double HALF_WIDTH = WIDTH / 2.d;
     public static final double HALF_HEIGHT = HEIGHT / 2.d;
 
@@ -137,10 +138,7 @@ public final class Constants {
   public static final class OIConstants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
-    
-    public static final double LEFT_AXIS_DEADBAND = 0.1;
-    public static final double RIGHT_AXIS_DEADBAND = 0.6;
-
-    public static final boolean SKEW_STICKS = true; 
+    public static final double LEFT_AXIS_DEADBAND = 0.04;
+    public static final boolean SKEW_STICKS = false;
   }
 }

src/main/java/frc4388/robot/Robot.java

@@ -7,15 +7,15 @@
 
 package frc4388.robot;
 
-
-import frc4388.robot.subsystems.Vision;
-
+import java.lang.System;
+import java.lang.reflect.Array;
 import java.util.Arrays;
 
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.RobotTime;
+import frc4388.robot.subsystems.Vision;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -29,7 +29,9 @@ public class Robot extends TimedRobot {
   
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
-  private Vision Vision = new Vision();
+
+  //private Vision Apriltag = new Vision();
+
 
   /**
    * This function is run when the robot is first started up and should be
@@ -40,7 +42,6 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
-
   }
 
   /**
@@ -59,7 +60,10 @@ public class Robot extends TimedRobot {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
-    System.out.println(Arrays.toString(Vision.getAprilTags()));
+
+    //final Object[] apriltagPos = Apriltag.getApriltagLocation();
+
+    //ystem.out.print(apriltagPos[0]);
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -10,12 +10,11 @@ package frc4388.robot;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.AutoBalance;
+import frc4388.robot.commands.DriveWithInput;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
 
@@ -31,16 +30,13 @@ public class RobotContainer {
     public final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
-    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.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
     // private final LED m_robotLED = new LED(m_robotMap.LEDController);
     
 
     /* Controllers */
-    // private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
-    // private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
-
-    private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
-    private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
+    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
+    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -50,11 +46,15 @@ public class RobotContainer {
 
         /* Default Commands */
 
-        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() ->
-            m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true)
-          , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
+        m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, 
+                                                                () -> getDriverController().getLeftXAxis(), 
+                                                                () -> getDriverController().getLeftYAxis(), 
+                                                                () -> getDriverController().getRightXAxis(),
+                                                                false));
         
         // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+
+    
     }
 
 
@@ -67,20 +67,18 @@ public class RobotContainer {
     private void configureButtonBindings() {
         /* Driver Buttons */
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
-        
-        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
-        //     // .onFalse()
+        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset()));
+            // .onFalse()
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        /* Operator Buttons */
+
+        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
             .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-            
-        // /* Operator Buttons */
-        // // interrupt button
-        // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-        //     .onTrue(new InstantCommand());
+
+        // interrupt button
+        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand());
     }
 
     /**
@@ -96,36 +94,28 @@ public class RobotContainer {
     /**
      * Add your docs here.
      */
-    // public IHandController getDriverController() {
-    //     return m_driverXbox;
-    // }
-
-    public DeadbandedXboxController getDeadbandedDriverController() {
-        return this.m_driverXbox;
-    }
-
-    public DeadbandedXboxController getDeadbandedOperatorController() {
-        return this.m_operatorXbox;
+    public IHandController getDriverController() {
+        return m_driverXbox;
     }
 
     /**
      * Add your docs here.
      */
-    // public IHandController getOperatorController() {
-    //     return m_operatorXbox;
-    // }
+    public IHandController getOperatorController() {
+        return m_operatorXbox;
+    }
 
     /**
      * Add your docs here.
      */
-    // public Joystick getOperatorJoystick() {
-    //     return m_operatorXbox.getJoyStick();
-    // }
+    public Joystick getOperatorJoystick() {
+        return m_operatorXbox.getJoyStick();
+    }
 
     /**
      * Add your docs here.
      */
-    // public Joystick getDriverJoystick() {
-    //     return m_driverXbox.getJoyStick();
-    // }
+    public Joystick getDriverJoystick() {
+        return m_driverXbox.getJoyStick();
+    }
 }

src/main/java/frc4388/robot/RobotMap.java

@@ -7,7 +7,6 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
@@ -114,12 +113,6 @@ public class RobotMap {
     rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
     rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
 
-    // set neutral mode
-    leftFrontSteer.setNeutralMode(NeutralMode.Brake);
-    rightFrontSteer.setNeutralMode(NeutralMode.Brake);
-    leftBackSteer.setNeutralMode(NeutralMode.Brake);
-    rightBackSteer.setNeutralMode(NeutralMode.Brake);
-
     // initialize SwerveModules
     this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
     this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);

src/main/java/frc4388/robot/commands/AutoBalance.java

@@ -5,7 +5,6 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.RobotGyro;
@@ -16,7 +15,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 
 	/** Creates a new AutoBalanceTF2. */
 	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
-		super(0.6, 0, 0, 0);
+		super(1.0, 0, 0, 0);
 
 		this.gyro = gyro;
 		this.drive = drive;
@@ -35,7 +34,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 	public void runWithOutput(double output) {
 		double out2 = MathUtil.clamp(output / 40, -.5, .5);
 		if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
-		drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
+		drive.drive(out2, 0, 0, false);
 	}
 
 	@Override

src/main/java/frc4388/robot/commands/DriveWithInput.java

@@ -0,0 +1,75 @@
+// 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.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class DriveWithInput extends CommandBase {
+
+  private final SwerveDrive swerve;
+
+  private final Supplier<Double> xSpeed;
+  private final Supplier<Double> ySpeed;
+  private final Supplier<Double> rot;
+  private final boolean fieldRelative;
+
+  private final SlewRateLimiter xLimiter, yLimiter, rotLimiter;
+
+  /** Creates a new DriveWithInput. */
+  public DriveWithInput(SwerveDrive swerve, Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rot, boolean fieldRelative) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.swerve = swerve;
+    this.xSpeed = xSpeed;
+    this.ySpeed = ySpeed;
+    this.rot = rot;
+    this.fieldRelative = fieldRelative;
+
+    this.xLimiter = new SlewRateLimiter(3);
+    this.yLimiter = new SlewRateLimiter(3);
+    this.rotLimiter = new SlewRateLimiter(3);
+
+    addRequirements(swerve);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double x = xSpeed.get();
+    double y = ySpeed.get();
+    double r = rot.get();
+
+    x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI));
+  
+    swerve.drive(x, y, r, fieldRelative);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    System.out.println("----------------------------------------------------------------");
+    System.out.println("DriveWithInput ended");
+    System.out.println("Interrupted: " + interrupted);
+    System.out.println("----------------------------------------------------------------");
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/subsystems/Apriltags.java

@@ -0,0 +1,35 @@
+package frc4388.robot.subsystems;
+
+//import edu.wpi.first.apriltag.AprilTag;
+//import edu.wpi.first.math.geometry.Pose3d;
+//import edu.wpi.first.math.geometry.Rotation3d;
+//import edu.wpi.first.networktables.NetworkTable;
+//import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Apriltags {
+    public Object[] getApriltagPosition() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        return new Object[] {true, 
+            tagTable.getEntry("TagPosX"),
+            tagTable.getEntry("TagPosY"),
+            tagTable.getEntry("TagPosZ")
+        };
+    }
+
+    public Object[] getApriltagRotation() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        return new Object[] {true, 
+            tagTable.getEntry("TagRotY"),
+            tagTable.getEntry("TagRotP"),
+            tagTable.getEntry("TagRotR")
+        };
+    }
+
+    public boolean isAprilTag() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+        return tagTable.getEntry("IsTag").getBoolean(false);
+    }
+}

src/main/java/frc4388/robot/subsystems/Location.java

@@ -0,0 +1,44 @@
+package frc4388.robot.subsystems;
+
+import frc4388.robot.subsystems.Apriltags;
+
+public class Location {
+    final Apriltags Apriltag = new Apriltags();
+
+    public boolean isLimelight = false;
+    public boolean isApriltag = false;
+
+    //Determines which source to get pos and rot from and also resets
+    public void reoderPrio(){
+        isLimelight = false; //If limelight gets position and if within a certain range of poles
+        isApriltag = Apriltag.isAprilTag();
+    }
+
+    public Object[] getPosition() {
+        Object[] Position = {};
+
+        if(isLimelight){
+            //Return Limelight Position
+        }else if(isApriltag){
+            return Apriltag.getApriltagPosition();
+        }else{
+            //Return odometry Position, last resort
+        }
+
+        return Position;
+    }
+
+    public Object[] getRotation() {
+        Object[] Rotation = {};
+
+        if(isLimelight){
+            //Return Limelight Rotation
+        }else if(isApriltag){
+            return Apriltag.getApriltagRotation();
+        }else{
+            //Return odometry Rotation, last resort
+        }
+
+        return Rotation;
+    }
+}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -4,27 +4,20 @@
 
 package frc4388.robot.subsystems;
 
-
-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.SwerveDriveOdometry;
-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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.utility.RobotGyro;
 
 public class SwerveDrive extends SubsystemBase {
   
-  private SwerveModule leftFront;
-  private SwerveModule rightFront;
-  private SwerveModule leftBack;
-  private SwerveModule rightBack;
+  public SwerveModule leftFront;
+  public SwerveModule rightFront;
+  public SwerveModule leftBack;
+  public SwerveModule rightBack;
 
   private SwerveModule[] modules;
 
@@ -33,64 +26,39 @@ public class SwerveDrive extends SubsystemBase {
   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 SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); 
 
-  private RobotGyro gyro;
-
-  private SwerveDriveOdometry odometry;
+  // private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
+  //   kinematics, 
+  //   gyro.getRotation2d(),
+  //   new SwerveModulePosition[] {
+  //     leftFront.getPosition(),
+  //     rightFront.getPosition(),
+  //     leftBack.getPosition(),
+  //     rightBack.getPosition()
+  //   }
+  // );
 
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
-  public Rotation2d rotTarget = new Rotation2d();
-  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   /** Creates a new SwerveDrive. */
-  public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
+  public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) {
     this.leftFront = leftFront;
     this.rightFront = rightFront;
     this.leftBack = leftBack;
     this.rightBack = rightBack;
-    this.gyro = gyro;
-
-    this.odometry = new SwerveDriveOdometry(
-      kinematics, 
-      gyro.getRotation2d(),
-      new SwerveModulePosition[] {
-        leftFront.getPosition(),
-        rightFront.getPosition(),
-        leftBack.getPosition(),
-        rightBack.getPosition()
-      }
-    );
 
     this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
   }
 
-
-  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-    if (fieldRelative) {
-
-      if (rightStick.getNorm() > 0.1) {
-        rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
-      }
-
-      double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
-
-
-      // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
-      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-
-      // if (rightStick.getNorm() < .1) {
-      //   rot = 0;
-      // }
-
-      // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
-
-    } else {
-      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
-    }
-    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  // WPILib swerve drive example
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    // SwerveModuleState[] states = kinematics.toSwerveModuleStates(
+    //   fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) 
+    //                 : new ChassisSpeeds(xSpeed, ySpeed, rot)
+    // );
+    SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
+    setModuleStates(states);
   }
 
   /**
@@ -98,71 +66,70 @@ public class SwerveDrive extends SubsystemBase {
    * @param desiredStates Array of module states to set.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(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, false);
+      module.setDesiredState(state);
     }
   }
 
-  public double getGyroAngle() {
-    return gyro.getAngle();
-  }
-
-  public void resetGyro() {
-    gyro.reset();
-    setOdometry(getOdometry());
-    rotTarget = new Rotation2d(0);
-  }
-
   /**
    * Updates the odometry of the SwerveDrive.
    */
-  public void updateOdometry() {
-    odometry.update(
-      gyro.getRotation2d(), 
-      new SwerveModulePosition[] {
-        leftFront.getPosition(),
-        rightFront.getPosition(),
-        leftBack.getPosition(),
-        rightBack.getPosition()
-      }
-    );
-  }
+  // public void updateOdometry() {
+  //   odometry.update(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     }
+  //   );
+  // }
 
   /**
    * Gets the odometry of the SwerveDrive.
    * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
    */
-  public Pose2d getOdometry() {
-    return odometry.getPoseMeters();
-  }
+  // public Pose2d getOdometry() {
+  //   return odometry.getPoseMeters();
+  // }
 
   /**
    * Sets the odometry of the SwerveDrive.
    * @param pose Pose to set the odometry to.
    */
-  public void setOdometry(Pose2d pose) {
-    odometry.resetPosition(
-      gyro.getRotation2d(), 
-      new SwerveModulePosition[] {
-        leftFront.getPosition(),
-        rightFront.getPosition(),
-        leftBack.getPosition(),
-        rightBack.getPosition()
-      },
-      pose
-    );
-  }
+  // public void setOdometry(Pose2d pose) {
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     pose
+  //   );
+  // }
 
   /**
    * Resets the odometry of the SwerveDrive to 0.
-   * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
+   * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
    */
-  public void resetOdometry() {
-    setOdometry(new Pose2d());
-  }
+  // public void resetOdometry() {
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     new Pose2d()
+  //   );
+  // }
 
   public SwerveDriveKinematics getKinematics() {
     return this.kinematics;
@@ -171,14 +138,7 @@ public class SwerveDrive extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    updateOdometry();
-
-    SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
-    SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
-    SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
-
-    SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
-    SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
+    // updateOdometry();
   }
 
   /**

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -20,8 +20,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
 
 public class SwerveModule extends SubsystemBase {
-    private WPI_TalonFX driveMotor;
-    private WPI_TalonFX angleMotor;
+    public WPI_TalonFX driveMotor;
+    public WPI_TalonFX angleMotor;
     private CANCoder encoder;
 
     public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
@@ -44,9 +44,6 @@ public class SwerveModule extends SubsystemBase {
         angleMotor.configAllSettings(angleConfig);
 
         encoder.configMagnetOffset(offset);
-        
-        driveMotor.setSelectedSensorPosition(0);
-        driveMotor.config_kP(0, 0.2);
     }
 
     /**
@@ -82,18 +79,6 @@ public class SwerveModule extends SubsystemBase {
         return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
     }
 
-    public double getAngularVel() {
-        return this.angleMotor.getSelectedSensorVelocity();
-    }
-
-    public double getDrivePos() {
-        return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
-    }
-
-    public double getDriveVel() {
-        return this.driveMotor.getSelectedSensorVelocity(0);
-    }
-
     public void stop() {
         driveMotor.set(0);
         angleMotor.set(0);
@@ -126,32 +111,25 @@ public class SwerveModule extends SubsystemBase {
      * 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 desiredState, boolean ignoreAngle) {
+    public void setDesiredState(SwerveModuleState desiredState) {
         Rotation2d currentRotation = this.getAngle();
 
         SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
 
         // calculate the difference between our current rotational position and our new rotational position
-        Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative
+        Rotation2d rotationDelta = state.angle.minus(currentRotation);
 
         // calculate the new absolute position of the SwerveModule based on the difference in rotation
         double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
 
         // convert the CANCoder from its position reading to ticks
-        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient
-
-        if (!ignoreAngle) {
-            angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
-        }
+        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
+        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
 
         double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-        // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12;
 
+        driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
         // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
-        // driveMotor.set(0.1);
-        // double angleCorrection = getAngularVel() * 2.69;
-        driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-        // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
     }
 
     public void reset(double position) {