Merge branch 'Amrit-S' of https://github.com/frc-7419/FinalAssessment2024 into Amrit-S

Author: Amrit-s-RIS

src/main/java/frc/robot/subsystems/drive/DrivebaseSubsytem.java

@@ -1,12 +1,10 @@
 package frc.robot.subsystems.drive;
 
-import com.kauailabs.vmx.AHRSJNI;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc.Constants.SwerveConstants;
-import com.kauailabs.navx.frc.AHRS;
 
 public class DrivebaseSubsytem extends SubsystemBase {
   /** Creates a new DriveBase. */
@@ -15,10 +13,10 @@ public class DrivebaseSubsytem extends SubsystemBase {
   private final SwerveModule backRight;
   private final SwerveModule backLeft;
   public final SwerveDriveOdometry odometry;
-  private final AHRS ahrs;
+  
   public DrivebaseSubsytem() {
     this.odometry = new SwerveDriveOdometry(null, null, null);
-    this.ahrs = new AHRS();
+   
     frontRight = new SwerveModule(SwerveConstants.frontRightTurnMotorID, SwerveConstants.frontRightDriveMotorID, SwerveConstants.frontRightTurnEncoderID,SwerveConstants.frontRightEncoderOffset);
     frontLeft = new SwerveModule(SwerveConstants.frontLeftTurnMotorID, SwerveConstants.frontLeftDriveMotorID, SwerveConstants.frontLeftTurnEncoderID, SwerveConstants.frontLeftEncoderOffset);
     backRight = new SwerveModule(SwerveConstants.backRightTurnMotorID, SwerveConstants.backRightDriveMotorID, SwerveConstants.backRightTurnEncoderID, SwerveConstants.backRightEncoderOffset);
@@ -31,18 +29,7 @@ public DrivebaseSubsytem() {
   public void periodic() {
 
   }
-  public double getPitch(){
-    return ahrs.getPitch();
-  }
-  public void resetYaw(){
-     ahrs.zeroYaw();
-  }
-  public double getYaw(){
-    return ahrs.getYaw();
-  }
-  public double getRoll(){
-    return ahrs.getRoll();
-  }
+  
   public Boolean distanceReached(double distance){
     return backLeft.reachedDistanced(distance) && frontRight.reachedDistanced(distance) && backRight.reachedDistanced(distance) && frontLeft.reachedDistanced(distance);
     //why is the and && in java :(

src/main/java/frc/robot/subsystems/drive/SwerveModule.java

@@ -30,6 +30,7 @@ public class SwerveModule {
 
 
 //to be frank this code is pretty chopped
+//i take that back this code is horrendous but it functions so i will not be touching it.
 public SwerveModule(int turnMotorId, int driveMotorId, int turnEncoderId, double turnEncoderOffset){
     this.angleController = new PIDController(SwerveConstants.anglekP, SwerveConstants.anglekI, SwerveConstants.anglekD);
 
@@ -65,6 +66,19 @@ public void setPower(double power){
     turnMotor.setVoltage(power);
     driveMotor.setVoltage(power);
 
+    
+
+
+}
+public void setModuleState(SwerveModuleState state){
+    double speed = state.speedMetersPerSecond;
+    angleController.setSetpoint(state.angle.getDegrees());
+    driveMotor.set(speed);
+    angleController.calculate(state.angle.getDegrees());
+
+
+
+
 
 
 }