Arm homing

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.drive.TankDrive;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ArmSubsystem.ArmPositions;
 import frc.robot.subsystems.ArmSubsystem;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.commands.Intake;
@@ -45,7 +46,8 @@ private void configureButtonBindings() {
         new JoystickButton(gamepad, Button.kX.value)
             .onTrue(new InstantCommand(intake::spit))
             .onFalse(new InstantCommand(intake::stop));
-
+        new JoystickButton(gamepad, Button.kA.value)
+            .onTrue(new InstantCommand(intake::home));
     }
 
     private void configureSubsystems() {

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

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ArmSubsystem;
+import frc.robot.subsystems.ArmSubsystem.ArmPositions;
 
 public class Intake extends CommandBase {
 
@@ -33,4 +34,8 @@ public void stop(){
         arm.setShooterMotorPower(0.0);
     }
 
+    public void home(){
+        arm.setMode(ArmPositions.Home);
+    }
+
 }

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

@@ -23,6 +23,8 @@ public class ArmSubsystem extends SubsystemBase{
     private double shooterGoalPower;
     private double intakeGoalPower;
 
+    private double wristOffset;
+
 
     private double wristIntake, wristShootHigh, wristShootMid, wristShootLow, wristIdle;
     private double shooterIntake, shooterShootHigh, shooterShootMid, shooterShootLow, shooterIdle = 0;
@@ -33,7 +35,8 @@ public static enum ArmPositions {
         ShootHigh,
         ShootMid,
         ShootLow,
-        Idle;
+        Idle,
+        Home;
     }
 
     private ArmPositions currentArmPos = ArmPositions.Idle;
@@ -44,6 +47,7 @@ public ArmSubsystem() {
         leftMotor = new CANSparkMax(Constants.ArmConstants.leftMotorID, MotorType.kBrushless);
         rightMotor = new CANSparkMax(Constants.ArmConstants.rightMotorID, MotorType.kBrushless);
         leftMotor.follow(rightMotor, true);
+        wristOffset = 0.0;
 
     }
 
@@ -70,8 +74,15 @@ public void setMode(ArmPositions armPos) {
             wristGoalRad = wristIdle;
             shooterGoalPower = shooterIdle;
             intakeGoalPower = intakeOff;
+        } else if(armPos == ArmPositions.Home){
+            shooterGoalPower = shooterIdle;
+            intakeGoalPower = intakeOff;
+            wristGoalRad = wristShootLow;
+            if(getWristMotorAmps() > 10.0){
+                wristGoalRad = getWristPositionRad();
+                wristOffset = getWristPositionRad(); 
+            }
         }
-
     }
 
     public ArmPositions getArmPos() {
@@ -81,7 +92,7 @@ public ArmPositions getArmPos() {
     ////////////////////////////////////////////////////////////////////////////////////////////////////
 
     public double getWristPositionRad() {
-        return wristMotor.getEncoder().getPosition() * 2 * Math.PI;
+        return wristMotor.getEncoder().getPosition() * 2 * Math.PI - wristOffset;
     }
 
     public void setWristMotorPower(double percent) {
@@ -192,6 +203,9 @@ public void periodic() {
 
         intakeOn = SmartDashboard.getNumber("intake power", 0);
 
+        if(getArmPos() == ArmPositions.Home){
+
+        }
 
         wristControl();