made zeroing when enabled based off of on season

.vscode/settings.json

@@ -66,6 +66,6 @@
   "git.pullBeforeCheckout": true,
   "extensions.ignoreRecommendations": false,
   "java.saveActions.organizeImports": true,
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable",
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable",
   "vscode-pokemon.pokemonSize": "small"
 }

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

@@ -29,8 +29,8 @@ public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
-
-  private boolean bothSubsystemsZeroed = false;
+  boolean hasAutonomousRun = false;
+  private boolean allSubsystemsZeroed = false;
 
   @Override
   public void robotInit() {
@@ -63,7 +63,7 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
-    bothSubsystemsZeroed = m_robotContainer.allZeroed();
+    allSubsystemsZeroed = m_robotContainer.allZeroed();
   }
 
   @Override
@@ -83,11 +83,15 @@ public void disabledExit() {
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-    bothSubsystemsZeroed = m_robotContainer.allZeroed();
+    allSubsystemsZeroed = m_robotContainer.allZeroed();
 
     if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
+      m_robotContainer.zeroSubsystems.andThen(Commands.deferredProxy(() -> m_autonomousCommand)).schedule();
+    } else {
+      m_robotContainer.zeroSubsystems.schedule();
     }
+
+    hasAutonomousRun = true;
   }
 
   @Override
@@ -103,6 +107,9 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    if (!hasAutonomousRun || !allSubsystemsZeroed) {
+      m_robotContainer.zeroSubsystems.schedule();
+    }
   }
 
   @Override

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

@@ -7,11 +7,14 @@
 import com.frcteam3255.joystick.SN_XboxController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.constControllers;
 import frc.robot.Constants.constField;
+import frc.robot.Constants.constMotion;
 import frc.robot.RobotMap.mapControllers;
 import frc.robot.commands.*;
 import frc.robot.commands.driver_states.DriveManual;
@@ -175,9 +178,15 @@ public class RobotContainer {
       () -> subDriverStateMachine.tryState(DriverStateMachine.DriverState.CAGE_ROTATION_SNAPPING,
           conDriver.axis_LeftY, conDriver.axis_LeftX, conDriver.axis_RightX));
 
+  Command zeroSubsystems = new ParallelCommandGroup(
+      new ZeroLift(subMotion).withTimeout(constMotion.ZEROING_TIMEOUT.in(Units.Seconds)),
+      new ZeroPivot(subMotion).withTimeout(constMotion.ZEROING_TIMEOUT.in(Units.Seconds)),
+      new ZeroWrist(subMotion).withTimeout(constMotion.ZEROING_TIMEOUT.in(Units.Seconds)))
+      .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).withName("ZeroSubsystems");
+
   public RobotContainer() {
     conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND);
-
+    zeroSubsystems.addRequirements(subStateMachine);
     subDrivetrain
         .setDefaultCommand(new DriveManual(
             subDrivetrain, subDriverStateMachine, conDriver.axis_LeftY, conDriver.axis_LeftX, conDriver.axis_RightX));
@@ -259,6 +268,9 @@ private void configDriverBindings() {
     isInProcessorAutoDriveState
         .whileTrue(TRY_PREP_ALGAE_PROCESSOR)
         .whileTrue(TRY_PREP_ALGAE_PROCESSOR_WITH_CORAL);
+
+    conDriver.btn_A
+        .onTrue(zeroSubsystems);
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/Zeroing/ZeroLift.java

@@ -0,0 +1,80 @@
+// 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.Zeroing;
+
+import frc.robot.subsystems.Motion;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.constMotion;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ZeroLift extends Command {
+  /** Creates a new ZeroLift. */
+  Motion globalMotion;
+
+  Time zeroingTimestamp;
+  boolean hasLiftZeroed = false;
+
+  public ZeroLift(Motion subMotion) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    globalMotion = subMotion;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalMotion.setLiftVoltage(Units.Volts.zero());
+    zeroingTimestamp = Units.Seconds.zero();
+    hasLiftZeroed = globalMotion.hasLiftZeroed;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    globalMotion.setLiftVoltage(constMotion.ZEROING_VOLTAGE);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+
+    // Stop all movement
+    globalMotion.setLiftVoltage(Units.Volts.zero());
+
+    // Reset to the current position if this command was not interrupted
+    if (!interrupted) {
+      globalMotion.resetLiftSensorPosition(constMotion.LIFT_ZEROED_POSITION);
+      globalMotion.hasLiftZeroed = true;
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (hasLiftZeroed) {
+      return true;
+    }
+
+    // If the current velocity is low enough to be considered as zeroed
+    if (globalMotion.getLiftVelocity().lt(constMotion.ZEROED_VELOCITY)) {
+      // And this is the first loop it has happened, begin the timer
+      if (zeroingTimestamp.equals(Units.Seconds.zero())) {
+        zeroingTimestamp = Units.Seconds.of(Timer.getFPGATimestamp());
+        return false;
+      }
+
+      // If this isn't the first loop, return if it has been below the threshold for
+      // long enough
+      return (Units.Seconds.of(Timer.getFPGATimestamp()).minus(zeroingTimestamp).gte(constMotion.ZEROED_TIME));
+    }
+
+    // If the above wasn't true, we have gained too much velocity, so we aren't at 0
+    // & need to restart the timer
+    zeroingTimestamp = Units.Seconds.zero();
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Zeroing/ZeroPivot.java

@@ -0,0 +1,78 @@
+// 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.Zeroing;
+
+import frc.robot.subsystems.Motion;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.constMotion;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ZeroPivot extends Command {
+  Motion globalMotion;
+
+  Time zeroingTimestamp;
+  boolean hasPivotZeroed = false;
+
+  /** Creates a new ZeroPivot. */
+  public ZeroPivot(Motion subMotion) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    globalMotion = subMotion;
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalMotion.setPivotVoltage(Units.Volts.zero());
+    zeroingTimestamp = Units.Seconds.zero();
+    hasPivotZeroed = globalMotion.hasPivotZeroed;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    globalMotion.setPivotVoltage(constMotion.ZEROING_VOLTAGE);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    globalMotion.setPivotVoltage(Units.Volts.zero());
+
+    if (!interrupted) {
+      globalMotion.resetPivotSensorPosition(constMotion.PIVOT_ZEROED_POSITION);
+      globalMotion.hasPivotZeroed = true;
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (hasPivotZeroed) {
+      return true;
+    }
+
+    // If the current velocity is low enough to be considered as zeroed
+    if (globalMotion.getPivotVelocity().lt(constMotion.ZEROED_VELOCITY)) {
+      // And this is the first loop it has happened, begin the timer
+      if (zeroingTimestamp.equals(Units.Seconds.zero())) {
+        zeroingTimestamp = Units.Seconds.of(Timer.getFPGATimestamp());
+        return false;
+      }
+
+      // If this isn't the first loop, return if it has been below the threshold for
+      // long enough
+      return (Units.Seconds.of(Timer.getFPGATimestamp()).minus(zeroingTimestamp).gte(constMotion.ZEROED_TIME));
+    }
+
+    // If the above wasn't true, we have gained too much velocity, so we aren't at 0
+    // & need to restart the timer
+    zeroingTimestamp = Units.Seconds.zero();
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Zeroing/ZeroWrist.java

@@ -0,0 +1,78 @@
+// 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.Zeroing;
+
+import frc.robot.subsystems.Motion;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.constMotion;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ZeroWrist extends Command {
+  Motion globalMotion;
+
+  Time zeroingTimestamp;
+  boolean hasWristZeroed = false;
+
+  /** Creates a new ZeroWrist. */
+  public ZeroWrist(Motion subMotion) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    globalMotion = subMotion;
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    globalMotion.setWristVoltage(Units.Volts.zero());
+    zeroingTimestamp = Units.Seconds.zero();
+    hasWristZeroed = globalMotion.hasWristZeroed;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    globalMotion.setWristVoltage(constMotion.ZEROING_VOLTAGE);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    globalMotion.setWristVoltage(Units.Volts.zero());
+
+    if (!interrupted) {
+      globalMotion.resetWristSensorPosition(constMotion.WRIST_ZEROED_POSITION);
+      globalMotion.hasWristZeroed = true;
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (hasWristZeroed) {
+      return true;
+    }
+
+    // If the current velocity is low enough to be considered as zeroed
+    if (globalMotion.getWristVelocity().lt(constMotion.ZEROED_VELOCITY)) {
+      // And this is the first loop it has happened, begin the timer
+      if (zeroingTimestamp.equals(Units.Seconds.zero())) {
+        zeroingTimestamp = Units.Seconds.of(Timer.getFPGATimestamp());
+        return false;
+      }
+
+      // If this isn't the first loop, return if it has been below the threshold for
+      // long enough
+      return (Units.Seconds.of(Timer.getFPGATimestamp()).minus(zeroingTimestamp).gte(constMotion.ZEROED_TIME));
+    }
+
+    // If the above wasn't true, we have gained too much velocity, so we aren't at 0
+    // & need to restart the timer
+    zeroingTimestamp = Units.Seconds.zero();
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/Motion.java

@@ -9,6 +9,7 @@
 
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
@@ -17,6 +18,7 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.MechanismPositionGroup;
 import frc.robot.Constants.constMotion;
@@ -38,6 +40,7 @@ public class Motion extends SubsystemBase {
   private Angle wristLastDesiredAngle = Degrees.zero();
   private Distance elevatorLiftLastDesiredPosition = Units.Inches.zero();
   MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0);
+  VoltageOut voltageRequest = new VoltageOut(0);
   public boolean attemptingLiftZeroing = false;
   public boolean attemptingPivotZeroing = false;
   public boolean attemptingWristZeroing = false;
@@ -54,7 +57,7 @@ public Motion() {
     frontLeftPivotMotorFollower = new TalonFX(mapMotion.FRONT_LEFT_PIVOT_CAN);
     frontRightPivotMotorLeader = new TalonFX(mapMotion.FRONT_RIGHT_PIVOT_CAN);
     wristPivotMotor = new TalonFX(mapMotion.INTAKE_PIVOT_CAN);
-
+    voltageRequest = new VoltageOut(0);
     elevatorLiftLastDesiredPosition = Units.Inches.of(0);
     // Set default motor configurations if needed
     // e.g., elevatorLeftMotor.configFactoryDefault();
@@ -134,6 +137,22 @@ public void setWristCoastMode(boolean coastMode) {
     }
   }
 
+  public void setLiftVoltage(Voltage voltage) {
+    rightLiftMotorLeader.setControl(voltageRequest.withOutput(voltage));
+    leftLiftMotorFollower.setControl(new Follower(rightLiftMotorLeader.getDeviceID(), true));
+  }
+
+  public void setPivotVoltage(Voltage voltage) {
+    frontRightPivotMotorLeader.setControl(voltageRequest.withOutput(voltage));
+    frontLeftPivotMotorFollower.setControl(new Follower(frontRightPivotMotorLeader.getDeviceID(), true));
+    backLeftPivotMotorFollower.setControl(new Follower(frontRightPivotMotorLeader.getDeviceID(), true));
+    backRightPivotMotorFollower.setControl(new Follower(frontRightPivotMotorLeader.getDeviceID(), false));
+  }
+
+  public void setWristVoltage(Voltage voltage) {
+    wristPivotMotor.setControl(voltageRequest.withOutput(voltage));
+  }
+
   public Distance getLiftPosition() {
     if (Robot.isSimulation()) {
       return elevatorLiftLastDesiredPosition;