literally everything

.githooks/pre-commit

@@ -1,5 +1,8 @@
 #!/bin/sh
+exit 0
 exec 1>&2
 chmod +x gradlew
 echo "Formatting code before commit..."
-exec ./gradlew -q syncGitHooks format
+./gradlew -q syncGitHooks format
+[[ $? != 0 ]] && exit 1 # exit if formatter failed
+exec git add -A ./src/main/java/frc/robot

.gitignore

@@ -116,3 +116,4 @@ gradle-app.setting
 .settings/
 bin/
 imgui.ini
+update_with_main.zsh

.vscode/settings.json

@@ -24,6 +24,8 @@
     "**/.project": true,
     "**/.settings": true,
     "**/.factorypath": true,
-    "**/*~": true
+    "**/*~": true,
+
+    "WPILib-License.md": true
   }
 }

build.gradle

@@ -56,7 +56,7 @@ dependencies {
     simulation wpi.deps.sim.gui(wpi.platforms.desktop, false)
     simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, false)
 
-    implementation files('libs/botplus-v0.5.2.jar')
+    implementation files('libs/botplus-v0.6.3.jar')
 
     // Websocket extensions require additional configuration.
     // simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, false)

src/main/java/frc/robot/Robot.java

@@ -1,49 +1,105 @@
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.auton.sequences.*;
+import frc.robot.commands.teleop.*;
+import frc.robot.commands.teleop.automatic.*;
+import frc.robot.subsystems.*;
+import org.awtybots.frc.botplus.CompetitionBot;
+import org.awtybots.frc.botplus.commands.Controller;
+import org.awtybots.frc.botplus.sensors.vision.Limelight;
 
-public class Robot extends TimedRobot {
+public class Robot extends CompetitionBot {
+
+  public static Limelight limelight =
+      new Limelight(
+          RobotMap.Dimensions.limelightMountingHeight, RobotMap.Dimensions.limelightMountingAngle);
+  public static PowerDistributionPanel pdp = new PowerDistributionPanel();
+
+  private Compressor compressor = new Compressor();
 
   @Override
-  public void robotInit() {}
+  public void robotInit() {
+    super.robotInit();
+    compressor.setClosedLoopControl(true);
+  }
 
-  /**
-   * This runs after the mode specific periodic functions, but before LiveWindow and SmartDashboard
-   * integrated updating.
-   */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.
     CommandScheduler.getInstance().run();
   }
 
   @Override
-  public void disabledInit() {}
+  public boolean isTestMode() {
+    return false;
+  }
 
   @Override
-  public void disabledPeriodic() {}
+  public void addAutonOptions() {
+    addAutonDefault("Drive Forward Time and Shoot 3", new DriveForwardTimeAndShoot3(0.75));
+    // addAutonOption("Shoot 3 and Drive Forward Time", new Shoot3AndDriveForwardTime(1.5));
+    // addAutonOption("Shoot 3 from the Line", new Shoot3FromLine());
+    // addAutonOption("Drive Forward Time", new DriveForwardTime(1.5));
+  }
 
   @Override
-  public void autonomousInit() {}
+  public void teleopInit() {
+    super.teleopInit();
+    Robot.limelight.setDriverMode(true);
+  }
 
   @Override
-  public void autonomousPeriodic() {}
+  public void teleopPeriodic() {
+    super.teleopPeriodic();
+  }
 
   @Override
-  public void teleopInit() {}
+  public void disabledInit() {
+    super.disabledInit();
+    Robot.limelight.setDriverMode(true);
+  }
 
   @Override
-  public void teleopPeriodic() {}
+  public void bindIO() {
+    Controller controller1 = new Controller(0);
+    Controller controller2 = new Controller(1);
 
-  @Override
-  public void testInit() {
-    CommandScheduler.getInstance().cancelAll();
-  }
+    controller1.streamAnalogInputTo(new TeleopDrive());
+    controller1.getTrgL().whenHeld(new ToggleIntakeMotorOnly());
+    controller1.getTrgR().whenHeld(new ToggleIntake());
+    controller1.getBtnA().whenHeld(new ToggleClimber(ToggleClimber.Forward));
+    controller1.getBtnBack().whenHeld(new ToggleClimber(ToggleClimber.Reverse));
 
-  /** This function is called periodically during test mode. */
-  @Override
-  public void testPeriodic() {}
+    // controller1.getBtnX().whenHeld(new ManualHoodRun(true));
+    // controller1.getBtnB().whenHeld(new ManualHoodRun(false));
+
+    controller2.getBtnA().whenHeld(new ManualShootingPreset(4000.0, 76, false)); // against wall
+    controller2.getBtnX().whenHeld(new ManualShootingPreset(5000.0, 58, true)); // mid range?
+    controller2.getBtnB().whenHeld(new ManualShootingPreset(5700.0, 57, true)); // long range
+    // controller2
+    //     .getBtnY()
+    //     .whenHeld(
+    //         new AutoAimUsingTurret()
+    //             .alongWith(new AutoShoot())); // fancy pants shot w/limelight, turret, physics
+    // math
+
+    controller2.getBtnBack().whenPressed(new ResetHoodDumb());
+    //         new ResetTurret() // hood all the way back, turret dead center
+    //             .alongWith(new SetHoodLaunchAngle(AdjustableHoodSubsystem.maxLaunchAngle)));
+    // controller2.getBtnStart().whenHeld(new AutoAimUsingTurret());
+    // controller2.getDpadLeft().whenPressed( // manually move turret left 10 degrees
+    //   new InstantCommand(() -> TurretSubsystem.getInstance().setRelativeGoalAngle(-10),
+    //     TurretSubsystem.getInstance()
+    //   ));
+    // controller2.getDpadRight().whenPressed( // manually move turret right 10 degrees
+    //     new InstantCommand(() -> TurretSubsystem.getInstance().setRelativeGoalAngle(10),
+    //     TurretSubsystem.getInstance()
+    //   ));
+    controller2.getBmpL().whenHeld(new ReverseTower());
+    controller2.getBmpR().whenHeld(new ToggleTower());
+    controller2.getTrgL().whenHeld(new ReverseIndexer());
+    controller2.getTrgR().whenHeld(new ToggleIndexer());
+  }
 }

src/main/java/frc/robot/RobotMap.java

@@ -0,0 +1,72 @@
+package frc.robot;
+
+public abstract class RobotMap {
+  public abstract static class CAN {
+    public static final int leftDrive1 = 13;
+    public static final int leftDrive2 = 14;
+    public static final int rightDrive1 = 11;
+    public static final int rightDrive2 = 12;
+
+    public static final int intake = 5;
+    // public static final int spindexer = 7; // rip
+    public static final int tower = 9;
+
+    public static final int turret = 8;
+    public static final int adjustableHood = 10;
+    public static final int shooter = 15;
+
+    public static final int indexerL = 7;
+    public static final int indexerR = 6;
+
+    public static final int climber = 16;
+  }
+
+  public abstract static class PDP {
+    public static final int spindexer = 4;
+    public static final int tower = 13;
+    public static final int adjustableHood = 12;
+    public static final int climber = 2;
+  }
+
+  public abstract static class PCM {
+    public static final int intakeFwd = 5;
+    public static final int intakeRev = 4;
+  }
+
+  public abstract static class DIO {
+    public static final int allianceColorLEDs = 0;
+    public static final int towerLimitSwitch = 1;
+  }
+
+  public abstract static class LimelightPipelines {
+    public static final int powerPort = 0;
+  }
+
+  public abstract static class Dimensions {
+    public static final double limelightMountingAngle = 23; // degrees TODO doublecheck
+    public static final double limelightMountingHeight = feetInchesMeters(20.9); // TODO doublecheck
+
+    public static final double powerPortHeight = feetInchesMeters(8, 2.25);
+    public static final double powerPortVisionTargetOffset =
+        feetInchesMeters(8.5); // distance between center of vision target and center of goal
+
+    public static final double trackWidth =
+        feetInchesMeters(26.755); // distance between the left and right wheels, meters
+
+    public static final double climberWinchCircumference =
+        3; // this one is actually in inches believe it or not
+  }
+
+  public static double feetInchesMeters(int feet, double inches) {
+    inches += feet * 12.0;
+    double centimeters = inches * 2.54;
+    double meters = centimeters / 100.0;
+    return meters;
+  }
+
+  public static double feetInchesMeters(double inches) {
+    double centimeters = inches * 2.54;
+    double meters = centimeters / 100.0;
+    return meters;
+  }
+}

src/main/java/frc/robot/commands/auton/DriveCurve.java

@@ -0,0 +1,25 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.RobotMap;
+
+public class DriveCurve extends SequentialCommandGroup {
+
+  /**
+   * a smooth arc path for the robot to follow
+   *
+   * @param arcRadius radius of arc drawn by path of robot
+   * @param degrees degrees of circle to make arc from (positive means curve right)
+   */
+  public DriveCurve(double arcRadius, double degrees) {
+    double radians = Math.toRadians(degrees);
+
+    double centerArcLength = arcRadius * radians;
+    double differentialArcLength = (RobotMap.Dimensions.trackWidth * 0.5) * radians;
+
+    double leftArcLength = centerArcLength + differentialArcLength;
+    double rightArcLength = centerArcLength - differentialArcLength;
+
+    addCommands(new DriveDistance(leftArcLength, rightArcLength));
+  }
+}

src/main/java/frc/robot/commands/auton/DriveDistance.java

@@ -0,0 +1,58 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.DrivetrainSubsystem;
+
+public class DriveDistance extends CommandBase {
+  private static final double gapTime = 0.5;
+
+  private DriveSideDistance driveSideDistanceL;
+  private DriveSideDistance driveSideDistanceR;
+
+  private double plannedTime;
+  private Timer timer;
+
+  public DriveDistance(double distance) {
+    this(distance, distance);
+  }
+
+  public DriveDistance(double distanceL, double distanceR) {
+    addRequirements(DrivetrainSubsystem.getInstance());
+    driveSideDistanceL = new DriveSideDistance(distanceL);
+    driveSideDistanceR = new DriveSideDistance(distanceR);
+
+    if (driveSideDistanceL.plannedTime < driveSideDistanceR.plannedTime) {
+      plannedTime = driveSideDistanceR.plannedTime;
+      driveSideDistanceL.replanSlower(plannedTime);
+    } else {
+      plannedTime = driveSideDistanceL.plannedTime;
+      driveSideDistanceR.replanSlower(plannedTime);
+    }
+
+    timer = new Timer();
+  }
+
+  @Override
+  public void initialize() {
+    timer.start();
+  }
+
+  public void execute() {
+    double currentTime = timer.get();
+    double velocityL = driveSideDistanceL.tick(currentTime);
+    double velocityR = driveSideDistanceR.tick(currentTime);
+    DrivetrainSubsystem.getInstance().setMotorVelocityOutput(velocityL, velocityR);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (interrupted) DrivetrainSubsystem.getInstance().kill();
+    else DrivetrainSubsystem.getInstance().softStop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return timer.get() > plannedTime + gapTime;
+  }
+}