From 594061a8dfa575ec4b44ce630cd297eabdf6085d Mon Sep 17 00:00:00 2001 From: Rong Date: Tue, 30 Jan 2024 20:24:49 -0800 Subject: [PATCH] UsefulArmPositions and starting new test writing --- .../components/BaseRobotComponent.java | 8 +++ .../subsystems/arm/ArmSubsystem.java | 52 +++++++++++++------ .../subsystems/arm/ArmSoftLimitTest.java | 7 +++ .../subsystems/arm/BaseArmCommandsTest.java | 8 +-- 4 files changed, 55 insertions(+), 20 deletions(-) create mode 100644 src/test/java/competition/subsystems/arm/ArmSoftLimitTest.java diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 7765de67..9d73f821 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/src/main/java/competition/injection/components/BaseRobotComponent.java @@ -1,6 +1,10 @@ package competition.injection.components; import competition.subsystems.arm.ArmSubsystem; +import competition.subsystems.arm.commands.ExtendArmCommand; +import competition.subsystems.arm.commands.ReconcileArmAlignmentCommand; +import competition.subsystems.arm.commands.RetractArmCommand; +import competition.subsystems.arm.commands.StopArmCommand; import competition.subsystems.collector.CollectorSubsystem; import competition.subsystems.schoocher.ScoocherSubsystem; import competition.subsystems.shooter.ShooterWheelSubsystem; @@ -31,6 +35,10 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract VisionSubsystem visionSubsystem(); public abstract ArmSubsystem armSubsystem(); + public abstract ExtendArmCommand extendArmCommand(); + public abstract RetractArmCommand retractArmCommand(); + public abstract StopArmCommand stopArmCommand(); + public abstract ReconcileArmAlignmentCommand reconcileArmAlignmentCommand(); public abstract ScoocherSubsystem scoocherSubsystem(); diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index 662d806a..2a725691 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -41,8 +41,8 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF boolean hasCalibratedLeft; boolean hasCalibratedRight; - LimitState leftArmAtLimit; - LimitState rightArmAtLimit; + LimitState leftArmLimitState; + LimitState rightArmLimitState; private double targetAngle; @@ -58,8 +58,11 @@ public enum LimitState { NOT_AT_LIMIT } - public enum UsefulArmPositions { - + public enum UsefulArmPosition { + DEFAULT, + RAISED, + COLLECTING_FROM_GROUND, + FIRING_FROM_SPEAKER_FRONT } @Inject @@ -91,8 +94,8 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa } this.armState = ArmState.STOPPED; - this.leftArmAtLimit = LimitState.NOT_AT_LIMIT; - this.rightArmAtLimit = LimitState.NOT_AT_LIMIT; + this.leftArmLimitState = LimitState.NOT_AT_LIMIT; + this.rightArmLimitState = LimitState.NOT_AT_LIMIT; } public void setPower(double leftPower, double rightPower) { @@ -106,15 +109,15 @@ public void setPower(double leftPower, double rightPower) { } // Check if arm at limit - if (leftArmAtLimit == LimitState.AT_FORWARD) { + if (leftArmLimitState == LimitState.AT_FORWARD) { leftPower = MathUtils.constrainDouble(leftPower, armPowerMin.get(), 0); - } else if (leftArmAtLimit == LimitState.AT_REVERSE) { + } else if (leftArmLimitState == LimitState.AT_REVERSE) { leftPower = MathUtils.constrainDouble(leftPower, 0, armPowerMax.get()); } - if (rightArmAtLimit == LimitState.AT_FORWARD) { + if (rightArmLimitState == LimitState.AT_FORWARD) { rightPower = MathUtils.constrainDouble(rightPower, armPowerMin.get(), 0); - } else if (rightArmAtLimit == LimitState.AT_REVERSE) { + } else if (rightArmLimitState == LimitState.AT_REVERSE) { rightPower = MathUtils.constrainDouble(rightPower, 0, armPowerMax.get()); } @@ -164,6 +167,23 @@ public double ticksToShooterAngle(double ticks) { return ticks * 1000; // To be modified into ticks to shooter angle formula } + public double shooterAngleToTicks(double angle) { + return 0; + } + + public double getUsefulArmPosition(UsefulArmPosition usefulArmPosition) { + double revolutions; + switch(usefulArmPosition) { + case DEFAULT -> revolutions = shooterAngleToTicks(40); + case RAISED -> revolutions = shooterAngleToTicks(40); + case COLLECTING_FROM_GROUND -> revolutions = shooterAngleToTicks(40); + case FIRING_FROM_SPEAKER_FRONT -> revolutions = shooterAngleToTicks(40); + default -> revolutions = shooterAngleToTicks(40); + } + return revolutions; + } + + public void armEncoderTicksUpdate() { aKitLog.record("ArmMotorLeftTicks", armMotorLeft.getPosition()); aKitLog.record("ArmMotorRightTicks", armMotorRight.getPosition()); @@ -199,18 +219,18 @@ public void calibrateArmOffset() { } } - leftArmAtLimit = LimitState.NOT_AT_LIMIT; - rightArmAtLimit = LimitState.NOT_AT_LIMIT; + leftArmLimitState = LimitState.NOT_AT_LIMIT; + rightArmLimitState = LimitState.NOT_AT_LIMIT; if (armMotorLeft.getForwardLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) { - leftArmAtLimit = LimitState.AT_FORWARD; + leftArmLimitState = LimitState.AT_FORWARD; } else if (armMotorLeft.getReverseLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) { - leftArmAtLimit = LimitState.AT_REVERSE; + leftArmLimitState = LimitState.AT_REVERSE; } if (armMotorRight.getForwardLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) { - rightArmAtLimit = LimitState.AT_FORWARD; + rightArmLimitState = LimitState.AT_FORWARD; } else if (armMotorRight.getReverseLimitSwitchPressed(SparkLimitSwitch.Type.kNormallyOpen)) { - rightArmAtLimit = LimitState.AT_REVERSE; + rightArmLimitState = LimitState.AT_REVERSE; } aKitLog.record("HasCalibratedLeftArm", hasCalibratedLeft); diff --git a/src/test/java/competition/subsystems/arm/ArmSoftLimitTest.java b/src/test/java/competition/subsystems/arm/ArmSoftLimitTest.java new file mode 100644 index 00000000..fc528ad4 --- /dev/null +++ b/src/test/java/competition/subsystems/arm/ArmSoftLimitTest.java @@ -0,0 +1,7 @@ +package competition.subsystems.arm; + +import competition.BaseCompetitionTest; + +public class ArmSoftLimitTest extends BaseCompetitionTest { + +} diff --git a/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java b/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java index 73d496aa..1e3d80db 100644 --- a/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java +++ b/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java @@ -24,10 +24,10 @@ public class BaseArmCommandsTest extends BaseCompetitionTest { public void setUp() { super.setUp(); arm = getInjectorComponent().armSubsystem(); - extend = new ExtendArmCommand(arm); - retract = new RetractArmCommand(arm); - stop = new StopArmCommand(arm); - reconcile = new ReconcileArmAlignmentCommand(arm); + extend = getInjectorComponent().extendArmCommand(); + retract = getInjectorComponent().retractArmCommand(); + stop = getInjectorComponent().stopArmCommand(); + reconcile = getInjectorComponent().reconcileArmAlignmentCommand(); } private void checkMotorPower(double power) {