robot relative

grt192 · Feb 7, 2024 · 8bb0ec1 · 8bb0ec1
1 parent fb06ca8
commit 8bb0ec1
Show file tree

Hide file tree

Showing 7 changed files with 39 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -28,7 +28,7 @@ public static class ElevatorConstants {
 
     public static final int ZERO_LIMIT_ID = 3;
 
-    public static final double GROUND_POSITION = Units.inchesToMeters(0.5);
+    public static final double GROUND_POSITION = Units.inchesToMeters(1);
     public static final double SPEAKER_POSITION = 0;
     public static final double AMP_POSITION = Units.inchesToMeters(29);
     public static final double CHUTE_POSITION = Units.inchesToMeters(1);
@@ -66,7 +66,7 @@ public static class SwerveConstants {
 
     public static final int BR_DRIVE = 6; 
     public static final int BR_STEER = 7; 
-    public static final double BR_OFFSET = 2.43 - Math.PI * 1 / 4;
+    public static final double BR_OFFSET = 2.66 + Math.PI * 3.0 / 4.0;
 
     private static double MODULE_DIST = Units.inchesToMeters(27.25 / 2.0);
     public static final Translation2d FL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -173,11 +173,17 @@ private void configureBindings() {
         final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem;
 
         ledSubsystem.setDefaultCommand(new RunCommand(() -> {
-          ledSubsystem.setDriverHeading(-swerveSubsystem.getDriverHeading().getRadians());// - swerveSubsystem.getRobotPosition().getRotation().getRadians());
+
+            ledSubsystem.setDriverHeading(driveController.getRelativeMode() ? 0 : -swerveSubsystem.getDriverHeading().getRadians());
+
         }, ledSubsystem));
 
         swerveSubsystem.setDefaultCommand(new RunCommand(() -> {
-            swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()));
+            if(driveController.getRelativeMode()){
+                swerveSubsystem.setRobotRelativeDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());
+            } else {
+                swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());
+            }
             // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition());
         }, swerveSubsystem));
 

diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java
@@ -26,4 +26,6 @@ public abstract class BaseDriveController {
     public abstract JoystickButton getLeftBumper();
 
     public abstract JoystickButton getRightBumper();
+
+    public abstract Boolean getRelativeMode();
 }
diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java
@@ -76,5 +76,8 @@ public JoystickButton getRightBumper(){
     public JoystickButton getLeftBumper(){
         return rightMiddleLeftButton;
     }
-
+
+    public Boolean getRelativeMode(){
+        return rightTrigger.getAsBoolean();
+    }
 }
diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java
@@ -1,5 +1,6 @@
 package frc.robot.controllers;
 
+import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
@@ -46,4 +47,8 @@ public JoystickButton getLeftBumper() {
     public JoystickButton getRightBumper() {
         return driveRBumper;
     }
+
+    public Boolean getRelativeMode() {
+        return driveController.getRightTriggerAxis() > .1;
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -35,7 +35,7 @@ public class ElevatorSubsystem extends SubsystemBase{
     private double manualPower = 0;
 
     private ElevatorState state = ElevatorState.GROUND;
-    private ElevatorState targetState = ElevatorState.START;
+    private ElevatorState targetState = ElevatorState.GROUND;
 
     private final CANSparkMax extensionMotor;
     private RelativeEncoder extensionEncoder;

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -160,13 +160,28 @@ public void setDrivePowers(double xPower, double yPower, double angularPower){
             getDriverHeading()
         );
 
+        states = kinematics.toSwerveModuleStates(speeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            states, speeds,
+            MAX_VEL, MAX_VEL, MAX_OMEGA);
+    }
+
+    public void setRobotRelativeDrivePowers(double xPower, double yPower, double angularPower){
+        ChassisSpeeds speeds = ChassisSpeeds.fromRobotRelativeSpeeds(
+            xPower * MAX_VEL, 
+            yPower * MAX_VEL, 
+            angularPower * MAX_OMEGA, 
+            new Rotation2d(0)
+        );
 
-        this.states = kinematics.toSwerveModuleStates(speeds);
+        states = kinematics.toSwerveModuleStates(speeds);
         SwerveDriveKinematics.desaturateWheelSpeeds(
-            this.states, speeds,
+            states, speeds,
             MAX_VEL, MAX_VEL, MAX_OMEGA);
     }
 
+
+
     public void setSwerveModuleStates(SwerveModuleState[] states){
         this.states = states;
     }