Merge remote-tracking branch 'origin/staging' into ramsete-controller

this will prolly break stuff but I'll fix it

--Gabe

.github/workflows/compile.yml

@@ -20,6 +20,18 @@ jobs:
     steps:
     # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
     - uses: actions/checkout@v3
+    # Download distance sensor library
+    - name: Download distance sensor library
+      run: wget -O distsen.zip https://github.com/REVrobotics/2m-Distance-Sensor/releases/download/v2022.0.4/2m-Distance-Sensor-SDK-2022.0.4.zip
+    # Extract the distance sensor
+    - name: Extract distance sensor library
+      run: unzip distsen.zip
+    # Create /root/wpilib/2023/
+    - name: Create /root/wpilib/2023/ directory
+      run: mkdir -p /root/wpilib/2023/
+    # Copy maven to /root/wpilib/2023/
+    - name: Copy maven to /root/wpilib/2023/
+      run: cp -R 2m-Distance-Sensor-SDK-2022.0.4/maven /root/wpilib/2023/
 
     # Declares the repository safe and not under dubious ownership.
     - name: Add repository to git safe directories

.github/workflows/format.yml

@@ -1,7 +1,8 @@
 name: Format with Google Style
 
 on:
-  push:
+  schedule:
+  - cron: "0 9 * * *" # 9 am UTC is 3 am CST, which is our timezone
 
 permissions:
   contents: write

.vscode/settings.json

@@ -26,5 +26,8 @@
     },
   ],
   "java.test.defaultConfig": "WPIlibUnitTests",
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable",
+  "editor.detectIndentation": false,
+  "editor.tabSize": 2,
+  "editor.insertSpaces": true,
 }

src/main/java/frc/robot/ArbitraryValues.java

@@ -0,0 +1,50 @@
+// 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 frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/** Add your docs here. */
+public class ArbitraryValues {
+
+  /**
+   * arbitrary value for unknown doubles; intended for values that we don't know due to unbuilt bot;
+   * DONT USE FOR ACTUAL BOT
+   *
+   * @return 0.0 if the robot base isn't real; will throw exception if base is real
+   */
+  public static double arbitraryDouble() {
+    if (RobotBase.isReal()) {
+      throw new IllegalStateException("unsafe value for use on bot");
+    }
+    return 0.0;
+  }
+
+  /**
+   * arbitrary value for unknown integer; intended for value that we don't know due to unbuilt bot;
+   * DON'T USE FOR ACTUAL BOT
+   *
+   * @return 0 if the robot base isn't real; will throw exception if base is real
+   */
+  public static int arbitraryInteger() {
+    if (RobotBase.isReal()) {
+      throw new IllegalStateException("unsafe value for use on bot");
+    }
+    return 0;
+  }
+
+  /**
+   * arbitrary value for unknown boolean; intended for value that we don't know due to unbuilt bot;
+   * DON'T USE FOR ACTUAL BOT
+   *
+   * @return false if the robot base isn't real; will throw exception if base is real
+   */
+  public static boolean arbitraryBoolean() {
+    if (RobotBase.isReal()) {
+      throw new IllegalStateException("unsafe value for use on bot");
+    }
+    return false;
+  }
+}

src/main/java/frc/robot/Constants.java

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import frc.robot.positioning.Pose;
 import edu.wpi.first.math.util.Units;
 
 /**
@@ -18,6 +19,16 @@
 public final class Constants {
   public static class OperatorConstants {
     public static final int CONTROLLER_NUMBER = 0;
+    public static final int JOYSTICK_NUMBER = 1;
+  }
+
+  public static class RobotDimensions {
+    /** The pose of the robot's center */
+    public static final Pose CENTER_POSE = new Pose(0, 0, 8.5);
+    /** The front/back length of the robot's frame in inches */
+    public static final double FRAME_LENGTH = 29.0;
+    /** The left/right width of the robot's frame in inches */
+    public static final double FRAME_WIDTH = 29.0;
   }
 
   /** General constants for the drivetrain. Primarily used by {@link frc.robot.subsystems.Drive}. */
@@ -96,10 +107,178 @@ public static class ContollerButtons {
   /** General constants for the drivetrain. Primarily used by {@link frc.robot.subsystems.Drive}. */
   public static class DriveConstants {
     /* CAN IDs for the drivetrain motor controllers */
-    public static final int ID_TALON_FRONT_LEFT = 0;
+    public static final int ID_TALON_FRONT_LEFT = 3;
     public static final int ID_TALON_FRONT_RIGHT = 1;
-    public static final int ID_TALON_REAR_LEFT = 2;
-    public static final int ID_TALON_REAR_RIGHT = 3;
+    public static final int ID_TALON_REAR_LEFT = 4;
+    public static final int ID_TALON_REAR_RIGHT = 2;
+  }
+
+  /* TODO: reorganize this class after Cow Town, make separate classes for each individual
+   * sub-subsystem
+   */
+  public static class Arm {
+    /** The pose of the arm's base in robot oriented coordinates */
+    public static final Pose BASE_POSE = new Pose(0, 0, 8.5);
+    /** The distance between the floor and the arm base in inches */
+    public static final double BASE_DISTANCE_TO_FLOOR = 8.5;
+    /** Length in inches from the base of the arm to the shoulder joint */
+    public static final double BASE_TO_SHOULDER_LENGTH = 19;
+    /**
+     * How high above the arm base should we keep the end effector to stop the arm from stabbing the
+     * robot? This only matters inside the frame perimeter. Distance in inches.
+     */
+    public static final double KEEP_OUT_HEIGHT = 6;
+
+    public static final int ID_TALON_ARM_SHOULDER = 6;
+    public static final int ID_TALON_ARM_WINCH = 2; /* this is actually a Talon, not a Talon SRX */
+    public static final int ID_TALON_ARM_CLAW = 0; /* This is actually a Spark */
+    public static final int ID_TALON_ARM_BASE = 5;
+
+    public static final int CHANNEL_ANALOG_PRESSURE_SENSOR = 0;
+
+    public static final double SETPOINT_PRESSURE_CONE = 5.7;
+    public static final double SETPOINT_PRESSURE_CUBE = 5.5;
+
+    public static final boolean INVERTED_TALON_SENSOR_ARM_SHOULDER = true;
+    public static final boolean INVERTED_TALON_SENSOR_ARM_BASE = true;
+
+    public static final boolean INVERTED_TALON_ARM_SHOULDER = false;
+    public static final boolean INVERTED_TALON_ARM_BASE = false;
+
+    public static final boolean AUTO_ZERO_REVERSE_LIMIT_SHOULDER = true;
+    public static final boolean AUTO_ZERO_REVERSE_LIMIT_BASE = true;
+
+    public static final double MIN_LENGTH_ARM_EXTENDER = 30.5;
+
+    public static final double EXTRA_LENGTH_ARM_EXTENDER = 26;
+
+    public static final double DONE_THRESHOLD_ARM_EXTENSION = 1.0;
+    /* DONE_THRESHOLD_ARM_CLAW is not tuned yet - update it when implementing pressure sensor */
+    public static final double DONE_THRESHOLD_ARM_CLAW = 0.1;
+
+    public static final double CONE_PRESSURE_THRESHOLD = 4.0;
+
+    public static final double SPEED_WINCH_ARM_EXTENSION = 1.0;
+
+    public static final double SPEED_ARM_CLAW = 1;
+
+    public static final double SPEED_ARM_SHOULDER_HOMING = 0.5;
+    public static final double SPEED_ARM_BASE_HOMING = 0.5;
+
+    public static class ShoulderPID {
+      public static final double kP = 1;
+      public static final double kI = 0.002;
+      public static final double kD = 100;
+      public static final double kFF = 0.0;
+      public static final double kIZone = 800;
+      public static final double ACCEPTABLE_ERROR = 50;
+    }
+
+    public static class BasePID {
+      public static final double kP = 1;
+      public static final double kI = 0.001;
+      public static final double kD = 75;
+      public static final double kFF = 0.0;
+      public static final double kIZone = 150;
+      public static final double ACCEPTABLE_ERROR = 300;
+    }
+
+    public static class Poses {
+      public static final Pose RESET = new Pose(0, 0, 44);
+      public static final Pose SCORE_LOWER = new Pose(0, 23, -6);
+      public static final Pose SCORE_MID_CONE = new Pose(0, 38, 32);
+      public static final Pose SCORE_MID_CUBE = new Pose(0, 38, 18);
+      public static final Pose SCORE_HIGH_CONE = new Pose(0, 55, 44);
+      public static final Pose SCORE_HIGH_CUBE = new Pose(0, 55, 28);
+      public static final Pose PICKUP = new Pose(-1, 32, -8);
+      public static final Pose HUMAN_SHELF = new Pose(0, 27, 32);
+      public static final Pose CARRYING = new Pose(0, 0, 44);
+    }
+  }
+
+  public static class I2C {
+    public static int DISTANCE_SENSOR_MULTIPLEXER_PORT = 0;
+    public static int COLOR_SENSOR_MULTIPLEXER_PORT = 1;
+  }
+
+  public static class Limits {
+    /** Maximum horizontal extension over the frame in inches */
+    public static final double MAX_FRAME_EXTENSION = 48.0;
+    /** Maximum extended robot height in inches */
+    public static final double MAX_EXTENDED_HEIGHT = 78.0;
+    /** Minimum distance from overextending we want to keep in inches */
+    public static final double OVEREXTENSION_DANGER_DISTANCE = 4.0;
+  }
+
+  /** Holds constants for things not related to the robot */
+  public static class Game {
+    public static class Field {
+      public static class Grid {
+        public static class ConeNode {
+          public static class Top {
+            /** Height of the node in feet */
+            public static final double HEIGHT = 3.8333;
+            /** Horizontal distance from the bumper to the node in feet */
+            public static final double DISTANCE = 3.3125;
+            /** Height of the reflective tape off the ground in feet */
+            public static final double TAPE_HEIGHT = 3.4896;
+            /** The distance from the top of the tape to the top of the node */
+            public static final double ABOVE_TAPE = 0.0156;
+          }
+
+          public static class Middle {
+            /** Height of the node in feet */
+            public static final double HEIGHT = 2.8333;
+            /** Horizontal distance from the bumper to the node in feet */
+            public static final double DISTANCE = 1.8958;
+            /** Height of the reflective tape off the ground in feet */
+            public static final double TAPE_HEIGHT = 1.8438;
+            /** The distance from the top of the tape to the top of the node */
+            public static final double ABOVE_TAPE = 0.6667;
+          }
+
+          public static class CubeNode {
+            public static class Top {
+              /** Height of the node in feet */
+              public static final double HEIGHT = 2.9583;
+              /** Horizontal distance from the bumper to the middle of the node in feet */
+              public static final double DISTANCE = 3.3125;
+            }
+
+            public static class Middle {
+              /** Height of the node in feet */
+              public static final double HEIGHT = 1.9583;
+              /** Horizontal distance from the bumper to the middle of the node in feet */
+              public static final double DISTANCE = 1.8958;
+            }
+          }
+        }
+      }
+
+      public static class Objects {
+        public static class Cone {
+          /** Height in inches */
+          public static final double HEIGHT = 12.8125;
+          /** Width of the square base in inches */
+          public static final double WIDTH = 8.375;
+          /** Diameter of the bottom of the cone */
+          public static final double BOTTOM_DIAMETER = 6.625;
+          /** Diameter of top bottom of the cone */
+          public static final double TOP_DIAMETER = 1.75;
+          /** Average diameter of the cone's grabbable area */
+          public static final double AVG_DIAMETER = 4.1875;
+          /** The pressure reading required to securely hold the cone */
+          public static final double PRESSURE_TO_HOLD = -1;
+        }
+
+        public static class Cube {
+          /** Side length of a properly inflated cube in inches */
+          public static final double LENGTH = 9.5;
+          /** The pressure reading required to securely hold the cube */
+          public static final double PRESSURE_TO_HOLD = -1;
+        }
+      }
+    }
   }
 
   /**
@@ -121,5 +300,15 @@ public static class BalanceConstants {
     /* Angle threshholds for when the bot is considered balanced and not */
     public static final double OFF_ANGLE_THRESHOLD = 10;
     public static final double ON_ANGLE_THRESHOLD = 5;
+    public static final double SPEED_MULTIPLIER = 0.4;
+  }
+
+  public static class Plunger {
+    public static final int ID_PWMMOTOR_PLUNGER = 1;
+    public static final double SPEED_PLUNGER_DEPLOY = 0.2;
+  }
+
+  public static class Camera {
+    public static final int PORT_CAMERA_AIM = 0;
   }
 }

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

@@ -1,3 +1,4 @@
+
 // 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.
@@ -7,15 +8,20 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.positioning.ArmKinematics;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
  * each mode, as described in the TimedRobot documentation. If you change the name of this class or
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
+@SuppressWarnings("unused")
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
+  private Command m_homingRoutine;
+
+  private Command displayAimVideo;
 
   private RobotContainer m_robotContainer;
 
@@ -28,6 +34,10 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+    displayAimVideo = m_robotContainer.getDisplayAimVideoCommand();
+
+    displayAimVideo.initialize();
   }
 
   /**
@@ -66,7 +76,23 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+
+    // Opens the claw if the arm is close to being overextended
+    checkOverexteneded();
+  }
+
+  private boolean isAlmostOverextended() {
+    return ArmKinematics.isAlmostOverextended(m_robotContainer.containerGetArmPose());
+  }
+
+  private void checkOverexteneded() {
+    if (isAlmostOverextended()) {
+      m_robotContainer.getRetractCommand().schedule();
+    } else if (m_robotContainer.getRetractCommand().isScheduled()) {
+      m_robotContainer.getRetractCommand().end(true);
+    }
+  }
 
   @Override
   public void teleopInit() {
@@ -76,12 +102,17 @@ public void teleopInit() {
     // this line or comment it out.
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
+      // Opens the
+      m_robotContainer.getResetCommand().schedule();
     }
   }
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+
+    checkOverexteneded();
+  }
 
   @Override
   public void testInit() {