diff --git a/src/main/java/frc/robot/commands/arm/ArmMove.java b/src/main/java/frc/robot/commands/arm/ArmMove.java index 1985eaf..a38c551 100644 --- a/src/main/java/frc/robot/commands/arm/ArmMove.java +++ b/src/main/java/frc/robot/commands/arm/ArmMove.java @@ -3,32 +3,27 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.arm.ArmSubsystem; +import frc.robot.subsystems.arm.ArmSubsystem.ArmPositions; public class ArmMove extends CommandBase { private ArmSubsystem arm; - private double wristGoalRad, shooterPowerGoal; - private boolean intakeMotorStatus; + private ArmPositions armPos; - public ArmMove (ArmSubsystem arm, double wristGoalRad, double shooterPowerGoal, boolean intakeMotorStatus) { + public ArmMove (ArmSubsystem arm, ArmPositions armPos) { this.arm = arm; - this.wristGoalRad = wristGoalRad; - this.shooterPowerGoal = shooterPowerGoal; - this.intakeMotorStatus = intakeMotorStatus; + this.armPos = armPos; } @Override public void initialize() { - wristGoalRad = SmartDashboard.getNumber("wrist goal rad", wristGoalRad); - shooterPowerGoal = SmartDashboard.getNumber("shooter power goal", shooterPowerGoal); - intakeMotorStatus = SmartDashboard.getBoolean("intake motor activated", intakeMotorStatus); - arm.setWristGoalRads(wristGoalRad); arm.setShooterGoalPower(shooterPowerGoal); + arm.setMode(armPos); } @Override public boolean isFinished() { // TODO Auto-generated method stub - return super.isFinished(); + return arm.getArmPos() == ArmPositions.Idle; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 411a6de..cec1637 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -24,6 +24,25 @@ public class ArmSubsystem extends SubsystemBase{ private double intakeGoalPower; + public static enum ArmPositions { + + Intake(0,0,0), + ShootHigh(0,0,0), + ShootMid(0,0,0), + ShootLow(0,0,0), + Idle(0,0,0); + + public final int wristPos, shooterPower, intakePower; + + private ArmPositions(int wristPos, int shooterPower, int intakePower) { + this.wristPos = wristPos; + this.shooterPower = shooterPower; + this.intakePower = intakePower; + } + } + + private ArmPositions currentArmPos = ArmPositions.Idle; + public ArmSubsystem() { wristMotor = new CANSparkMax(Constants.ArmConstants.wristMotorID, MotorType.kBrushless); intakeMotor = new CANSparkMax(Constants.ArmConstants.intakeMotorID, MotorType.kBrushless); @@ -33,6 +52,18 @@ public ArmSubsystem() { } + + public void setMode(ArmPositions armPos) { + currentArmPos = armPos; + wristGoalRad = armPos.wristPos; + shooterGoalPower = armPos.shooterPower; + intakeGoalPower = armPos.intakePower; + } + + public ArmPositions getArmPos() { + return currentArmPos; + } + //////////////////////////////////////////////////////////////////////////////////////////////////// public double getWristPositionRad() { @@ -56,9 +87,6 @@ private void checkWristAmps() { } } - public void setWristGoalRads(double rad) { - wristGoalRad = rad; - } private void wristControl() { // add TrapezoidProfile + feedforward later? @@ -91,9 +119,6 @@ private void checkShooterAmps() { } } - public void setShooterGoalPower(double power) { - shooterGoalPower = power; - } public void shooterControl() { setShooterMotorPower(shooterGoalPower);