Made wheels reset to zero when enabled and tried subtracting offsets for PID

Author: RushilGhosh

src/main/java/frc/robot/Robot.java

@@ -82,7 +82,8 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer.stopAllMotors();
+    m_robotContainer.resetToForwardPosition();
+    //m_robotContainer.stopAllMotors();
   }
 
   /** This function is called periodically during operator control. */

src/main/java/frc/robot/RobotContainer.java

@@ -78,11 +78,16 @@ private void configureBindings() {
     // // cancelling on release.
     // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
   }
-
   public void stopAllMotors() {
     m_driveSubsystem.stopAllMotors();
   }
 
+public void resetToForwardPosition() {
+  m_driveSubsystem.resetToForwardPosition();
+}
+
+
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -80,7 +80,7 @@ public class DriveSubsystem extends SubsystemBase {
     private double[] CAN_CODER_ANGLE_OFFSETS = { // These values are all in degrees.
         21.53,  // BACK_RIGHT
         74.62,  // BACK_LEFT
-        83.50,  // FRONT_LEFT
+        80.07,  // FRONT_LEFT
         111.80, // FRONT_RIGHT
     };
 
@@ -438,7 +438,6 @@ public void drive(double xAxis, double yAxis, double turn) {
      * or according to the current trajectory (during autonomous).
      */
     public void periodic() {
-        if (true) return;
         switch (driveType) {
             case DIFFERENTIAL_DRIVE:
                 // If the joystick is being moved, then the shuffleboard will be
@@ -513,8 +512,9 @@ public void periodic() {
                     // automatically.
                     // rotations.refresh();
                     CANCoderAnglesRadians[i] = rotations.getValueAsDouble() * 2 * Math.PI;
-
-                    // TODO: We need to subtract the offset to the CANCoder angle.
+                    
+                    //Uche suggested this but it did not work ):
+                    //CANCoderAnglesRadians[i] -= Math.toRadians(CAN_CODER_ANGLE_OFFSETS[i]);
                 }
 
                 // TODO: Use the CANCoder's measurements for the PID
@@ -548,7 +548,13 @@ public void periodic() {
                                                                          swerveModuleState.angle.getRadians());
 
                         // Set the output to the pivot motor.
+                        //if(i == 0) {
                         pivotMotor.set(power);
+                        if (DriverStation.isTeleopEnabled()) {
+                            System.out.println("i: " + i + " can: " + CANCoderAnglesRadians[i] + " angle: " + swerveModuleState.angle.getRadians());
+                            System.out.println("i: " + i + " power: " + power);
+                        }
+                        //}
                     }
 
                     // We are powering the drive motor without PID because we do

src/main/java/frc/robot/subsystems/InputSubsystem.java

@@ -300,11 +300,11 @@ private void controllerCheck() {
             try {
                 if (xboxController == null) {
                     xboxController = new XboxController(i);
-                    System.out.printf("controllerCheck: Found xbox controller on port %d.\n", i);
+                    //System.out.printf("controllerCheck: Found xbox controller on port %d.\n", i);
                     continue;
                 } else if (!xboxController.isConnected()) {
                     xboxController = null;
-                    System.out.printf("controllerCheck: Xbox controller is disconnected.\n");
+                    //System.out.printf("controllerCheck: Xbox controller is disconnected.\n");
                     continue;
                 }
             } catch (Exception e) {
@@ -400,7 +400,7 @@ private boolean assignJoystick(Joystick j) {
                 !secondaryJoystick.isConnected() ? "secondary joystick on port %d was disconnected".formatted(secondaryJoystick.getPort()) :
                 "secondary joystick is connected on port %d".formatted(secondaryJoystick.getPort());
 
-            System.out.printf("%s: %s and %s; %s.\n", prefix, mainJoystickString, secondaryJoystickString, message);
+            //System.out.printf("%s: %s and %s; %s.\n", prefix, mainJoystickString, secondaryJoystickString, message);
         }
         return success;
     }