diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d1e1e3..72431de 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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() { diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index f859b94..b467f0e 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/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); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index ed3333e..015571b 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/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();