From e1b86b2a5b0fdbcb6baab509850c534b1648ff97 Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Sat, 13 Jan 2024 14:14:43 -0500 Subject: [PATCH 1/2] Basic Drive Interface/Stub --- src/main/java/frc/robot/RobotContainer.java | 27 +++++++++++++++- .../frc/robot/commands/DriveBaseCommand.java | 31 +++++++++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 21 +++++++++++++ .../frc/robot/subsystems/drive/DriveBase.java | 19 ++++++++++++ .../robot/subsystems/drive/DriveSwerve.java | 10 ++++++ .../frc/robot/subsystems/drive/DriveTank.java | 10 ++++++ 6 files changed, 117 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/DriveBaseCommand.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Drive.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveBase.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveSwerve.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveTank.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d09..e705fc03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,40 @@ package frc.robot; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.DriveBaseCommand; +import frc.robot.subsystems.drive.DriveBase; +import frc.robot.subsystems.drive.DriveSwerve; +import frc.robot.subsystems.drive.DriveTank; public class RobotContainer { + public final XboxController controller; + public final DriveBase drivetrain; + public RobotContainer() { + controller = new XboxController(0); + + boolean swerveBot = false; + boolean tankBot = false; + + if (swerveBot) { + drivetrain = new DriveSwerve(); + } else if (tankBot) { + drivetrain = new DriveTank(); + } else { + drivetrain = new DriveBase(); + } + configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + drivetrain.setDefaultCommand( + new DriveBaseCommand( + drivetrain, () -> -controller.getLeftY(), () -> -controller.getLeftX())); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/commands/DriveBaseCommand.java b/src/main/java/frc/robot/commands/DriveBaseCommand.java new file mode 100644 index 00000000..e3ce8333 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveBaseCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveBase; +import java.util.function.DoubleSupplier; + +public class DriveBaseCommand extends Command { + DriveBase drive; + DoubleSupplier speed; + DoubleSupplier rot; + + public DriveBaseCommand(DriveBase drive, DoubleSupplier speed, DoubleSupplier rot) { + this.drive = drive; + this.speed = speed; + this.rot = rot; + + addRequirements(drive); + } + + @Override + public void execute() { + ChassisSpeeds speeds = + new ChassisSpeeds( + speed.getAsDouble() * drive.getMaxLinearSpeed(), + 0, + rot.getAsDouble() * drive.getMaxAngularSpeed()); + + drive.runVelocity(speeds); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java new file mode 100644 index 00000000..b8186d75 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public interface Drive { + public void runVelocity(ChassisSpeeds speeds); + + public double getMaxLinearSpeed(); + + public double getMaxAngularSpeed(); + + /* + public void stop(); + + public Pose2d getPose(); + + public void setPose(Pose2d Pose2d); + + public Rotation2d getRotation(); + */ +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java new file mode 100644 index 00000000..120e9f55 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DriveBase extends SubsystemBase implements Drive { + public DriveBase() {} + + public void runVelocity(ChassisSpeeds speeds) {} + ; + + public double getMaxLinearSpeed() { + return 0; + } + + public double getMaxAngularSpeed() { + return 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerve.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerve.java new file mode 100644 index 00000000..8e275769 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSwerve.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class DriveSwerve extends DriveBase { + @Override + public void runVelocity(ChassisSpeeds speeds) { + /* Swerve Controls Here */ + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveTank.java b/src/main/java/frc/robot/subsystems/drive/DriveTank.java new file mode 100644 index 00000000..9d2fb975 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveTank.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class DriveTank extends DriveBase { + @Override + public void runVelocity(ChassisSpeeds speeds) { + /* Tank Controls Here */ + } +} From 2b7efbacea78ac2dc96613510e85900e21877422 Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Sat, 13 Jan 2024 19:42:07 -0500 Subject: [PATCH 2/2] Added comments. Added DifferentialDriveKinematics. Added AutoLogOutput support --- .../frc/robot/subsystems/drive/Drive.java | 19 ++++++++++++--- .../frc/robot/subsystems/drive/DriveBase.java | 23 +++++++++++++++---- 2 files changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b8186d75..223f70d1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -3,19 +3,32 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; public interface Drive { + /** + * Sets the desired chassis speed + * + * @param speeds specifies the setpoint speed for the chassis + */ public void runVelocity(ChassisSpeeds speeds); + /** + * This returns the robot's max linear speed in meter/sec + * + * @return speed in meter/sec + */ public double getMaxLinearSpeed(); + /** + * This returns the robot's max angular speed in radian/sec + * + * @return speed in radian/sec + */ public double getMaxAngularSpeed(); /* public void stop(); - public Pose2d getPose(); - public void setPose(Pose2d Pose2d); - public Rotation2d getRotation(); */ + } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index 120e9f55..9c735466 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -1,19 +1,34 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; public class DriveBase extends SubsystemBase implements Drive { + protected static final DifferentialDriveKinematics kinematics = + new DifferentialDriveKinematics(Units.inchesToMeters(27.0)); + + @AutoLogOutput protected double leftVelocityMetersPerSecond; + @AutoLogOutput protected double rightVelocityMetersPerSecond; + public DriveBase() {} - public void runVelocity(ChassisSpeeds speeds) {} - ; + public void runVelocity(ChassisSpeeds chassisSpeeds) { + // Convert to wheel speeds + DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + + leftVelocityMetersPerSecond = wheelSpeeds.leftMetersPerSecond; + rightVelocityMetersPerSecond = wheelSpeeds.rightMetersPerSecond; + } public double getMaxLinearSpeed() { - return 0; + return 5; } public double getMaxAngularSpeed() { - return 0; + return 5; } }