From cceadc20bdc8cb59bebadccaaacf2f8c957f6844 Mon Sep 17 00:00:00 2001 From: Richie_Xue Date: Tue, 9 Jan 2024 00:03:03 -0500 Subject: [PATCH] Finish kitbot, still needs button bindings --- .../com/stuypulse/robot/RobotContainer.java | 14 ++- .../commands/drivetrain/DrivetrainDrive.java | 49 ++++++++++ .../drivetrain/DrivetrainDriveForever.java | 27 ++++++ .../robot/commands/launcher/LaunchNote.java | 19 ++++ .../commands/launcher/LaunchPrepare.java | 23 +++++ .../commands/launcher/LauncherIntakeNote.java | 19 ++++ .../robot/commands/launcher/LauncherStop.java | 20 +++++ .../com/stuypulse/robot/constants/Ports.java | 14 ++- .../stuypulse/robot/constants/Settings.java | 20 ++++- .../robot/subsystems/Drivetrain.java | 90 +++++++++++++++++++ .../stuypulse/robot/subsystems/Launcher.java | 78 ++++++++++++++++ 11 files changed, 369 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java create mode 100644 src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java create mode 100644 src/main/java/com/stuypulse/robot/commands/launcher/LaunchNote.java create mode 100644 src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java create mode 100644 src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java create mode 100644 src/main/java/com/stuypulse/robot/commands/launcher/LauncherStop.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/Launcher.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b55349..130eb8b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,7 +6,11 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive; +import com.stuypulse.robot.commands.launcher.LauncherIntakeNote; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.subsystems.Drivetrain; +import com.stuypulse.robot.subsystems.Launcher; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -21,6 +25,8 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Drivetrain drivetrain = Drivetrain.getInstance(); + public final Launcher launcher = Launcher.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -37,13 +43,17 @@ public RobotContainer() { /*** DEFAULTS ***/ /****************/ - private void configureDefaultCommands() {} + private void configureDefaultCommands() { + drivetrain.setDefaultCommand(new DrivetrainDrive(drivetrain, driver)); + } /***************/ /*** BUTTONS ***/ /***************/ - private void configureButtonBindings() {} + private void configureButtonBindings() { + //TODO: Add button bindings + } /**************/ /*** AUTONS ***/ diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java new file mode 100644 index 0000000..8d48723 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java @@ -0,0 +1,49 @@ +package com.stuypulse.robot.commands.drivetrain; + +import com.stuypulse.robot.subsystems.Drivetrain; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.numbers.IStream; + +import edu.wpi.first.wpilibj2.command.Command; + +public class DrivetrainDrive extends Command { + public final Drivetrain drivetrain; + private final Gamepad driver; + + double rightSpeed; + double leftSpeed; + + private final IStream speed; + private final IStream angle; + + public DrivetrainDrive(Drivetrain drivetrain, Gamepad driver) { + this.drivetrain = drivetrain; + this.driver = driver; + + this.speed = IStream.create( + () -> driver.getRightY() - driver.getLeftY()) + .filtered( + x -> SLMath.deadband(x, 0), + x -> SLMath.spow(x, 2) + ); + + this.angle = IStream.create( + () -> driver.getRightX() - driver.getLeftX()) + .filtered( + x -> SLMath.deadband(x, 0), + x -> SLMath.spow(x, 2) + ); + + addRequirements(drivetrain); + + } + + @Override + public void execute() { + if (driver.getRawLeftButton() || driver.getRawRightButton()) { + drivetrain.curvatureDrive(speed.get(), angle.get(), true); + } + else drivetrain.stop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java new file mode 100644 index 0000000..84b334d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java @@ -0,0 +1,27 @@ +/************************ PROJECT DORCAS ************************/ +/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved. */ +/* This work is licensed under the terms of the MIT license. */ +/****************************************************************/ + +package com.stuypulse.robot.commands.drivetrain; + +import com.stuypulse.robot.subsystems.Drivetrain; + +import edu.wpi.first.wpilibj2.command.Command; + +public class DrivetrainDriveForever extends Command { + private final Drivetrain drivetrain; + private final double speed; + + public DrivetrainDriveForever(Drivetrain drivetrain, double speed) { + this.drivetrain = drivetrain; + this.speed = speed; + + addRequirements(drivetrain); + } + + @Override + public void execute() { + drivetrain.curvatureDrive(speed, 0, false); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LaunchNote.java b/src/main/java/com/stuypulse/robot/commands/launcher/LaunchNote.java new file mode 100644 index 0000000..ff0ab13 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LaunchNote.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.launcher; + +import com.stuypulse.robot.subsystems.Launcher; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class LaunchNote extends InstantCommand { + Launcher launcher; + + public LaunchNote() { + launcher = Launcher.getInstance(); + addRequirements(launcher); + } + + @Override + public void initialize() { + launcher.launch(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java b/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java new file mode 100644 index 0000000..789a9f0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LaunchPrepare.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.commands.launcher; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.Launcher; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +/** + * PrepareLaunch sets the launcher to the launch speed, spins just the outside wheel of the launcher to allow it to get up to speed before launching + */ +public class LaunchPrepare extends InstantCommand { + Launcher launcher; + + public LaunchPrepare() { + launcher = Launcher.getInstance(); + addRequirements(launcher); + } + + @Override + public void initialize() { + launcher.setLaunchSpeed(Settings.Launcher.kLauncherSpeed); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java new file mode 100644 index 0000000..a2c25a3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherIntakeNote.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.launcher; + +import com.stuypulse.robot.subsystems.Launcher; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class LauncherIntakeNote extends InstantCommand { + Launcher launcher; + + public LauncherIntakeNote() { + launcher = Launcher.getInstance(); + addRequirements(launcher); + } + + @Override + public void initialize() { + launcher.intake(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/launcher/LauncherStop.java b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherStop.java new file mode 100644 index 0000000..2808254 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/launcher/LauncherStop.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.launcher; + +import com.stuypulse.robot.subsystems.Launcher; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class LauncherStop extends InstantCommand{ + Launcher launcher; + + public LauncherStop() { + launcher = Launcher.getInstance(); + addRequirements(launcher); + } + + @Override + public void initialize() { + launcher.stop(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1a2106d..a93fc97 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,4 +12,16 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } -} + + public interface Drivetrain { + int LEFTREAR = 1; + int LEFTFRONT = 2; + int RIGHTREAR = 3; + int RIGHTFRONT = 4; + } + + public interface Launcher { + int FEEDER = 5; + int LAUNCHER = 6; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 570bbaa..340fa9b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -14,4 +14,22 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -public interface Settings {} +public interface Settings { + public interface Drivetrain { + int kCurrentLimit = 60; + } + + public interface Launcher { + int kLauncherCurrentLimit = 80; + int kFeedCurrentLimit = 80; + + // Speeds for wheels when intaking and launching. Intake speeds are negative to run the wheels + // in reverse + double kLauncherSpeed = 1; + double kLaunchFeederSpeed = 1; + double kIntakeLauncherSpeed = -1; + double kIntakeFeederSpeed = -.2; + + double kLauncherDelay = 1; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java new file mode 100644 index 0000000..aca6d8e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/Drivetrain.java @@ -0,0 +1,90 @@ +package com.stuypulse.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Drivetrain extends SubsystemBase{ + private static Drivetrain instance = new Drivetrain(); + + public static Drivetrain getInstance() { + return instance; + } + + private DifferentialDrive drivetrain; + + CANSparkMax leftFront; + CANSparkMax leftBack; + CANSparkMax rightFront; + CANSparkMax rightBack; + + public Drivetrain() { + leftFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed); + leftBack = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed); + rightFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed); + rightBack = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed); + + leftBack.follow(leftFront); + rightBack.follow(rightFront); + + leftFront.setInverted(true); + rightFront.setInverted(false); + + leftFront.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit); + leftBack.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit); + rightFront.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit); + leftBack.setSmartCurrentLimit(Settings.Drivetrain.kCurrentLimit); + + drivetrain = new DifferentialDrive(leftFront, rightFront); + } + + //********** GETTERS ********** + public double getLeftSpeed() { + return leftFront.getEncoder().getVelocity(); + } + + public double getRightSpeed() { + return rightFront.getEncoder().getVelocity(); + } + + public double getLeftVoltage() { + return leftFront.getAppliedOutput(); + } + + public double getRightVoltage() { + return rightFront.getAppliedOutput(); + } + + //********** Drive Methods ********** + public void tankDrive(double leftSpeed, double rightSpeed) { + drivetrain.tankDrive(leftSpeed, rightSpeed); + } + + public void arcadeDrive(double speed, double rotation) { + drivetrain.arcadeDrive(speed, rotation); + } + + public void curvatureDrive(double speed, double rotation, boolean isQuickTurn) { + drivetrain.curvatureDrive(speed, rotation, isQuickTurn); + } + + public void stop() { + drivetrain.stopMotor(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Drivetrain/ Left Speed", getLeftSpeed()); + SmartDashboard.putNumber("Drivetrain/ Right Speed", getRightSpeed()); + SmartDashboard.putNumber("Drivetrain/ Left Voltage", getLeftVoltage()); + SmartDashboard.putNumber("Drivetrain/ Right Voltage", getRightVoltage()); + + } + + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/Launcher.java b/src/main/java/com/stuypulse/robot/subsystems/Launcher.java new file mode 100644 index 0000000..51ae3a8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/Launcher.java @@ -0,0 +1,78 @@ +package com.stuypulse.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Ports; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Launcher extends SubsystemBase{ + private static Launcher instance = new Launcher(); + + public static Launcher getInstance() { + return instance; + } + + CANSparkMax feeder; + CANSparkMax launcher; + + public Launcher() { + feeder = new CANSparkMax(Ports.Launcher.FEEDER, MotorType.kBrushed); + launcher = new CANSparkMax(Ports.Launcher.LAUNCHER, MotorType.kBrushed); + + feeder.setSmartCurrentLimit(Settings.Launcher.kFeedCurrentLimit); + launcher.setSmartCurrentLimit(Settings.Launcher.kLauncherCurrentLimit); + } + + //********** SETTERS ********** + public void setLaunchSpeed(double speed) { + launcher.set(speed); + } + + public void setFeederSpeed(double speed) { + feeder.set(speed); + } + + public void stop() { + setLaunchSpeed(0); + setFeederSpeed(0); + } + + public void launch() { + launcher.set(Settings.Launcher.kLauncherSpeed); + feeder.set(Settings.Launcher.kLaunchFeederSpeed); + } + + public void intake() { + launcher.set(Settings.Launcher.kIntakeLauncherSpeed); + feeder.set(Settings.Launcher.kIntakeFeederSpeed); + } + + //********** GETTERS ********** + public double getLauncherSpeed() { + return launcher.getEncoder().getVelocity(); + } + + public double getFeederSpeed() { + return feeder.getEncoder().getVelocity(); + } + + public double getLauncherVoltage() { + return launcher.getAppliedOutput(); + } + + public double getFeederVoltage() { + return feeder.getAppliedOutput(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Launcher/ Launcher Speed", getLauncherSpeed()); + SmartDashboard.putNumber("Launcher/ Feeder Speed", getFeederSpeed()); + SmartDashboard.putNumber("Launcher/ Launcher Voltage", getLauncherVoltage()); + SmartDashboard.putNumber("Launcher/ Feeder Voltage", getFeederVoltage()); + + } +} \ No newline at end of file