DRIVING WORKS FIELD RELATIVE NO PROBLEMS

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 = 2.0;
+    public static final double ROTATION_SPEED = -0.7;
 
     public static final class IDs {
       public static final int LEFT_FRONT_WHEEL_ID = 2;
@@ -65,7 +65,7 @@ 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 = 2.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 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
@@ -105,7 +105,7 @@ public final class Constants {
       
     }
 
-    public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value
+    public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
 
     // dimensions
     public static final double WIDTH = 18.5; // TODO: find the actual value
@@ -137,5 +137,10 @@ 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; 
   }
 }

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

@@ -10,11 +10,12 @@ 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;
 
@@ -35,8 +36,11 @@ public class RobotContainer {
     
 
     /* Controllers */
-    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
+    // private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
-    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_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);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -46,11 +50,9 @@ public class RobotContainer {
 
         /* Default Commands */
 
-        m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, 
+        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() ->
-                                                                () -> getDriverController().getLeftXAxis(), 
+            m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true)
-                                                                () -> getDriverController().getLeftYAxis(), 
+          , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
-                                                                () -> getDriverController().getRightXAxis(),
-                                                                true));
         
         // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
 
@@ -67,20 +69,20 @@ public class RobotContainer {
     private void configureButtonBindings() {
         /* Driver Buttons */
         
-        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
         
-        new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
+        // new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
-            // .onFalse()
+        //     // .onFalse()
 
-        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
             .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
             
-        /* Operator Buttons */
+        // /* Operator Buttons */
-        // interrupt button
+        // // interrupt button
-        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
+        // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand());
+        //     .onTrue(new InstantCommand());
     }
 
     /**
@@ -96,28 +98,36 @@ public class RobotContainer {
     /**
      * Add your docs here.
      */
-    public IHandController getDriverController() {
+    // public IHandController getDriverController() {
-        return m_driverXbox;
+    //     return m_driverXbox;
+    // }
+
+    public DeadbandedXboxController getDeadbandedDriverController() {
+        return this.m_driverXbox;
+    }
+
+    public DeadbandedXboxController getDeadbandedOperatorController() {
+        return this.m_operatorXbox;
     }
 
     /**
      * Add your docs here.
      */
-    public IHandController getOperatorController() {
+    // public IHandController getOperatorController() {
-        return m_operatorXbox;
+    //     return m_operatorXbox;
-    }
+    // }
 
     /**
      * Add your docs here.
      */
-    public Joystick getOperatorJoystick() {
+    // public Joystick getOperatorJoystick() {
-        return m_operatorXbox.getJoyStick();
+    //     return m_operatorXbox.getJoyStick();
-    }
+    // }
 
     /**
      * Add your docs here.
      */
-    public Joystick getDriverJoystick() {
+    // public Joystick getDriverJoystick() {
-        return m_driverXbox.getJoyStick();
+    //     return m_driverXbox.getJoyStick();
-    }
+    // }
 }

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

@@ -7,6 +7,7 @@
 
 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;
@@ -113,6 +114,12 @@ 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,6 +5,7 @@
 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;
@@ -15,7 +16,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 
 	/** Creates a new AutoBalanceTF2. */
 	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
-		super(1.0, 0, 0, 0);
+		super(0.6, 0, 0, 0);
 
 		this.gyro = gyro;
 		this.drive = drive;
@@ -34,7 +35,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.drive(out2, 0, 0, false);
+		drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
 	}
 
 	@Override

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

@@ -1,75 +0,0 @@
-// 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/SwerveDrive.java

@@ -4,7 +4,6 @@
 
 package frc4388.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -41,6 +40,8 @@ public class SwerveDrive extends SubsystemBase {
   private SwerveDriveOdometry odometry;
 
   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) {
@@ -64,26 +65,31 @@ public class SwerveDrive extends SubsystemBase {
     this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
   }
 
-  // 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, Rotation2d.fromDegrees(-gyro.getRotation2d().getDegrees())) 
-    //                 : new ChassisSpeeds(xSpeed, ySpeed, rot)
-    // );
 
-    SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(0.3, 0, 0));
+  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {
 
-    // // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
+      if (rightStick.getNorm() > 0.1) {
-    setModuleStates(states);
+        rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
-    // modules[0].getDriveMotor().set(0.2);
+      }
-    // modules[1].getDriveMotor().set(0.2);
-    // modules[2].getDriveMotor().set(0.2);
-    // modules[3].getDriveMotor().set(0.2);
 
-    // modules[0].getAngleMotor().set(TalonFXControlMode.Position, 1017);
+      double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
-    // modules[1].getAngleMotor().set(TalonFXControlMode.Position, 509);
+
-    // modules[2].getAngleMotor().set(TalonFXControlMode.Position, 683);
+
-    // modules[3].getAngleMotor().set(TalonFXControlMode.Position, 1816);
+      // 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() < 0.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));
+
+      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+    }
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
 
   /**
@@ -91,11 +97,11 @@ public class SwerveDrive extends SubsystemBase {
    * @param desiredStates Array of module states to set.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(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);
+      module.setDesiredState(state, false);
     }
   }
 
@@ -106,6 +112,7 @@ public class SwerveDrive extends SubsystemBase {
   public void resetGyro() {
     gyro.reset();
     setOdometry(getOdometry());
+    rotTarget = new Rotation2d(0);
   }
 
   /**
@@ -167,23 +174,7 @@ public class SwerveDrive extends SubsystemBase {
 
     // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()});
 
-    // SmartDashboard.putNumber("LF CC Angle", modules[0].getAngle().getDegrees());
-    // SmartDashboard.putNumber("RF CC Angle", modules[1].getAngle().getDegrees());
-    // SmartDashboard.putNumber("LB CC Angle", modules[2].getAngle().getDegrees());
-    // SmartDashboard.putNumber("RB CC Angle", modules[3].getAngle().getDegrees());
-    
-    SmartDashboard.putNumber("LF Vel", modules[0].getDriveVel());
-    SmartDashboard.putNumber("RF Vel", modules[1].getDriveVel());
-    SmartDashboard.putNumber("LB Vel", modules[2].getDriveVel());
-    SmartDashboard.putNumber("RB Vel", modules[3].getDriveVel());
-
     SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
-
-    SmartDashboard.putNumber("LF Pos", modules[0].getDrivePos());
-    SmartDashboard.putNumber("RF Pos", modules[1].getDrivePos());
-    SmartDashboard.putNumber("LB Pos", modules[2].getDrivePos());
-    SmartDashboard.putNumber("RB Pos", modules[3].getDrivePos());
-    
   }
 
   /**

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

@@ -126,7 +126,7 @@ 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) {
+    public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
         Rotation2d currentRotation = this.getAngle();
 
         SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
@@ -139,7 +139,10 @@ public class SwerveModule extends SubsystemBase {
 
         // convert the CANCoder from its position reading to ticks
         double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient
-        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+
+        if (!ignoreAngle) {
+            angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+        }
 
         double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
         // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12;

src/main/java/frc4388/utility/controller/DeadbandedXboxController.java

@@ -0,0 +1,27 @@
+package frc4388.utility.controller;
+
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.XboxController;
+import frc4388.robot.Constants.OIConstants;
+
+public class DeadbandedXboxController extends XboxController {
+  public DeadbandedXboxController(int port) { super(port); }
+  
+  @Override public double getLeftX() { return getLeft().getX(); }
+  @Override public double getLeftY() { return getLeft().getY(); }
+  @Override public double getRightX() { return getRight().getX(); }
+  @Override public double getRightY() { return getRight().getY(); }
+
+  public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
+  public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
+
+  public static Translation2d skewToDeadzonedCircle(double x, double y) {
+    Translation2d translation2d = new Translation2d(x, y);
+    double magnitude = translation2d.getNorm();
+    if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
+    if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
+    return translation2d;
+  }
+}