Initial Commit

src/main/java/first/Main.java

@@ -0,0 +1,25 @@
+// 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 first;
+
+import org.wpilib.framework.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(first.robot.Robot::new);
+  }
+}

src/main/java/first/robot/Robot.java

@@ -0,0 +1,41 @@
+// 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 first.robot;
+
+import org.wpilib.command3.Scheduler;
+import org.wpilib.command3.button.CommandGamepad;
+import org.wpilib.epilogue.Epilogue;
+import org.wpilib.epilogue.Logged;
+import org.wpilib.framework.OpModeRobot;
+
+import first.robot.constants.OIConstants;
+import first.robot.mechanisms.Drivetrain;
+import first.robot.mechanisms.Pusher;
+import first.robot.mechanisms.Shooter;
+
+@Logged
+public class Robot extends OpModeRobot {
+  public final Drivetrain drive;
+  public final Pusher pusher;
+  public final Shooter shooter;
+  public final CommandGamepad gamepad;
+  
+  public Robot() {
+    drive = new Drivetrain(true);
+    pusher = new Pusher();
+    shooter = new Shooter();
+    gamepad = new CommandGamepad(OIConstants.kDriverUSB);
+
+    drive.setDefaultCommand(drive.idle());
+    pusher.setDefaultCommand(pusher.idle());
+    shooter.setDefaultCommand(shooter.idle());
+  }
+
+  @Override
+  public void robotPeriodic() {
+    Scheduler.getDefault().run();
+    Epilogue.update(this);
+  }
+}

src/main/java/first/robot/constants/DrivetrainConstants.java

@@ -0,0 +1,13 @@
+package first.robot.constants;
+
+public class DrivetrainConstants {
+    public static final int kFrontLeftPWM = 0;
+    public static final int kRearLeftPWM = 1;
+    public static final int kFrontRightPWM = 2;
+    public static final int kRearRightPWM = 3;
+
+    public static final double kSlowModeMultiplier = .75;
+
+    public static final boolean kInvertForwardBackward = true;
+    public static final boolean kInvertLeftRight = true;
+}

src/main/java/first/robot/constants/OIConstants.java

@@ -0,0 +1,5 @@
+package first.robot.constants;
+
+public class OIConstants {
+    public static final int kDriverUSB = 0;
+}

src/main/java/first/robot/constants/PusherConstants.java

@@ -0,0 +1,12 @@
+package first.robot.constants;
+
+import org.wpilib.hardware.bus.SerialPort;
+
+public class PusherConstants {
+    public static final int kBaudRate = 9600;
+
+    public static final SerialPort.Port kPort = SerialPort.Port.USB;
+
+    public static final String kStopString = "s0~";
+    public static final String kSpinString = "s-250~";
+}

src/main/java/first/robot/constants/ShooterConstants.java

@@ -0,0 +1,9 @@
+package first.robot.constants;
+
+import org.wpilib.hardware.bus.CANPort;
+
+public class ShooterConstants {
+    public static final CANPort kShooterCANPort = CANPort.CAN_S0;
+    public static final int kFrontShooterCAN = 1;
+    public static final int kRearShooterCAN = 2;
+}

src/main/java/first/robot/mechanisms/Drivetrain.java

@@ -0,0 +1,73 @@
+package first.robot.mechanisms;
+
+import org.wpilib.command3.Command;
+import org.wpilib.command3.Mechanism;
+import org.wpilib.command3.button.CommandGamepad;
+import org.wpilib.drive.DifferentialDrive;
+import org.wpilib.drivers.motor.VictorSP;
+
+import first.robot.constants.DrivetrainConstants;
+
+public class Drivetrain implements Mechanism {
+    private VictorSP frontLeft;
+    private VictorSP rearLeft;
+    private VictorSP frontRight;
+    private VictorSP rearRight;
+
+    private DifferentialDrive drive;
+
+    private boolean slowMode;
+
+    public Drivetrain(boolean slowModeAtStartup) {
+        frontLeft = new VictorSP(DrivetrainConstants.kFrontLeftPWM);
+        rearLeft = new VictorSP(DrivetrainConstants.kRearLeftPWM);
+        frontRight = new VictorSP(DrivetrainConstants.kFrontRightPWM);
+        rearRight = new VictorSP(DrivetrainConstants.kRearRightPWM);
+
+        frontLeft.addFollower(rearLeft);
+        frontRight.addFollower(rearRight);
+        frontRight.setInverted(true);
+
+        drive = new DifferentialDrive(frontLeft, frontRight);
+
+        slowMode = slowModeAtStartup;
+    }
+
+    public boolean isSlowMode() {
+        return slowMode;
+    }
+
+    public Command toggleSlowMode() {
+        return run((coroutine) -> {
+            slowMode = !slowMode;
+        })
+        .named("Drivetrain.ToggleSlowMode");
+    }
+
+    public Command tankDrive(CommandGamepad gamepad) {
+        return runRepeatedly(() -> {
+            drive.tankDrive(
+                (DrivetrainConstants.kInvertForwardBackward ? -1 : 1) * gamepad.getLeftY() * (slowMode ? DrivetrainConstants.kSlowModeMultiplier : 1), 
+                (DrivetrainConstants.kInvertLeftRight ? -1 : 1) * gamepad.getRightY() * (slowMode ? DrivetrainConstants.kSlowModeMultiplier : 1)
+            );
+        })
+        .named("Drivetrain.TankDrive");
+    }
+
+    public Command arcadeDrive(CommandGamepad gamepad) {
+        return runRepeatedly(() -> {
+            drive.arcadeDrive(
+                (DrivetrainConstants.kInvertForwardBackward ? -1 : 1) * gamepad.getLeftY() * (slowMode ? DrivetrainConstants.kSlowModeMultiplier : 1), 
+                (DrivetrainConstants.kInvertLeftRight ? -1 : 1) * gamepad.getLeftX() * (slowMode ? DrivetrainConstants.kSlowModeMultiplier : 1)
+            );
+        })
+        .named("Drivetrain.ArcadeDrive");
+    }
+
+    public Command stop() {
+        return runRepeatedly(() -> {
+            drive.tankDrive(0, 0);
+        })
+        .named("Drivetrain.Stop");
+    }
+}

src/main/java/first/robot/mechanisms/Pusher.java

@@ -0,0 +1,37 @@
+package first.robot.mechanisms;
+
+import org.wpilib.command3.Command;
+import org.wpilib.command3.Mechanism;
+import org.wpilib.hardware.bus.SerialPort;
+
+import first.robot.constants.PusherConstants;
+
+public class Pusher implements Mechanism {
+    private SerialPort pusher;
+
+    public Pusher() {
+        pusher = new SerialPort(
+            PusherConstants.kBaudRate, 
+            PusherConstants.kPort
+        );
+
+        pusher.writeString(PusherConstants.kStopString);
+        pusher.flush();
+    }
+
+    public Command spin() {
+        return run((coroutine) -> {
+            pusher.writeString(PusherConstants.kSpinString);
+            pusher.flush();
+        })
+        .named("Pusher.Spin");
+    }
+
+    public Command stop() {
+        return run((coroutine) -> {
+            pusher.writeString(PusherConstants.kStopString);
+            pusher.flush();
+        })
+        .named("Pusher.Stop");
+    }
+}

src/main/java/first/robot/mechanisms/Shooter.java

@@ -0,0 +1,46 @@
+package first.robot.mechanisms;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command3.Command;
+import org.wpilib.command3.Mechanism;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import first.robot.constants.ShooterConstants;
+
+public class Shooter implements Mechanism {
+    private SparkMax frontShooter;
+    private SparkMax rearShooter;
+
+    public Shooter() {
+        frontShooter = new SparkMax(
+            ShooterConstants.kShooterCANPort, 
+            ShooterConstants.kFrontShooterCAN, 
+            MotorType.kBrushless
+        );
+
+        rearShooter = new SparkMax(
+            ShooterConstants.kShooterCANPort, 
+            ShooterConstants.kRearShooterCAN, 
+            MotorType.kBrushless
+        );
+    }
+
+    public Command setSpeed(DoubleSupplier speed) {
+        return runRepeatedly(() -> {
+            frontShooter.setThrottle(speed.getAsDouble());
+            rearShooter.setThrottle(speed.getAsDouble());
+        })
+        .named("Shooter.SetSpeed");
+    }
+
+    public Command stop() {
+        return runRepeatedly(() -> {
+            frontShooter.setThrottle(0);
+            rearShooter.setThrottle(0);
+        })
+        .named("Shooter.Stop");
+    }
+}

src/main/java/first/robot/opmodes/auto/DoNothingAuto.java

@@ -0,0 +1,11 @@
+package first.robot.opmodes.auto;
+
+import org.wpilib.opmode.Autonomous;
+import org.wpilib.opmode.OpMode;
+
+@Autonomous
+public class DoNothingAuto implements OpMode{
+    public DoNothingAuto() {
+        
+    }
+}

src/main/java/first/robot/opmodes/teleop/DefaultTeleop.java

@@ -0,0 +1,26 @@
+package first.robot.opmodes.teleop;
+
+import org.wpilib.opmode.OpMode;
+import org.wpilib.opmode.Teleop;
+
+import first.robot.Robot;
+
+@Teleop
+public class DefaultTeleop implements OpMode {
+    public DefaultTeleop(Robot robot) {
+        robot.drive.setDefaultCommand(
+            robot.drive.arcadeDrive(robot.gamepad)
+        );
+
+        robot.gamepad.start().onTrue(
+            robot.drive.toggleSlowMode()
+        );
+
+        robot.shooter.setDefaultCommand(
+            robot.shooter.setSpeed(robot.gamepad::getRightTrigger)
+        );
+
+        robot.gamepad.faceDown().onTrue(robot.pusher.spin());
+        robot.gamepad.faceDown().onFalse(robot.pusher.stop());
+    }
+}

src/main/java/first/robot/opmodes/teleop/OneHandedTeleop.java

@@ -0,0 +1,26 @@
+package first.robot.opmodes.teleop;
+
+import org.wpilib.opmode.OpMode;
+import org.wpilib.opmode.Teleop;
+
+import first.robot.Robot;
+
+@Teleop
+public class OneHandedTeleop implements OpMode {
+    public OneHandedTeleop(Robot robot) {
+        robot.drive.setDefaultCommand(
+            robot.drive.arcadeDrive(robot.gamepad)
+        );
+
+        robot.gamepad.start().onTrue(
+            robot.drive.toggleSlowMode()
+        );
+
+        robot.shooter.setDefaultCommand(
+            robot.shooter.setSpeed(robot.gamepad::getLeftTrigger)
+        );
+
+        robot.gamepad.leftBumper().onTrue(robot.pusher.spin());
+        robot.gamepad.leftBumper().onFalse(robot.pusher.stop());
+    }
+}