From 9a2542fb38b3862ca62dd1d7f4dfba61c2c20db6 Mon Sep 17 00:00:00 2001
From: Ryan <90010729+Abhishrek05@users.noreply.github.com>
Date: Sat, 14 Jan 2023 15:30:38 -0700
Subject: [PATCH] converstions and getState

---
 .../java/frc4388/robot/RobotContainer.java    |  9 +++++++-
 .../robot/subsystems/SwerveModule.java        | 23 +++++++++++++++++++
 2 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 7e6699b..a42a463 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -34,6 +34,7 @@ public class RobotContainer {
     /* Subsystems */
     private 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);
@@ -67,13 +68,19 @@ public class RobotContainer {
      */
     private void configureButtonBindings() {
         /* Driver Buttons */
-        // test command to spin the robot while pressing A on the driver controller
+        
+        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> gyroRef.reset()));
 
         /* Operator Buttons */
         // activates "Lit Mode"
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED))
             .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED));
+
+        //New interupt button
+        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand());
     }
 
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index ea9bc9b..89a986a 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -7,6 +7,7 @@ package frc4388.robot.subsystems;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
@@ -81,6 +82,26 @@ public class SwerveModule extends SubsystemBase {
         return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
     }
 
+    public void stop() {
+        driveMotor.set(0);
+        angleMotor.set(0);
+    }
+
+    public void rotateToAngle(double angle){
+        angleMotor.set(TalonFXControlMode.Position, angle);
+    }
+
+    /**
+     * Get state of swerve module
+     * @return speed in m/s and angle in degrees
+     */
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+            Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, 
+            getAngle()
+        );
+    }
+
     /**
      * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
      * @param desiredState a SwerveModuleState representing the desired new state of the module
@@ -104,4 +125,6 @@ public class SwerveModule extends SubsystemBase {
         driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
     }
 
+    
+
 }