From 5cfd6f971ae4ce0048b21695b55841a20be0682f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 4 Feb 2023 15:06:42 -0700 Subject: [PATCH] DRIVING WORKS FIELD RELATIVE NO PROBLEMS --- src/main/java/frc4388/robot/Constants.java | 11 ++- .../java/frc4388/robot/RobotContainer.java | 66 +++++++++------- src/main/java/frc4388/robot/RobotMap.java | 7 ++ .../frc4388/robot/commands/AutoBalance.java | 5 +- .../robot/commands/DriveWithInput.java | 75 ------------------- .../frc4388/robot/subsystems/SwerveDrive.java | 63 +++++++--------- .../robot/subsystems/SwerveModule.java | 7 +- .../controller/DeadbandedXboxController.java | 27 +++++++ 8 files changed, 115 insertions(+), 146 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/DriveWithInput.java create mode 100644 src/main/java/frc4388/utility/controller/DeadbandedXboxController.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4b8c518..1b94ed1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5a2d29d..4a03806 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_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 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, - () -> getDriverController().getLeftXAxis(), - () -> getDriverController().getLeftYAxis(), - () -> getDriverController().getRightXAxis(), - true)); + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true) + , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); // 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) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); - // .onFalse() + // new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + // // .onFalse() - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - /* Operator Buttons */ - // interrupt button - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .onTrue(new InstantCommand()); + // /* Operator Buttons */ + // // interrupt button + // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand()); } /** @@ -96,28 +98,36 @@ public class RobotContainer { /** * Add your docs here. */ - public IHandController getDriverController() { - return m_driverXbox; + // public IHandController getDriverController() { + // return m_driverXbox; + // } + + public DeadbandedXboxController getDeadbandedDriverController() { + return this.m_driverXbox; + } + + public DeadbandedXboxController getDeadbandedOperatorController() { + return this.m_operatorXbox; } /** * Add your docs here. */ - public IHandController getOperatorController() { - return m_operatorXbox; - } + // public IHandController getOperatorController() { + // return m_operatorXbox; + // } /** * Add your docs here. */ - public Joystick getOperatorJoystick() { - return m_operatorXbox.getJoyStick(); - } + // public Joystick getOperatorJoystick() { + // return m_operatorXbox.getJoyStick(); + // } /** * Add your docs here. */ - public Joystick getDriverJoystick() { - return m_driverXbox.getJoyStick(); - } + // public Joystick getDriverJoystick() { + // return m_driverXbox.getJoyStick(); + // } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 93a114d..9b5ae04 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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); diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 374de0a..ffccee4 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/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 diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java deleted file mode 100644 index 3b4d638..0000000 --- a/src/main/java/frc4388/robot/commands/DriveWithInput.java +++ /dev/null @@ -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 xSpeed; - private final Supplier ySpeed; - private final Supplier rot; - private final boolean fieldRelative; - - private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; - - /** Creates a new DriveWithInput. */ - public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier 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; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f35a557..38e7415 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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)); - setModuleStates(states); - // modules[0].getDriveMotor().set(0.2); - // modules[1].getDriveMotor().set(0.2); - // modules[2].getDriveMotor().set(0.2); - // modules[3].getDriveMotor().set(0.2); + if (rightStick.getNorm() > 0.1) { + rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); + } - // modules[0].getAngleMotor().set(TalonFXControlMode.Position, 1017); - // modules[1].getAngleMotor().set(TalonFXControlMode.Position, 509); - // modules[2].getAngleMotor().set(TalonFXControlMode.Position, 683); - // modules[3].getAngleMotor().set(TalonFXControlMode.Position, 1816); + double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + + + // 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); } /** @@ -166,24 +173,8 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); // 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()); - } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index efdae40..2616b71 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/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; diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java new file mode 100644 index 0000000..bad1605 --- /dev/null +++ b/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; + } +} \ No newline at end of file