diff --git a/rio/src/main/java/org/teamtators/levitator/commands/BumpLift.java b/rio/src/main/java/org/teamtators/levitator/commands/BumpLift.java new file mode 100644 index 00000000..64af0cd5 --- /dev/null +++ b/rio/src/main/java/org/teamtators/levitator/commands/BumpLift.java @@ -0,0 +1,23 @@ +package org.teamtators.levitator.commands; + +import org.teamtators.common.scheduler.Command; +import org.teamtators.levitator.TatorRobot; +import org.teamtators.levitator.subsystems.Lift; + +public class BumpLift extends Command{ + + private Lift lift; + private double bumpheight; + + public BumpLift(TatorRobot robot, boolean goUp, double bumpHeight) { + super("bumpLift"); + lift = robot.getSubsystems().getLift(); + requires(lift); + } + + @Override + protected boolean step() { + lift.setDesiredHeight(lift.getCurrentHeight() + bumpheight); + return true; + } +} diff --git a/rio/src/main/java/org/teamtators/levitator/commands/BumpPivot.java b/rio/src/main/java/org/teamtators/levitator/commands/BumpPivot.java new file mode 100644 index 00000000..d2b99915 --- /dev/null +++ b/rio/src/main/java/org/teamtators/levitator/commands/BumpPivot.java @@ -0,0 +1,24 @@ +package org.teamtators.levitator.commands; + +import org.teamtators.common.scheduler.Command; +import org.teamtators.levitator.TatorRobot; +import org.teamtators.levitator.subsystems.Lift; + +public class BumpPivot extends Command { + + private Lift lift; + private double bumpAngle; + + public BumpPivot(TatorRobot robot, boolean pivotRight, double bumpAngle) { + super("bumpPivot"); + lift = robot.getSubsystems().getLift(); + requires(lift); + } + + @Override + protected boolean step() { + lift.setDesiredPivotAngle(lift.getCurrentPivotAngle() + bumpAngle); + return true; + } + +} diff --git a/rio/src/main/java/org/teamtators/levitator/commands/CommandRegistrar.java b/rio/src/main/java/org/teamtators/levitator/commands/CommandRegistrar.java index cc93da8b..9f9b20bc 100644 --- a/rio/src/main/java/org/teamtators/levitator/commands/CommandRegistrar.java +++ b/rio/src/main/java/org/teamtators/levitator/commands/CommandRegistrar.java @@ -31,9 +31,7 @@ public void register(ConfigCommandStore commandStore) { commandStore.putCommand("PickerExtend", Commands.instant(picker::extend, picker)); commandStore.putCommand("PickerRetract", Commands.instant(picker::retract, picker)); - commandStore.putCommand("BumpLiftUp", Commands.instant(lift::bumpLiftUp)); - commandStore.putCommand("BumpLiftDown", Commands.instant(lift::bumpLiftDown)); - commandStore.putCommand("BumpPivotRight", Commands.instant(lift::bumpPivotRight)); - commandStore.putCommand("BumpPivotLeft", Commands.instant(lift::bumpPivotRight)); + commandStore.registerCommand("BumpLift", () -> new BumpLift(robot, true, 8)); + commandStore.registerCommand("BumpPivot", () -> new BumpPivot(robot, false, 56)); } } diff --git a/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java b/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java index 71981182..a432eaa7 100644 --- a/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java +++ b/rio/src/main/java/org/teamtators/levitator/subsystems/Lift.java @@ -209,22 +209,6 @@ public void setLiftPower(double liftPower) { liftMotor.set(liftPower); } - public void bumpLiftUp() { - setDesiredHeight(getCurrentHeight() + config.bumpHeightValue); - } - - public void bumpLiftDown() { - setDesiredHeight(getCurrentHeight() - config.bumpHeightValue); - } - - public void bumpPivotRight() { - setDesiredPivotAngle(getCurrentPivotAngle() + config.bumpPivotValue); - } - - public void bumpPivotLeft() { - setDesiredPivotAngle(getCurrentPivotAngle() - config.bumpPivotValue); - } - public void setPivotPower(double pivotPower) { pivotMotor.set(pivotPower); }