Changes for INWLA (#21)

Author: rzblue

build.gradle

@@ -80,6 +80,9 @@ dependencies {
 
   implementation "com.github.Oblarg:Oblog:5.1.0"
 
+  implementation 'org.tinylog:tinylog-api:2.6.1'
+  implementation 'org.tinylog:tinylog-impl:2.6.1'
+
 }
 
 test {

simgui.json

@@ -62,7 +62,7 @@
               "autoFit": true
             }
           ],
-          "height": 521,
+          "height": 424,
           "series": [
             {
               "color": [

src/main/java/org/pikerobodevils/frc2023/Constants.java

@@ -115,7 +115,7 @@ public static class IntakeConstants {
 
     public static final double HOLD_CUBE_SPEED = -0.05;
 
-    public static final double SHOOT_CUBE_SPEED = .75;
+    public static final double DEFAULT_OUTTAKE = .75;
     public static final double INTAKE_STALL_DETECTION = 15; // Amps
   }
 

src/main/java/org/pikerobodevils/frc2023/LogConfig.java

@@ -0,0 +1,42 @@
+/* Copyright 2023 Pike RoboDevils, FRC Team 1018
+ * Use of this source code is governed by an MIT-style
+ * license that can be found in the LICENSE.md file or
+ * at https://opensource.org/licenses/MIT. */
+
+package org.pikerobodevils.frc2023;
+
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.tinylog.Logger;
+import org.tinylog.configuration.Configuration;
+
+public class LogConfig {
+  private static final String format =
+      "{date:yyyy-MM-dd HH:mm:ss} - [{tag: none}] - {level}: {message}";
+
+  private static final String consoleLevel = "debug";
+
+  private static final String dataLogLevel = "trace";
+
+  private static final String dataLogEntryKey = "messages";
+
+  public static void config() {
+    Configuration.set("writer1", "org.pikerobodevils.lib.logging.DataLogWriter");
+    Configuration.set("writer1.level", dataLogLevel);
+    Configuration.set("writer1.format", format);
+
+    Configuration.set("writer2", "org.pikerobodevils.lib.logging.DsConsoleWriter");
+    Configuration.set("writer2.level", consoleLevel);
+    Configuration.set("writer2.format", format);
+    Configuration.set("writer2.entryKey", dataLogEntryKey);
+
+    CommandScheduler.getInstance()
+        .onCommandFinish(
+            (cmd) -> Logger.tag("Scheduler").trace("Finish Command: {}", cmd.getName()));
+    CommandScheduler.getInstance()
+        .onCommandInitialize(
+            (cmd) -> Logger.tag("Scheduler").trace("Initialize Command: {}", cmd.getName()));
+    CommandScheduler.getInstance()
+        .onCommandInterrupt(
+            (cmd) -> Logger.tag("Scheduler").trace("Interrupted Command: {}", cmd.getName()));
+  }
+}

src/main/java/org/pikerobodevils/frc2023/Robot.java

@@ -11,7 +11,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import io.github.oblarg.oblog.Logger;
-import org.pikerobodevils.lib.DataLogUtils;
+import org.pikerobodevils.lib.LogUtils;
+import org.pikerobodevils.lib.vendor.SparkMax;
 
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
@@ -26,16 +27,18 @@ public void robotInit() {
       DataLogManager.start(Constants.SIM_LOG_DIR);
     }
     DriverStation.startDataLog(DataLogManager.getLog());
+    LogConfig.config();
 
     SmartDashboard.putData(CommandScheduler.getInstance());
-    m_robotContainer = new RobotContainer();
 
     if (isSimulation()) {
       DriverStation.silenceJoystickConnectionWarning(true);
     }
 
-    DataLogUtils.logManifestMetadata(this);
-    DataLogUtils.logSoftwareVersionMetadata();
+    LogUtils.logSoftwareVersionMetadata();
+    LogUtils.logManifestMetadata(this);
+    m_robotContainer = new RobotContainer();
+    SparkMax.burnFlashInSync();
   }
 
   @Override

src/main/java/org/pikerobodevils/frc2023/RobotContainer.java

@@ -15,7 +15,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import io.github.oblarg.oblog.Logger;
 import org.pikerobodevils.frc2023.commands.Autos;
-import org.pikerobodevils.frc2023.commands.Superstructure;
 import org.pikerobodevils.frc2023.simulation.ArmSim;
 import org.pikerobodevils.frc2023.subsystems.*;
 
@@ -56,10 +55,15 @@ public RobotContainer() {
 
     autoChooser.setDefaultOption("No auto", Commands.none());
     autoChooser.addOption("Drive Back", autos.driveBackAuto());
-    autoChooser.addOption("Mid cube drive back", autos.scoreMidCubeDriveBack());
+    autoChooser.addOption("Score low cube only", autos.scoreLowCube());
     autoChooser.addOption("Score mid cube only", autos.scoreMidCube());
-    autoChooser.addOption("Score high cube", autos.scoreHighCube());
-    autoChooser.addOption("Score high cube then drive", autos.scoreHighCubeDriveBack());
+    autoChooser.addOption("Score high cube only", autos.scoreHighCube());
+    autoChooser.addOption("Low cube drive back", autos.scoreLowCubeDriveBack());
+    autoChooser.addOption("Mid cube drive back", autos.scoreMidCubeDriveBack());
+    autoChooser.addOption("High cube drive back", autos.scoreHighCubeDriveBack());
+    autoChooser.addOption("Low cube balance", autos.scoreLowThenBalance());
+    autoChooser.addOption("Mid cube balance", autos.scoreMidThenBalance());
+    autoChooser.addOption("High cube balance", autos.scoreHighThenBalance());
     autoChooser.addOption("Auto Balance Forwards", autos.autoBalanceForwards());
     autoChooser.addOption("Auto Balance Backwards", autos.autoBalanceBackwards());
 
@@ -70,13 +74,11 @@ private void configureBindings() {
     controlboard
         .operator
         .x()
-        .onTrue(
-            Commands.runOnce(() -> superstructure.setCurrentState(Superstructure.GamePiece.Cube)));
+        .onTrue(Commands.runOnce(() -> superstructure.setGamePiece(Superstructure.GamePiece.Cube)));
     controlboard
         .operator
         .y()
-        .onTrue(
-            Commands.runOnce(() -> superstructure.setCurrentState(Superstructure.GamePiece.Cone)));
+        .onTrue(Commands.runOnce(() -> superstructure.setGamePiece(Superstructure.GamePiece.Cone)));
 
     controlboard.operator.leftTrigger().whileTrue(superstructure.runIntake());
 
@@ -87,6 +89,10 @@ private void configureBindings() {
         .operator
         .axisGreaterThan(XboxController.Axis.kLeftY.value, .5)
         .onTrue(superstructure.floorPickupCube());
+    controlboard
+        .operator
+        .axisLessThan(XboxController.Axis.kLeftY.value, -.5)
+        .onTrue(superstructure.setStateCommand(Superstructure.SuperstructureState.CUBE_SHOOT));
 
     controlboard.operator.a().onTrue(superstructure.stowCommand());
 

src/main/java/org/pikerobodevils/frc2023/commands/Autos.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.Commands;
 import org.pikerobodevils.frc2023.subsystems.Drivetrain;
+import org.pikerobodevils.frc2023.subsystems.Superstructure;
 
 public final class Autos {
   Drivetrain drivetrain;
@@ -20,11 +21,21 @@ public Autos(Drivetrain drivetrain, Superstructure superstructure) {
   }
 
   public CommandBase driveBackAuto() {
-    return drivetrain.setLeftRightVoltageCommand(-3, -3).withTimeout(3.5);
+    return drivetrain.setLeftRightVoltageCommand(-3, -3).withTimeout(3.6);
   }
 
-  public CommandBase scoreMidCubeDriveBack() {
-    return scoreMidCube().andThen(driveBackAuto());
+  public CommandBase scoreLowCube() {
+    return superstructure
+        .scoreLowPosition()
+        .andThen(superstructure.score())
+        .andThen(superstructure.stowCommand());
+  }
+
+  public CommandBase scoreMidCube() {
+    return superstructure
+        .scoreMidPosition()
+        .andThen(superstructure.score())
+        .andThen(superstructure.stowCommand());
   }
 
   public CommandBase scoreHighCube() {
@@ -36,15 +47,28 @@ public CommandBase scoreHighCube() {
         .andThen(superstructure.stowCommand());
   }
 
+  public CommandBase scoreLowCubeDriveBack() {
+    return scoreLowCube().andThen(driveBackAuto());
+  }
+
+  public CommandBase scoreMidCubeDriveBack() {
+    return scoreMidCube().andThen(driveBackAuto());
+  }
+
   public CommandBase scoreHighCubeDriveBack() {
     return scoreHighCube().andThen(driveBackAuto());
   }
 
-  public CommandBase scoreMidCube() {
-    return superstructure
-        .scoreMidPosition()
-        .andThen(superstructure.score())
-        .andThen(superstructure.stowCommand());
+  public CommandBase scoreLowThenBalance() {
+    return scoreLowCube().andThen(autoBalanceBackwards());
+  }
+
+  public CommandBase scoreMidThenBalance() {
+    return scoreMidCube().andThen(autoBalanceBackwards());
+  }
+
+  public CommandBase scoreHighThenBalance() {
+    return scoreHighCube().andThen(autoBalanceBackwards());
   }
 
   // Pitch: + --> backwards
@@ -66,7 +90,7 @@ public CommandBase autoBalanceBackwards() {
                 .withTimeout(1)
                 .andThen(
                     drivetrain
-                        .setLeftRightVoltageCommand(-.75, -.75)
+                        .setLeftRightVoltageCommand(-1, -1)
                         .until(
                             () -> {
                               System.out.println(drivetrain.getPitchRate());