Finish kitbot, still needs button bindings

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<Command> 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 ***/

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();        
+    }
+}

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);
+    }
+}

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();
+    }
+}

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);
+    } 
+}

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();
+    } 
+}

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();
+    }
+
+}

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;
+    }
+}

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;
+    }
+}

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());
+
+    }
+
+
+}