Fixed encoder syncing - Burn Flashes

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

@@ -19,7 +19,9 @@
 import frc.robot.SubsystemChecker.SubsystemType;
 import frc.robot.commands.BrakeModeDisabled;
 import frc.robot.commands.CheckArmSetpoints;
+import frc.robot.commands.SetBlingCommand;
 import frc.robot.commands.SyncArmEncoders;
+import frc.robot.commands.SyncArmEncodersV2;
 import frc.robot.commands.SyncSteerEncoders;
 import frc.robot.config.Config;
 import frc.robot.robotcontainers.ArmBotContainer;
@@ -30,6 +32,7 @@
 import frc.robot.robotcontainers.MiniNeoDiffContainer;
 import frc.robot.robotcontainers.MiniSwerveContainer;
 import frc.robot.robotcontainers.RobotContainer;
+import frc.robot.subsystems.BlingSubsystem;
 import frc.robot.subsystems.DiffNeoSubsystem;
 import frc.robot.subsystems.DiffTalonSubsystem;
 
@@ -46,6 +49,8 @@ public class Robot extends TimedRobot {
 
   Compressor pcmCompressor = new Compressor(1, PneumaticsModuleType.CTREPCM);
 
+  private boolean m_firstDisableAtBootup = true;
+
 
 
 
@@ -97,8 +102,9 @@ public void robotInit() {
     } 
 
     if (SubsystemChecker.canSubsystemConstruct(SubsystemType.SwerveSubsystem)) {
+      BlingSubsystem.getINSTANCE().setRed();
       new SyncSteerEncoders().schedule();
-      new WaitCommand(3).andThen(new SyncArmEncoders()).schedule();
+      new SyncArmEncodersV2().schedule();
     } 
 
     // Add CommandScheduler to shuffleboard so we can display what commands are scheduled
@@ -131,7 +137,15 @@ public void robotPeriodic() {
   @Override
   public void disabledInit() {
     if (brakeModeDisabledCommand != null) 
-      brakeModeDisabledCommand.schedule();  
+      brakeModeDisabledCommand.schedule(); 
+
+    if (SubsystemChecker.canSubsystemConstruct(SubsystemType.BlingSubsystem)) {
+      if (m_firstDisableAtBootup) {
+        m_firstDisableAtBootup = false;
+      } else {
+        BlingSubsystem.getINSTANCE().setDisabled();
+      }
+    }
   }
 
   @Override

src/main/java/frc/robot/SubsystemChecker.java

@@ -34,11 +34,12 @@ public enum SubsystemType {
      */
     // RobotID: 0, 2023 Competition robot, unnamed
     private static SubsystemType[] compBotId0 = new SubsystemType[] {
-        SubsystemType.SwerveSubsystem,  // Chassis unknown
+        SubsystemType.SwerveSubsystem,
         SubsystemType.ArmSubsystem,
         SubsystemType.RelaySubsystem,
         SubsystemType.VisionNTSubsystem,
         SubsystemType.GripperSubsystem,
+        SubsystemType.BlingSubsystem,
     };
 
     // RobotID: 1, 2022 robot, RapidReact, Clutch

src/main/java/frc/robot/commands/PhotonMoveToTarget.java

@@ -0,0 +1,108 @@
+// 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.commands;
+
+import java.util.List;
+
+import org.photonvision.*;
+import org.photonvision.targeting.TargetCorner;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.DiffTalonSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class PhotonMoveToTarget extends CommandBase {
+  double EXAMPLE_SIZE_HEIGHT = 104.6;
+  double EXAMPLE_DISTANCE = 1.000;
+  Pose2d cameraOffset = new Pose2d(new Translation2d(0,0.3), Rotation2d.fromDegrees(0));
+  //height of april = 1 foot 3 and 1/4 to bottom of black
+  /** Creates a new ExampleCommand. */
+  DoubleArrayPublisher pubSetPoint;
+  PhotonCamera camera1;
+  PIDController xController;
+  PIDController yController;
+  PIDController yawController;
+  Translation2d setPoint;
+  Rotation2d rotationSetPoint;
+  LinearFilter filteryaw = LinearFilter.movingAverage(10);
+  LinearFilter filterX = LinearFilter.movingAverage(10);
+  LinearFilter filterY = LinearFilter.movingAverage(10);
+
+
+  public PhotonMoveToTarget() {
+    camera1 = new PhotonCamera("OV9281");
+    xController = new PIDController(0.5, 0.01, 0);
+    yController = new PIDController(0.5, 0.01, 0);
+    yawController = new PIDController(0.02, 0, 0);
+    pubSetPoint = NetworkTableInstance.getDefault().getTable(("PhotonCamera")).getDoubleArrayTopic("PhotonAprilPoint").publish(PubSubOption.periodic(0.02));
+    addRequirements(SwerveSubsystem.getInstance());
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    filterX.reset();
+    filterY.reset();
+    filteryaw.reset();
+    rotationSetPoint = SwerveSubsystem.getInstance().getPose().getRotation();
+    setPoint = SwerveSubsystem.getInstance().getPose().getTranslation();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    var result = camera1.getLatestResult();
+    Pose2d odometryPose = SwerveSubsystem.getInstance().getPose();
+    if (result.hasTargets()){
+      List<TargetCorner> corners = result.getBestTarget().getDetectedCorners();
+      double heightSize = (corners.get(0).y - corners.get(3).y + corners.get(1).y - corners.get(2).y)/2;
+      //System.out.println("tagsize "+heightSize);
+      double range = EXAMPLE_SIZE_HEIGHT*EXAMPLE_DISTANCE/heightSize;
+      //System.out.println("range"+range);
+      Rotation2d yaw = Rotation2d.fromDegrees(-result.getBestTarget().getYaw());  
+
+      Translation2d visionXY = new Translation2d(range, yaw);
+      Translation2d robotRotated = visionXY.rotateBy(cameraOffset.getRotation());
+      Translation2d robotToTargetRELATIVE = robotRotated.plus(cameraOffset.getTranslation());
+      Translation2d robotToTarget = robotToTargetRELATIVE.rotateBy(odometryPose.getRotation());
+      Translation2d feildToTarget = robotToTarget.plus(odometryPose.getTranslation());
+      //System.out.println("robot to target "+ robotToTargetRELATIVE.toString());
+      //System.out.println("odometry"+ odometryPose.toString());
+      Rotation2d fieldOrientedTarget = yaw.rotateBy(odometryPose.getRotation());
+
+      setPoint = new Translation2d(filterX.calculate(feildToTarget.getX())-1,filterY.calculate(feildToTarget.getY()));
+      rotationSetPoint = Rotation2d.fromDegrees(filteryaw.calculate(fieldOrientedTarget.getDegrees()));
+      pubSetPoint.accept(new double[]{setPoint.getX(),setPoint.getY(),rotationSetPoint.getRadians()});
+    }
+    //System.out.println("set "+setPoint);
+    double xSpeed = xController.calculate(odometryPose.getX(), setPoint.getX());
+    double yspeed = yController.calculate(odometryPose.getY(), setPoint.getY());
+    double rotation = yawController.calculate(odometryPose.getRotation().getDegrees(), rotationSetPoint.getDegrees());
+    SwerveSubsystem.getInstance().drive(xSpeed, yspeed, rotation, true, false);
+  }
+
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    SwerveSubsystem.getInstance().stopMotors();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/ResetGyro.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.SwerveSubsystem;
@@ -24,8 +25,7 @@ public ResetGyro() {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    Pose2d pose = SwerveSubsystem.getInstance().getPose();
-    Pose2d newPose = new Pose2d(pose.getTranslation(), new Rotation2d(0));
+    Pose2d newPose = new Pose2d(new Translation2d(3, 3), new Rotation2d(0));
     SwerveSubsystem.getInstance().resetOdometry(newPose);
   }
 

src/main/java/frc/robot/commands/SyncArmEncodersV2.java

@@ -0,0 +1,150 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.config.ArmConfig;
+import frc.robot.subsystems.ArmSubsystem;
+import frc.robot.subsystems.BlingSubsystem;
+
+public class SyncArmEncodersV2 extends CommandBase {
+    private Timer m_smallTimer = new Timer();
+    private Timer m_permanantTimer = new Timer();
+    private int m_commandState = 0;
+    private int m_numSamples = 0;
+    private double m_sumBotSamples = 0;
+    private double m_sumTopSamples = 0;
+
+    /** Creates a new SyncSteerEncoders. */
+    public SyncArmEncodersV2() {
+        addRequirements(ArmSubsystem.getInstance());
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        m_smallTimer.restart();
+        m_permanantTimer.restart();
+        m_numSamples = 0;
+        m_sumBotSamples = 0;
+        m_sumTopSamples = 0;
+        m_commandState = 0;
+
+        BlingSubsystem.getINSTANCE().noEncodersSynced();
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        if (m_smallTimer.get() > ArmConfig.ENCODER_SYNCING_PERIOD_V2) {
+            m_smallTimer.reset();
+
+            if (m_commandState == 0 && m_permanantTimer.hasElapsed(1)) {
+                DriverStation.reportWarning(
+                    String.format("Arm encoders are not synced, collecting arm encoder samples... (%.1fs)", m_permanantTimer.get()),
+                    false);
+
+                m_commandState = 1;
+
+            } else if (m_commandState == 1) {
+                m_sumBotSamples += ArmSubsystem.getInstance().getAbsoluteBottom();
+                m_sumTopSamples += ArmSubsystem.getInstance().getAbsoluteTop();
+                m_numSamples++;
+
+                if (m_numSamples >= ArmConfig.NUM_SYNCING_SAMPLES) {
+                    m_commandState = 2;
+                }
+            } else if (m_commandState == 2) {
+                ArmSubsystem.getInstance().resetEncoder(
+                    m_sumBotSamples / m_numSamples,
+                    m_sumTopSamples / m_numSamples
+                );
+
+                if (ArmSubsystem.getInstance().areEncodersSynced() == false) {
+                    DriverStation.reportWarning(
+                        String.format("Arm encoders are not synced, attempting to sync them... (%.1fs)", m_permanantTimer.get()),
+                        false);
+
+                } else {
+                    m_commandState = 99; // End the command, the encoders are synced
+                }
+            }
+        }
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        if (m_commandState == 99) {
+            DriverStation.reportWarning(
+                        String.format("Arm encoders are synced (%.1f) \n", m_permanantTimer.get()),
+                        false);    
+
+            BlingSubsystem.getINSTANCE().armEncodersSynced();
+
+            new WaitCommand(1).andThen(
+                Commands.runOnce(() -> BlingSubsystem.getINSTANCE().armEncodersSynced()),
+                new WaitCommand(1),
+                Commands.runOnce(() -> BlingSubsystem.getINSTANCE().armEncodersSynced())
+            ).schedule();
+
+            ArmSubsystem.getInstance().sparkMaxBurnFlash();
+        }
+
+        m_permanantTimer.stop();
+        m_smallTimer.stop();
+
+        if (           
+            /** Temporary checks for current encoder setup. TODO: Remove once encoder wrapping is solved.*/
+            ArmSubsystem.getInstance().getBottomPosition() < Math.toRadians(5) ||
+            ArmSubsystem.getInstance().getBottomPosition() > Math.toRadians(170) ||
+            ArmSubsystem.getInstance().getTopPosition() < Math.toRadians(5) ||
+            ArmSubsystem.getInstance().getTopPosition() > Math.toRadians(70) ||
+            ArmSubsystem.getInstance().areEncodersSynced() == false ||
+            m_commandState != 99
+            ) {
+
+            DriverStation.reportError("Arm encoders were reset outside the proper range. " +
+                    " Disabling the arm to prevent damage." +
+                    " Make sure the code boots up with the arm in the reset position.", true);
+
+            // This line below will prevent any commands from being scheduled to the ArmSubsystem.
+            // This line is temporary while we haven't properly setup the absolute encoders properly yet.
+            // If it's blocking you from using the arm, you just need to put the arm in the reset position
+            //    and reboot the code (which will properly reset the NEO encoders the absolute encoders).
+            Commands.run(() -> ArmSubsystem.getInstance(), ArmSubsystem.getInstance()).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).ignoringDisable(true).schedule();
+        }
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        if (m_permanantTimer.get() > ArmConfig.ENCODER_SYNCING_TIMEOUT)  {
+            DriverStation.reportError(
+                String.format("Arm encoders are not synced. SyncArmEncodersV2 spent %.1fs trying to sync them and has timed out",
+                m_permanantTimer.get()),
+                false);
+
+            return true;
+        }
+        return m_commandState == 99;
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return true;
+    }
+
+    @Override
+    public InterruptionBehavior getInterruptionBehavior() {
+        DriverStation.reportWarning("Another ArmSubsystem command was scheduled while " +
+                "SyncArmEncodersV2 is still running. Cancelling the incoming command.", false);
+        return InterruptionBehavior.kCancelIncoming;
+    }
+}

src/main/java/frc/robot/commands/SyncSteerEncoders.java

@@ -43,7 +43,7 @@ public void initialize() {
     @Override
     public void execute() {
         SmartDashboard.putNumber("SyncSteerState", state);
-        if (state == 0 && m_permanantTimer.hasElapsed(6)) {
+        if (state == 0 && m_permanantTimer.hasElapsed(1)) {
             state = 1;
             DriverStation.reportWarning(
                     String.format("Starting to sync steer encoders (%.1fs)", m_permanantTimer.get()), false);
@@ -57,14 +57,16 @@ public void execute() {
                     false);
                 SwerveSubsystem.getInstance().resetEncodersFromCanCoder();
             } else {
-                state = 2;
+                state = 99;
                 m_smallTimer.restart();
             }
         }
-        if (state == 2 && m_smallTimer.hasElapsed(5)) {
-            SwerveSubsystem.getInstance().resetEncodersFromCanCoder();
-            state = 99;
-        }
+        // if (state == 2 && m_smallTimer.hasElapsed(1)) {
+        //     if (SwerveSubsystem.getInstance().checkSteeringEncoders() == false) {
+        //         SwerveSubsystem.getInstance().resetEncodersFromCanCoder();
+        //     }
+        //     state = 99;
+        // }
         SmartDashboard.putNumber("SyncSteerState", state);
     }
 
@@ -77,7 +79,11 @@ public void end(boolean interrupted) {
                         false);
             BlingSubsystem.getINSTANCE().steerEncodersSynced();
 
-            new WaitCommand(1).andThen(Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced())).schedule();
+            new WaitCommand(1).andThen(
+                Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced()),
+                new WaitCommand(1),
+                Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced())
+            ).schedule();
         }
 
         m_permanantTimer.stop();

src/main/java/frc/robot/config/ArmConfig.java

@@ -204,6 +204,7 @@ public enum ArmPosition {
 
       // Syncing encoders
       public static double ENCODER_SYNCING_PERIOD = 0.4; // seconds
+      public static double ENCODER_SYNCING_PERIOD_V2 = 0.05; // seconds
       public static int ENCODER_SYNCING_TIMEOUT = 20; // seconds
       public static double ENCODER_SYNCING_TOLERANCE = 0.01; // radians
       public static int NUM_SYNCING_SAMPLES = 20; // num of samples needed to average