Add pico power cycling system + QoL changes (#296)

Patribots4738 · Apr 18, 2024 · 1bccf8d · 1bccf8d
2 parents e66b095 + d1fecfe
commit 1bccf8d
Show file tree

Hide file tree

Showing 7 changed files with 66 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -5,6 +5,7 @@
 import org.littletonrobotics.urcl.URCL;
 
 import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -71,12 +72,13 @@ public void robotInit() {
     private boolean updatedAlready = false;
     private boolean updateTimer = false;
     private boolean startedURCL = false;
+
     @Override
     public void robotPeriodic() {
         // Set the previous to the current timestamp before it updates
         Robot.previousTimestamp = Robot.currentTimestamp;
         Robot.currentTimestamp = Timer.getFPGATimestamp();
-        if (gameMode != GameMode.DISABLED || true) {
+        if (gameMode != GameMode.DISABLED) {
             Monologue.updateAll();
         }
         else {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -107,7 +107,7 @@ public class RobotContainer implements Logged {
     private Ampper ampper;
     private ShooterCmds shooterCmds;
     @IgnoreLogged
-    private PicoColorSensor piPico = new PicoColorSensor();
+    private PicoColorSensor piPico;
 
     @IgnoreLogged
     private PieceControl pieceControl;
@@ -154,7 +154,9 @@ public RobotContainer() {
         operator = new PatriBoxController(OIConstants.OPERATOR_CONTROLLER_PORT, OIConstants.OPERATOR_DEADBAND);
 
         pdh = new PowerDistribution(30, ModuleType.kRev);
-        pdh.setSwitchableChannel(false); 
+        pdh.setSwitchableChannel(false);
+
+        piPico = new PicoColorSensor();
 
         intake = new Intake();
         climb = new Climb();
@@ -177,7 +179,6 @@ public RobotContainer() {
 
         ledStrip = new LedStrip(swerve::getPose);
         indexer = new Indexer();
-
         shooter = new Shooter();
         elevator = new Elevator();
         ampper = new Ampper();

diff --git a/src/main/java/frc/robot/subsystems/PicoColorSensor.java b/src/main/java/frc/robot/subsystems/PicoColorSensor.java
@@ -3,18 +3,20 @@
 import java.nio.charset.StandardCharsets;
 import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.concurrent.locks.ReentrantLock;
-
 import com.revrobotics.ColorMatchResult;
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SerialPortJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Robot;
 import frc.robot.util.Constants.ColorSensorConstants;
+import frc.robot.util.custom.TransistorPower;
 import monologue.Annotations.Log;
 import monologue.Logged;
 
+
 public class PicoColorSensor implements AutoCloseable, Logged {
     public static class RawColor {
         public RawColor(int r, int g, int b, int _ir) {
@@ -111,6 +113,8 @@ private int findNextComma(byte[] data, int readLen, int lastComma) {
     private String matchStringElevator;
     @Log
     private String matchStringShooter;
+    private TransistorPower powerToPico;
+    private double disabledTimestamp = 0;
 
     private void threadMain() {
         // Using JNI for a non allocating read
@@ -211,6 +215,10 @@ private void threadMain() {
                 } else {
                     this.hasNoteElevator = false;
                     this.proximityElevator = 0;
+                    if (Robot.currentTimestamp - disabledTimestamp > 3) {
+                        disabledTimestamp = Robot.currentTimestamp;
+                        powerToPico.getPowerCycleCommand().schedule();
+                    }
                 }
                 if (hasColorShooter) {
                     this.colorShooter.red = colorShooter.red;
@@ -235,6 +243,10 @@ private void threadMain() {
                 } else {
                     this.hasNoteShooter = false;
                     this.proximityShooter = 0;
+                    if (Robot.currentTimestamp - disabledTimestamp > 3) {
+                        disabledTimestamp = Robot.currentTimestamp;
+                        powerToPico.getPowerCycleCommand().schedule();
+                    }
                 }
             } finally {
                 threadLock.unlock();
@@ -245,6 +257,8 @@ private void threadMain() {
     }
 
     public PicoColorSensor() {
+        powerToPico = new TransistorPower(0);
+        powerToPico.set(true);
         readThread = new Thread(this::threadMain);
         readThread.setName("PicoColorSensorThread");
         readThread.start();

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -123,6 +123,7 @@ public static final class ShooterConstants {
         public static final double SHOOTER_VELOCITY_CONVERSION_FACTOR = 1.0;
         // degrees
         public static final double PIVOT_POSITION_CONVERSION_FACTOR = 360;
+        public static final double PIVOT_ZERO_OFFSET = 51.2235296;
 
         public static final PatrIDConstants SHOOTER_PID = new PatrIDConstants(
             0.002,
@@ -223,9 +224,9 @@ public static final class ShooterConstants {
                 put(10, SpeedAngleTriplet.of(3706.0, 3305.0, 37.8));
                 put(11, SpeedAngleTriplet.of(3856.0, 3539.0, 34.5));
                 put(12, SpeedAngleTriplet.of(3921.0, 3558.0, 33.3));
-                put(13, SpeedAngleTriplet.of(4075.0, 3691.0, 30.9));
+                put(13, SpeedAngleTriplet.of(4075.0, 3691.0, 31.8));
                 // Future note, 13.2ft is a common shot which should have its own calibration point
-                put(14, SpeedAngleTriplet.of(4190.0, 3731.0, 29.4));
+                put(14, SpeedAngleTriplet.of(4214.0, 3755.0, 30.9)); 
                 put(15, SpeedAngleTriplet.of(4531.0, 4058.0, 28));
                 put(16, SpeedAngleTriplet.of(4190.0, 3731.0, 26.7));
 
@@ -606,7 +607,7 @@ public static final class LEDConstants {
     }   
 
     public static final class NeoMotorConstants {
-        public static final boolean SAFE_SPARK_MODE = false;
+        public static final boolean SAFE_SPARK_MODE = true;
         public static final double VORTEX_FREE_SPEED_RPM = 6784;
         public static final double NEO_FREE_SPEED_RPM = 5676;
 

diff --git a/src/main/java/frc/robot/util/auto/PathPlannerStorage.java b/src/main/java/frc/robot/util/auto/PathPlannerStorage.java
@@ -297,7 +297,7 @@ private Command generateNonObjectDetectionCommand(int i, int endingNote, boolean
         Command skipNoteCommand = AutoBuilder.followPath(skipNote)
             .raceWith(NamedCommands.getCommand("ToIndexer"));
 
-        return Commands.waitUntil(() -> shooterSensor.getAsBoolean() || elevatorSensor.getAsBoolean()).withTimeout(0.2).andThen(
+        return Commands.waitUntil(() -> shooterSensor.getAsBoolean() || elevatorSensor.getAsBoolean()).withTimeout(0.4).andThen(
             Commands.defer(() -> 
                 Commands.either(
                     shootAndMoveToNextNote,

diff --git a/src/main/java/frc/robot/util/calc/AlignmentCalc.java b/src/main/java/frc/robot/util/calc/AlignmentCalc.java
@@ -177,7 +177,7 @@ public ChassisSpeeds getSpeakerRotationalSpeeds(double driverX, double driverY,
 
     public ChassisSpeeds getPassRotationalSpeeds(double driverX, double driverY, ShooterCmds shooterCmds) {
         return 
-            getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToPass(swerve.getPose(), swerve.getRobotRelativeVelocity()));
+            getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToPass(swerve.getPose(), swerve.getRobotRelativeVelocity().div(1.678)));
     }
 
     /**

diff --git a/src/main/java/frc/robot/util/custom/TransistorPower.java b/src/main/java/frc/robot/util/custom/TransistorPower.java
@@ -0,0 +1,38 @@
+package frc.robot.util.custom;
+
+import edu.wpi.first.wpilibj.AnalogOutput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+
+public class TransistorPower extends AnalogOutput {
+    public TransistorPower(int port) {
+        super(port);
+    }
+
+    public TransistorPower(int port, boolean state) {
+        super(port);
+        set(state);
+    }
+
+    public void setState(boolean state) {
+        setVoltage(state ? 5 : 0);
+    }
+
+    public void set(boolean state) {
+        setState(state);
+    }
+
+    public boolean get() {
+        return getVoltage() > 0.5;
+    }
+
+    public Command getPowerCycleCommand() {
+        return Commands.runOnce(() -> set(false))
+                    .andThen(Commands.waitSeconds(1), 
+                    Commands.runOnce(() -> set(true))).ignoringDisable(true);
+    }
+
+    public void schedulePowerCycleCommand() {
+        getPowerCycleCommand().schedule();
+    }
+}