Merge pull request #35 from Team4388/arm-command

Arm command --> Master

src/main/java/frc4388/robot/Constants.java

@@ -24,8 +24,8 @@ import frc4388.utility.LEDPatterns;
 public final class Constants {
   public static final class SwerveDriveConstants {
 
-    public static final double MAX_ROT_SPEED = -0.7;
+    public static final double MAX_ROT_SPEED = 1.5;
-    public static final double MIN_ROT_SPEED = -0.3;
+    public static final double MIN_ROT_SPEED = 0.8;
     public static       double ROTATION_SPEED = MAX_ROT_SPEED;
 
     public static final class IDs {
@@ -65,7 +65,7 @@ public final class Constants {
     public static final class Conversions {
       public static final int CANCODER_TICKS_PER_ROTATION = 4096;
 
-      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8;
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
       public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
 
       public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;

src/main/java/frc4388/robot/Robot.java

@@ -10,15 +10,22 @@ package frc4388.robot;
 import java.lang.System;
 import java.lang.reflect.Array;
 import java.util.Arrays;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
+
 import java.io.File;
 import java.io.IOException;
 import java.io.PrintWriter;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.DeferredBlock;
 import frc4388.utility.RobotTime;
 
 import frc4388.robot.subsystems.Location;
@@ -38,9 +45,6 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
-  private Location location = new Location();
-
-
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -65,18 +69,13 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {
     m_robotTime.updateTimes();
+    
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
+    
     CommandScheduler.getInstance().run();
-
-    final Tag pos = location.getPosRot();
-    if (pos != null) {
-      SmartDashboard.putNumber("x position", pos.x);
-    }
-
-    //ystem.out.print(apriltagPos[0]);
   }
 
   /**
@@ -90,7 +89,12 @@ public class Robot extends TimedRobot {
   }
 
   @Override
-  public void disabledPeriodic() {
+  public void disabledPeriodic() {}
+
+  @Override
+  public void disabledExit() {
+      DeferredBlock.execute();
+      super.disabledExit();
   }
 
   /**
@@ -134,13 +138,11 @@ public class Robot extends TimedRobot {
    * This function is called periodically during operator control.
    */
   @Override
-  public void teleopPeriodic() {
+  public void teleopPeriodic() {}
-  }
 
   /**
    * This function is called periodically during test mode.
    */
   @Override
-  public void testPeriodic() {
+  public void testPeriodic() {}
-  }
 }

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,7 +7,10 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+
 import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -17,8 +20,10 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.commands.JoystickPlayback;
 import frc4388.robot.subsystems.Arm;
+import frc4388.robot.subsystems.Claw;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.JoystickRecorder;
+import frc4388.robot.commands.PivotCommand;
 import frc4388.robot.commands.PlaybackChooser;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController; 
@@ -43,6 +48,8 @@ public class RobotContainer {
     
     public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
 
+    public final Claw m_robotClaw = new Claw(m_robotMap.servo);
+
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -63,8 +70,6 @@ public class RobotContainer {
     // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
 
     private PlaybackChooser playbackChooser;
-    private PWM             servo      = new PWM(0);
-    private boolean         servo_open = true;
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -113,8 +118,8 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
         
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive));
         //     // .onFalse()
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -134,15 +139,29 @@ public class RobotContainer {
             .onFalse(new InstantCommand());
 
         // * Operator Buttons
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+        //     // .onTrue(new InstantCommand(() -> System.out.println("Claw Button")));
+        //     .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
 
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> {
-                servo.setRaw(servo_open ? 1000 : 2000);
-                servo_open = !servo_open;
-            }));
         
+
         new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-        .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit()));
+            .onTrue(new PivotCommand(m_robotArm, 135));
+        
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
+            .onTrue(new PivotCommand(m_robotArm, 210));
+        
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
+        
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
+        
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
+
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+        //     .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw));
     }
 
     /**

src/main/java/frc4388/robot/RobotMap.java

@@ -12,6 +12,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
+import edu.wpi.first.wpilibj.PWM;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.SwerveModule;
@@ -115,6 +116,11 @@ public class RobotMap {
     rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
     
     // set neutral mode
+    leftFrontWheel.setNeutralMode(NeutralMode.Brake);
+    rightFrontWheel.setNeutralMode(NeutralMode.Brake);
+    leftBackWheel.setNeutralMode(NeutralMode.Brake);
+    rightBackWheel.setNeutralMode(NeutralMode.Brake);
+
     leftFrontSteer.setNeutralMode(NeutralMode.Brake);
     rightFrontSteer.setNeutralMode(NeutralMode.Brake);
     leftBackSteer.setNeutralMode(NeutralMode.Brake);
@@ -160,4 +166,8 @@ public class RobotMap {
     tele.configForwardSoftLimitEnable(false);
     tele.configReverseSoftLimitEnable(false);
   }
+
+  // claw stuff (WHAT IS A SOAP ENGINEER)
+  PWM servo = new PWM(0);
+
 }

src/main/java/frc4388/robot/commands/AutoBalance.java

@@ -14,9 +14,9 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 	RobotGyro gyro;
 	SwerveDrive drive;
 
-	/** Creates a new AutoBalanceTF2. */
+	/** Creates a new AutoBalance. */
 	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
-		super(0.6, 0, 0, 0);
+		super(0.6, 0, 0, 0, 0);
 
 		this.gyro = gyro;
 		this.drive = drive;

src/main/java/frc4388/robot/commands/LimeAlign.java

@@ -0,0 +1,34 @@
+// 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 frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class LimeAlign extends CommandBase {
+  public LimeAlign(SwerveDrive drive) {
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java

@@ -4,20 +4,24 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.utility.Gains;
 
 public abstract class PelvicInflammatoryDisease extends CommandBase {
-	protected Gains gains;
+	protected Gains  gains;
-	private double output = 0;
+	private   double output    = 0;
+	private   double tolerance = 0;
 
 	/** Creates a new PelvicInflammatoryDisease. */
-	public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
+	public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
-		gains = new Gains(kp, ki, kd, kf, 0);
+		gains          = new Gains(kp, ki, kd, kf, 0);
+		this.tolerance = tolerance;
 	}
 
-	public PelvicInflammatoryDisease(Gains gains) {
+	public PelvicInflammatoryDisease(Gains gains, double tolerance) {
-		this.gains = gains;
+		this.gains     = gains;
+		this.tolerance = tolerance;
 	}
 
 	/** produces the error from the setpoint */
@@ -48,11 +52,9 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 		runWithOutput(output);
 	}
 
-	// Called once the command ends or is interrupted.
-	@Override
-	public void end(boolean interrupted) {}
-
 	// Returns true when the command should end.
 	@Override
-	public boolean isFinished() { return false; }
+	public boolean isFinished() {
+		return Math.abs(getError()) < tolerance;
+	}
 }

src/main/java/frc4388/robot/commands/PivotCommand.java

@@ -0,0 +1,32 @@
+// 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 frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.subsystems.Arm;
+
+public class PivotCommand extends PelvicInflammatoryDisease {
+  private final Arm     arm;
+  private final double  target;
+  
+  /** Creates a new ArmCommand. */
+  public PivotCommand(Arm arm, double target) {
+    super(6, 1.5, 0, 0, 0.015);
+    this.arm    = arm;
+    this.target = target;
+    addRequirements(arm);
+  }
+
+  @Override
+  public double getError() {
+    return (target - arm.getArmRotation()) / 360;
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    SmartDashboard.putNumber("pivot output", output);
+    arm.setRotVel(output);
+  }
+}

src/main/java/frc4388/robot/commands/TeleCommand.java

@@ -0,0 +1,32 @@
+// 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 frc4388.robot.commands;
+
+import frc4388.robot.Constants.ArmConstants;
+import frc4388.robot.subsystems.Arm;
+
+public class TeleCommand extends PelvicInflammatoryDisease {
+  private final Arm    arm;
+  private final double target;
+  
+  /** Creates a new ArmCommand. */
+  public TeleCommand(Arm arm, double target) {
+    super(0.6, 0, 0, 0, 0);
+    this.arm    = arm;
+    this.target = target;
+    addRequirements(arm);
+  }
+
+  @Override
+  public double getError() {
+    return (arm.getArmLength() - target) /
+           (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    arm.setTeleVel(output);
+  }
+}

src/main/java/frc4388/robot/subsystems/Arm.java

@@ -5,18 +5,22 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.DeferredBlock;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Arm extends SubsystemBase {
     private WPI_TalonFX m_tele;
-    private WPI_TalonFX m_pivot;
+    public WPI_TalonFX  m_pivot;
     private CANCoder    m_pivotEncoder;
     private boolean     m_debug;
 
@@ -27,22 +31,12 @@ public class Arm extends SubsystemBase {
         m_pivotEncoder = encoder;
 
         m_tele.configFactoryDefault();
-        // m_tele.configReverseLimitSwitchSource(null, null);
         m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
         m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
         m_pivot.configFactoryDefault();
 
-        TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
+        // * Example of deferred code
-        pivotConfig.slot0.kP = ArmConstants.kP;
+        new DeferredBlock(() -> resetTeleSoftLimit());
-        pivotConfig.slot0.kI = ArmConstants.kI;
-        pivotConfig.slot0.kD = ArmConstants.kD;
-
-        pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
-        pivotConfig.remoteFilter0.remoteSensorSource   = RemoteSensorSource.CANCoder;
-        pivotConfig.primaryPID.selectedFeedbackSensor  = FeedbackDevice.RemoteSensor0;
-        m_pivot.configAllSettings(pivotConfig);
-
-        resetTeleSoftLimit();
 
         CANCoderConfiguration config = new CANCoderConfiguration();
         config.magnetOffsetDegrees = ArmConstants.OFFSET;
@@ -54,11 +48,21 @@ public class Arm extends SubsystemBase {
     }
 
     public void setRotVel(double vel) {
-        m_pivot.set(ControlMode.PercentOutput, vel / 5);
+        var degrees = Math.abs(getArmRotation()) - 135;
+        SmartDashboard.putNumber("arm degrees", degrees);
+        SmartDashboard.putNumber("arm rot vel", vel);
+
+        if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
+            m_pivot.set(ControlMode.PercentOutput, 0);
+        } else if (degrees > 90 && vel > 0) {
+            m_pivot.set(ControlMode.PercentOutput, .15 * vel);
+        } else {
+            m_pivot.set(ControlMode.PercentOutput, .3 * vel);
+        }
     }
 
     public void setTeleVel(double vel) {
-        m_tele.set(ControlMode.PercentOutput, -0.25 * vel);
+        m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
     }
 
     public void armSetRotation(double rot) {
@@ -82,21 +86,21 @@ public class Arm extends SubsystemBase {
     }
 
     public double getArmLength() {
-        return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) /
+        return m_tele.getSelectedSensorPosition();
-            (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT);
     }
 
     public double getArmRotation() {
-        return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) /
+        return m_pivotEncoder.getAbsolutePosition();
-            (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
     }
 
     public void runPivotTele(double pivot, double tele) {
-        double rot = 0;
+        double abs_pivot = Math.toRadians(getArmRotation() - 135);
+        double abs_tele  = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
+                           (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
 
-        if (checkLimits(tele, rot)) {
+        if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
-            armSetRotation(pivot);
+            setRotVel(pivot);
-            armSetLength(tele);
+            setTeleVel(tele);
         }
     }
 
@@ -112,10 +116,7 @@ public class Arm extends SubsystemBase {
         var y   = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
 
         var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
-        if (y < minHeight)
+        return y < minHeight;
-            return false;
-        
-        return true;
     }
 
     boolean tele_softLimit = false;
@@ -134,33 +135,36 @@ public class Arm extends SubsystemBase {
         tele_softLimit = !tele_softLimit;
     }
 
-    boolean resetable = true;
+    boolean resetable  = true;
+    boolean tele_reset = true;
 
     @Override
     public void periodic() {
         double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135);
-        if (degrees < 2 && resetable) {
+
-            var pivot_soft = m_pivot.getSelectedSensorPosition();
+        if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
-            var tele_soft  = m_tele.getSelectedSensorPosition();
+            var tele_soft = m_tele.getSelectedSensorPosition();
-            
+            m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
-            SmartDashboard.putNumber("start pivot", pivot_soft);
+            m_tele.configReverseSoftLimitThreshold(1000  - tele_soft);
-            SmartDashboard.putNumber("start tele", tele_soft);
+            m_tele.configForwardSoftLimitEnable(true);
-            
+            m_tele.configReverseSoftLimitEnable(true);
-            m_pivot.configForwardSoftLimitEnable(true);
+            tele_reset = false;
-            m_pivot.configReverseSoftLimitEnable(true);
+        } else if (m_tele.isFwdLimitSwitchClosed() == 0) {
-            SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value);
+            tele_reset = true;
-            SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value);
-            resetable = false;
-        } else if (degrees > 2) {
-            resetable = true;
         }
 
-        double x = Math.cos(Math.toRadians(degrees));
+        // double x = Math.cos(Math.toRadians(degrees));
-        SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED);
+    }
-        // if (m_debug)
+
-        //     SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition());
+    boolean soft_limits = true;
-        SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition());
+    public void killSoftLimits() {
-        SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition());
+        resetTeleSoftLimit();
-        SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition());
+        var pivot_soft = m_pivot.getSelectedSensorPosition();
+        var tele_soft  = m_tele.getSelectedSensorPosition();
+        
+        m_pivot.configForwardSoftLimitEnable(!soft_limits);
+        m_pivot.configReverseSoftLimitEnable(!soft_limits);
+
+        soft_limits = !soft_limits;
     }
 }

src/main/java/frc4388/robot/subsystems/Claw.java

@@ -1,9 +1,12 @@
 package frc4388.robot.subsystems;
+import edu.wpi.first.hal.PWMJNI;
 import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Claw {
+public class Claw extends SubsystemBase {
 	private PWM m_clawMotor;
-	private boolean m_open = false;
+	private boolean m_open     = false;
+	private boolean m_disabled = false;
 
     // Opens claw
 	public Claw(PWM m_clawMotor) {
@@ -14,10 +17,15 @@ public class Claw {
     public void setClaw(boolean open) {
         // Open claw
 		m_open = open;
-		m_clawMotor.setRaw(open ? 0 : 2000);
+		m_clawMotor.setRaw(m_open ? 1000 : 2000);
     }
 
+	public void toggle() {
+		setClaw(!m_open);
+	}
+
 	public boolean isClawOpen() {
 		return m_open;
 	}
-}
+
+}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -35,10 +35,6 @@ public class SwerveDrive extends SubsystemBase {
 
   private RobotGyro gyro;
 
-  // private SwerveDriveOdometry odometry;
-
-  private SwerveDrivePoseEstimator poseEstimator;
-
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
   
   public Rotation2d rotTarget = new Rotation2d();
@@ -53,48 +49,26 @@ public class SwerveDrive extends SubsystemBase {
     
     this.gyro = gyro;
 
-    // this.odometry = new SwerveDriveOdometry(
-    //   kinematics, 
-    //   gyro.getRotation2d(),
-    //   new SwerveModulePosition[] {
-    //     leftFront.getPosition(),
-    //     rightFront.getPosition(),
-    //     leftBack.getPosition(),
-    //     rightBack.getPosition()
-    //   },
-    //   getOdometry()
-    // );
-
-    this.poseEstimator = new SwerveDrivePoseEstimator(
-      kinematics, 
-      gyro.getRotation2d(), 
-      new SwerveModulePosition[] {
-        leftFront.getPosition(),
-        rightFront.getPosition(),
-        leftBack.getPosition(),
-        rightBack.getPosition()
-      },
-      new Pose2d(0,0, new Rotation2d(0))
-    );
-
     this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
   }
 
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
     if (fieldRelative) {
+
+      double rot = 0;
       if (rightStick.getNorm() > 0.1) {
         rotTarget = gyro.getRotation2d();
+        rot = rightStick.getX();
+      } else {
+        rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
       }
 
-      double rot = rightStick.getX();
+      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-
-      // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
       Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
-
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -121,82 +95,8 @@ public class SwerveDrive extends SubsystemBase {
 
   public void resetGyro() {
     gyro.reset();
-    // setOdometry(getOdometry());
     rotTarget = new Rotation2d(0);
   }
-
-  /**
-   * Updates the odometry of the SwerveDrive.
-   */
-  // public void updateOdometry() {
-  //   odometry.update(
-  //     gyro.getRotation2d(), 
-  //     new SwerveModulePosition[] {
-  //       leftFront.getPosition(),
-  //       rightFront.getPosition(),
-  //       leftBack.getPosition(),
-  //       rightBack.getPosition()
-  //     }
-  //   );
-  // }
-
-  public void updatePoseEstimator() {
-    poseEstimator.update(
-      gyro.getRotation2d(), 
-      new SwerveModulePosition[] {
-        leftFront.getPosition(),
-        rightFront.getPosition(),
-        leftBack.getPosition(),
-        rightBack.getPosition()
-      }
-    );
-  }
-
-  /**
-   * Gets the odometry of the SwerveDrive.
-   * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
-   */
-  // public Pose2d getOdometry() {
-  //   return odometry.getPoseMeters();
-  // }
-
-  public Pose2d getPoseEstimator() {
-    return poseEstimator.getEstimatedPosition();
-  }
-
-  /**
-   * Sets the odometry of the SwerveDrive.
-   * @param pose Pose to set the odometry to.
-   */
-  // public void setOdometry(Pose2d pose) {
-  //   odometry.resetPosition(
-  //     gyro.getRotation2d(), 
-  //     new SwerveModulePosition[] {
-  //       leftFront.getPosition(),
-  //       rightFront.getPosition(),
-  //       leftBack.getPosition(),
-  //       rightBack.getPosition()
-  //     },
-  //     pose
-  //   );
-  // }
-
-  public void setPoseEstimator(Pose2d pose) {
-    poseEstimator.resetPosition(
-      gyro.getRotation2d(), 
-      new SwerveModulePosition[] {
-        leftFront.getPosition(),
-        rightFront.getPosition(),
-        leftBack.getPosition(),
-        rightBack.getPosition()
-      },
-      pose
-    );
-  }
-
-  public void resetPoseEstimator() {
-    setPoseEstimator(new Pose2d());
-  }
   
   public void stopModules() {
     for (SwerveModule module : this.modules) {
@@ -204,14 +104,6 @@ public class SwerveDrive extends SubsystemBase {
     }
   }
 
-  /**
-   * Resets the odometry of the SwerveDrive to 0.
-   * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
-   */
-  // public void resetOdometry() {
-  //   setOdometry(new Pose2d());
-  // }
-
   public SwerveDriveKinematics getKinematics() {
     return this.kinematics;
   }
@@ -219,24 +111,14 @@ public class SwerveDrive extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-
-    // updateOdometry();
-    updatePoseEstimator();
-
-    // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
-    // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
-    // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
-
-    // SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
-    // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
   }
 
-  /**
+  public void toggleGear() {
-   * Shifts gear from high to low, or vice versa.
+    if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
-   * @param shift true to shift to high, false to shift to low
+      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
-   */
+    } else {
-  public void highSpeed(boolean shift) {
+      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
-    this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+    }
   }
 
 }

src/main/java/frc4388/utility/DeferredBlock.java

@@ -0,0 +1,23 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DeferredBlock {
+    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
+    private static boolean             m_hasRun = false;
+
+    public DeferredBlock(Runnable block) {
+        m_blocks.add(block);
+    }
+
+    public static void execute() {
+        if (m_hasRun) return;
+
+        for (Runnable block : m_blocks) {
+            block.run();
+        }
+
+        m_blocks.clear(); // for garbage collection
+        m_hasRun = true;
+    }
+}