Merge remote-tracking branch 'origin/profiling' into triggers

photon-scripts/restart-photon.sh

@@ -5,11 +5,14 @@ do
     error=`systemctl status photonvision.service | grep -i "Input was empty!"`
     if [[ $? -eq 0 ]];
     then
+        echo "Photon Vision reporting 'Input was empty'"
         dmesg | grep uvc > "/opt/uvc-$(date +%y%m%d%H%M%S).txt"
+        echo "Stopping Photon Vision"
         systemctl stop photonvision.service
         sleep 1
         modprobe uvcvideo
         sleep 1
+        echo "Starting Photon Vision"
         systemctl start photonvision.service
         sleep 15
     fi

src/main/java/frc/robot/RobotContainer.java

@@ -241,6 +241,10 @@ private void configureButtonBindings() {
         operator.rightTrigger().and(operator.leftTrigger()).whileTrue(intake.runIndexerMotor(1));
         // set shooter to home preset position
         operator.y().onTrue(elevatorWrist.homePosition());
+        operator.y().and(elevatorWrist.elevatorAtAmp).and(noteInIndexer)
+            .onTrue(intake.runIntakeMotorNonStop(0, -0.2).withTimeout(2.0)
+                .until(new Trigger(() -> !this.intake.getIndexerBeamBrakeStatus()).debounce(.5)));
+
         // increment once through states list to next state
         operator.povRight().onTrue(Commands.runOnce(() -> {
             OperatorState.increment();
@@ -249,6 +253,9 @@ private void configureButtonBindings() {
         operator.povLeft().onTrue(Commands.runOnce(() -> {
             OperatorState.decrement();
         }).ignoringDisable(true));
+        new Trigger(() -> OperatorState.getCurrentState() == OperatorState.State.kAmp)
+            .and(new Trigger(() -> !this.intake.getIndexerBeamBrakeStatus()).debounce(1.0))
+            .onTrue(elevatorWrist.homePosition());
         // run action based on current state as incremented through operator states list
         operator.a().and(OperatorState.isSpeakerMode).whileTrue(elevatorWrist.speakerPreset());
         operator.a().and(OperatorState.isAmpMode).whileTrue(Commands

src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java

@@ -71,6 +71,7 @@ public class ElevatorWrist extends SubsystemBase {
     private double estimatedWristAngle = 0;
 
     public Trigger elevatorAtHome = new Trigger(() -> elevatorAtHome());
+    public Trigger elevatorAtAmp = new Trigger(() -> elevatorAtAmp());
 
     /**
      * Create new ElevatorWrist.
@@ -395,4 +396,14 @@ public boolean elevatorAtHome() {
             3);
     }
 
+    /**
+     * Check if the elevator is at the AMP position
+     *
+     * @return True if the elevator is AMP
+     */
+    public boolean elevatorAtAmp() {
+        return MathUtil.isNear(Constants.ElevatorWristConstants.SetPoints.AMP_HEIGHT, getHeight(),
+            3);
+    }
+
 }