From 1e812f706b4192c64970eea4311812fa76b22d42 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 2 Feb 2023 17:55:52 -0700 Subject: [PATCH] drive done --- src/main/java/frc4388/robot/Robot.java | 13 --- .../java/frc4388/robot/RobotContainer.java | 8 ++ .../robot/commands/DriveWithInput.java | 79 +++++++++++++++++++ .../frc4388/robot/subsystems/SwerveDrive.java | 60 +++++++------- 4 files changed, 117 insertions(+), 43 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/DriveWithInput.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a54b4b4..5813025 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -64,10 +64,6 @@ public class Robot extends TimedRobot { } } - private final SlewRateLimiter xLimiter = new SlewRateLimiter(3); - private final SlewRateLimiter yLimiter = new SlewRateLimiter(3); - private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3); - // private MicroBot bot = new MicroBot(); /** @@ -165,15 +161,6 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); // SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); // SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); - this.drive(false); - } - - private void drive(boolean fieldRelative) { - final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis() * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis() * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis() * 0.3, 0.02) * Units.feetToMeters(Math.PI)); - - m_robotContainer.m_robotSwerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 01da8cf..6eaf51f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ 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.DriveWithInput; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -53,6 +54,12 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); + m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, + () -> getDriverController().getLeftXAxis(), + () -> getDriverController().getLeftYAxis(), + () -> getDriverController().getRightXAxis(), + false)); + /* Default Commands */ // drives the robot with a two-axis input from the driver controller // continually sends updates to the Blinkin LED controller to keep the lights on @@ -78,6 +85,7 @@ public class RobotContainer { // 0.3 * getDriverController().getRightYAxis(), // true), // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + } diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java new file mode 100644 index 0000000..784536d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithInput.java @@ -0,0 +1,79 @@ +// 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.DoubleSupplier; +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 edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveWithInput extends CommandBase { + /** Creates a new DriveWithInput. */ + 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; + + + + + 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(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 bcd0741..b1ca512 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -61,43 +61,43 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + // public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - Translation2d speed = new Translation2d(-xSpeed, ySpeed); - double mag = speed.getNorm(); - speed = speed.times(mag * speedAdjust); + // Translation2d speed = new Translation2d(-xSpeed, ySpeed); + // double mag = speed.getNorm(); + // speed = speed.times(mag * speedAdjust); - double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - // SwerveModuleState[] states = kinematics.toSwerveModuleStates( - // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) - // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) - //); + // double xSpeedMetersPerSecond = -speed.getX(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + // double ySpeedMetersPerSecond = speed.getY(); //SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + // // SwerveModuleState[] states = kinematics.toSwerveModuleStates( + // // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) + // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot) + // //); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); + // SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); - setModuleStates(states); - } + // setModuleStates(states); + // } - public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { - // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - Translation2d speed = new Translation2d(-leftX, leftY); - speed = speed.times(speed.getNorm() * speedAdjust); - // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); - // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = -speed.getX(); - double ySpeedMetersPerSecond = speed.getY(); - // chassisSpeeds = fieldRelative - // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + // public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { + // // ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; + // Translation2d speed = new Translation2d(-leftX, leftY); + // speed = speed.times(speed.getNorm() * speedAdjust); + // // if (Math.abs(rightX) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND || Math.abs(rightY) > SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) + // // rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + // // double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + // double xSpeedMetersPerSecond = -speed.getX(); + // double ySpeedMetersPerSecond = speed.getY(); + // // chassisSpeeds = fieldRelative + // // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + // // rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + // // : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + // ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); - SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); - setModuleStates(states); - } + // SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); + // setModuleStates(states); + // } // ! experimental WPILib swerve drive example public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {