diff --git a/build.gradle b/build.gradle index cef4cb6..d236403 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" id "com.diffplug.spotless" version "6.22.0" } diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 22585c8..e0f788d 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,23 +5,12 @@ package com.stuypulse.robot; -import com.stuypulse.robot.constants.Settings.RobotType; - import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; public class Robot extends TimedRobot { - public static final RobotType ROBOT; - - static { - if (Robot.isSimulation()) - ROBOT = RobotType.SIM; - else - ROBOT = RobotType.fromString(System.getenv("serialnum")); - } - private RobotContainer robot; private Command auto; diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bf6a60e..ef278c7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,30 +5,10 @@ package com.stuypulse.robot; -import java.lang.annotation.ElementType; -import com.stuypulse.robot.commands.Elevator.ElevatorToBottom; -import com.stuypulse.robot.commands.Elevator.ElevatorToLvl1; -import com.stuypulse.robot.commands.Elevator.ElevatorToLvl2; -import com.stuypulse.robot.commands.Elevator.ElevatorToLvl3; -import com.stuypulse.robot.commands.Elevator.ElevatorToLvl4; -import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.subsystems.Elevator.Elevator; -import javax.management.openmbean.OpenType; - -// Algae Commands -import com.stuypulse.robot.commands.algae.AlgaeGroundPickup; -import com.stuypulse.robot.commands.algae.AlgaeL2; -import com.stuypulse.robot.commands.algae.AlgaeL3; -import com.stuypulse.robot.commands.algae.AlgaeProcessorScore; -import com.stuypulse.robot.commands.algae.AlgaeReefKnockoff; -import com.stuypulse.robot.commands.algae.AlgaeStopRoller; -import com.stuypulse.robot.commands.algae.AlgaeStow; - import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.algae.Algae; - +import com.stuypulse.robot.subsystems.elevator.Elevator; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -79,41 +59,6 @@ private void configureDriverBindings() { private void configureOperatorBindings() { - operator.getDPadDown() - .onTrue(new ElevatorToLvl1()); - - operator.getDPadLeft() - .onTrue(new ElevatorToLvl2()); - - operator.getDPadRight() - .onTrue(new ElevatorToLvl3()); - - operator.getDPadUp() - .onTrue(new ElevatorToLvl4()); - - operator.getLeftButton() - .onTrue(new ElevatorToBottom()); - - operator.getBottomButton() - .onTrue(new AlgaeStow()); - - operator.getTopButton() - .onTrue(new AlgaeGroundPickup()); - - operator.getLeftButton() - .onTrue(new AlgaeL2()); - - operator.getRightButton() - .onTrue(new AlgaeL3()); - - operator.getLeftBumper() - .onTrue(new AlgaeProcessorScore()); - - operator.getRightBumper() - .onTrue(new AlgaeReefKnockoff()); - - operator.getLeftMenuButton() - .onTrue(new AlgaeStopRoller()); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java index 83403f3..9306caf 100644 --- a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java +++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java @@ -1,10 +1,10 @@ -package com.stuypulse.robot.commands.Elevator; +package com.stuypulse.robot.commands.elevator; import com.stuypulse.robot.constants.Settings.Elevator; public class ElevatorToBottom extends ElevatorToHeight { public ElevatorToBottom() { - super(Elevator.MIN_HEIGHT); + super(Elevator.MIN_HEIGHT_METERS); } } diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java index 0ce2a69..f8a6da0 100644 --- a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java @@ -1,6 +1,6 @@ -package com.stuypulse.robot.commands.Elevator; +package com.stuypulse.robot.commands.elevator; -import com.stuypulse.robot.subsystems.Elevator.Elevator; +import com.stuypulse.robot.subsystems.elevator.Elevator; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java index 4c2cfdf..6014eac 100644 --- a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java +++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java @@ -1,9 +1,9 @@ -package com.stuypulse.robot.commands.Elevator; +package com.stuypulse.robot.commands.elevator; import com.stuypulse.robot.constants.Settings.Elevator; public class ElevatorToLvl1 extends ElevatorToHeight{ public ElevatorToLvl1(){ - super(Elevator.L1); + super(Elevator.L1_HEIGHT_METERS); } } diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java index 5d825df..7fa1763 100644 --- a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java +++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java @@ -1,9 +1,9 @@ -package com.stuypulse.robot.commands.Elevator; +package com.stuypulse.robot.commands.elevator; import com.stuypulse.robot.constants.Settings.Elevator; public class ElevatorToLvl2 extends ElevatorToHeight { public ElevatorToLvl2(){ - super(Elevator.L2); + super(Elevator.L2_HEIGHT_METERS); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java index 2c5928c..9f6a3d6 100644 --- a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java +++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java @@ -1,9 +1,9 @@ -package com.stuypulse.robot.commands.Elevator; +package com.stuypulse.robot.commands.elevator; import com.stuypulse.robot.constants.Settings.Elevator; public class ElevatorToLvl3 extends ElevatorToHeight { public ElevatorToLvl3(){ - super(Elevator.L3); + super(Elevator.L3_HEIGHT_METERS); } } diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java index ce32054..8c3efb1 100644 --- a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java +++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java @@ -1,9 +1,9 @@ -package com.stuypulse.robot.commands.Elevator; +package com.stuypulse.robot.commands.elevator; import com.stuypulse.robot.constants.Settings.Elevator; public class ElevatorToLvl4 extends ElevatorToHeight{ public ElevatorToLvl4(){ - super(Elevator.L4); + super(Elevator.L4_HEIGHT_METERS); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireOver.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireOver.java new file mode 100644 index 0000000..9df4425 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireOver.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.subsystems.algae.Algae; + +import edu.wpi.first.wpilibj2.command.Command; + +public class AlgaeAcquireOver extends Command{ + + private final Algae algae; + + public AlgaeAcquireOver() { + this.algae = Algae.getInstance(); + addRequirements(algae); + } + + @Override + public void initialize() { + algae.acquireOver(); + } + + @Override + public boolean isFinished() { + return algae.hasAlgae(); + } + + @Override + public void end(boolean interrupted) { + algae.stopRollers(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireUnder.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireUnder.java new file mode 100644 index 0000000..5ef470a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireUnder.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.subsystems.algae.Algae; + +import edu.wpi.first.wpilibj2.command.Command; + +public class AlgaeAcquireUnder extends Command{ + + private final Algae algae; + + public AlgaeAcquireUnder() { + this.algae = Algae.getInstance(); + addRequirements(algae); + } + + @Override + public void initialize() { + algae.acquireUnder(); + } + + @Override + public boolean isFinished() { + return algae.hasAlgae(); + } + + @Override + public void end(boolean interrupted) { + algae.stopRollers(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetPivot.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireOver.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetPivot.java rename to src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireOver.java index aad7354..e4471ba 100644 --- a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetPivot.java +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireOver.java @@ -4,21 +4,18 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class AlgaeSetPivot extends InstantCommand { +public class AlgaeDeacquireOver extends InstantCommand { - protected final Algae algae; - private final double angle; //target angle + private final Algae algae; - public AlgaeSetPivot(double angle) { + public AlgaeDeacquireOver() { algae = Algae.getInstance(); - - this.angle = angle; addRequirements(algae); } @Override public void initialize() { - algae.setTargetAngle(angle); + algae.deacquireOver(); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireUnder.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireUnder.java new file mode 100644 index 0000000..d590bb3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireUnder.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.subsystems.algae.*; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AlgaeDeacquireUnder extends InstantCommand { + + private final Algae algae; + + public AlgaeDeacquireUnder() { + algae = Algae.getInstance(); + addRequirements(algae); + } + + @Override + public void initialize() { + algae.deacquireUnder(); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeGroundPickup.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeGroundPickup.java deleted file mode 100644 index f8888f8..0000000 --- a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeGroundPickup.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.stuypulse.robot.commands.algae; - -import com.stuypulse.robot.constants.Settings; - -public class AlgaeGroundPickup extends AlgaeSetPivot { - - public AlgaeGroundPickup(){ - super(Settings.Algae.GROUND_PICKUP_ANGLE); - algae.acquireUnder(); - } - public void initialize(){ - super.initialize(); - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL2.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL2.java deleted file mode 100644 index fd48511..0000000 --- a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL2.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.stuypulse.robot.commands.algae; - -import com.stuypulse.robot.constants.Settings; - -public class AlgaeL2 extends AlgaeSetPivot { - - public AlgaeL2(){ - super(Settings.Algae.L2_ANGLE); - algae.acquireUnder(); - } - - public void initialize(){ - super.initialize(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL3.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL3.java deleted file mode 100644 index d20a537..0000000 --- a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL3.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.stuypulse.robot.commands.algae; - -import com.stuypulse.robot.constants.Settings; - -public class AlgaeL3 extends AlgaeSetPivot { - - public AlgaeL3(){ - super(Settings.Algae.L3_ANGLE); - algae.acquireOver(); - } - - public void initialize(){ - super.initialize(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeProcessorScore.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeProcessorScore.java deleted file mode 100644 index b92b30f..0000000 --- a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeProcessorScore.java +++ /dev/null @@ -1,18 +0,0 @@ -package com.stuypulse.robot.commands.algae; - -import com.stuypulse.robot.constants.Settings; - - -public class AlgaeProcessorScore extends AlgaeSetPivot { - - public AlgaeProcessorScore(){ - super(Settings.Algae.PROCESSOR_ANGLE); - - algae.deacquireUnder(); - } - - public void initialize(){ - super.initialize(); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeReefKnockoff.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeReefKnockoff.java deleted file mode 100644 index 6029966..0000000 --- a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeReefKnockoff.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.stuypulse.robot.commands.algae; - -import com.stuypulse.robot.constants.Settings; - -public class AlgaeReefKnockoff extends AlgaeSetPivot { - - public AlgaeReefKnockoff(){ - super(Settings.Algae.REEF_KNOCKOFF_ANGLE); - } - - public void initialize(){ - super.initialize(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngle.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngle.java new file mode 100644 index 0000000..7871372 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngle.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.subsystems.algae.*; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AlgaeSetAngle extends InstantCommand { + + private final Algae algae; + private final Number targetAngle; + + public AlgaeSetAngle(Number targetAngle) { + algae = Algae.getInstance(); + + this.targetAngle = targetAngle; + addRequirements(algae); + } + + @Override + public void initialize() { + algae.setTargetAngle(targetAngle.doubleValue()); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleGroundPickup.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleGroundPickup.java new file mode 100644 index 0000000..bffdeda --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleGroundPickup.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeSetAngleGroundPickup extends AlgaeSetAngle { + + public AlgaeSetAngleGroundPickup() { + super(Settings.Algae.GROUND_PICKUP_ANGLE_DEGREES); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleL2.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleL2.java new file mode 100644 index 0000000..97fec16 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleL2.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeSetAngleL2 extends AlgaeSetAngle { + + public AlgaeSetAngleL2() { + super(Settings.Algae.L2_ANGLE_DEGREES); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleL3.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleL3.java new file mode 100644 index 0000000..4290fdc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleL3.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeSetAngleL3 extends AlgaeSetAngle { + + public AlgaeSetAngleL3() { + super(Settings.Algae.L3_ANGLE_DEGREES); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleProcessor.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleProcessor.java new file mode 100644 index 0000000..a7c6e45 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleProcessor.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeSetAngleProcessor extends AlgaeSetAngle { + + public AlgaeSetAngleProcessor() { + super(Settings.Algae.PROCESSOR_ANGLE_DEGREES); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleStow.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleStow.java new file mode 100644 index 0000000..1e26e37 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngleStow.java @@ -0,0 +1,8 @@ +package com.stuypulse.robot.commands.algae; + +public class AlgaeSetAngleStow extends AlgaeSetAngle { + + public AlgaeSetAngleStow() { + super(0); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStow.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStow.java deleted file mode 100644 index 49919f0..0000000 --- a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStow.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.stuypulse.robot.commands.algae; - -import com.stuypulse.robot.constants.Settings; - -public class AlgaeStow extends AlgaeSetPivot { - - public AlgaeStow() { - super(Settings.Algae.STOW_ANGLE); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelAcquire.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelAcquire.java new file mode 100644 index 0000000..afcf537 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelAcquire.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.funnel; + +import com.stuypulse.robot.subsystems.funnel.Funnel; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class FunnelAcquire extends InstantCommand{ + + private final Funnel funnel; + + public FunnelAcquire() { + this.funnel = Funnel.getInstance(); + addRequirements(funnel); + } + + @Override + public void initialize() { + funnel.acquire(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDeacquire.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDeacquire.java new file mode 100644 index 0000000..0e8af79 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelDeacquire.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.funnel; + +import com.stuypulse.robot.subsystems.funnel.Funnel; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class FunnelDeacquire extends InstantCommand{ + + private final Funnel funnel; + + public FunnelDeacquire() { + this.funnel = Funnel.getInstance(); + addRequirements(funnel); + } + + @Override + public void initialize() { + funnel.deacquire(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java new file mode 100644 index 0000000..c6b0df1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.funnel; + +import com.stuypulse.robot.subsystems.funnel.Funnel; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class FunnelStop extends InstantCommand{ + + private final Funnel funnel; + + public FunnelStop() { + this.funnel = Funnel.getInstance(); + addRequirements(funnel); + } + + @Override + public void initialize() { + funnel.stop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquire.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquire.java new file mode 100644 index 0000000..c789527 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquire.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ShooterAcquire extends Command{ + + private final Shooter shooter; + + public ShooterAcquire() { + this.shooter = Shooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.acquire(); + } + + @Override + public boolean isFinished() { + return shooter.hasCoral(); + } + + @Override + public void end(boolean interrupted) { + shooter.stop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index bc883b3..eee4bfa 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -5,9 +5,10 @@ package com.stuypulse.robot.constants; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; /*- * File containing all of the configurations that different motors require. @@ -19,67 +20,23 @@ * - The Open Loop Ramp Rate */ public interface Motors { - - /** Classes to store all of the values a motor needs */ - - public static class TalonSRXConfig { - public final boolean INVERTED; - public final NeutralMode NEUTRAL_MODE; - public final int PEAK_CURRENT_LIMIT_AMPS; - public final double OPEN_LOOP_RAMP_RATE; - - public TalonSRXConfig( - boolean inverted, - NeutralMode neutralMode, - int peakCurrentLimitAmps, - double openLoopRampRate) { - this.INVERTED = inverted; - this.NEUTRAL_MODE = neutralMode; - this.PEAK_CURRENT_LIMIT_AMPS = peakCurrentLimitAmps; - this.OPEN_LOOP_RAMP_RATE = openLoopRampRate; - } - - public TalonSRXConfig(boolean inverted, NeutralMode neutralMode, int peakCurrentLimitAmps) { - this(inverted, neutralMode, peakCurrentLimitAmps, 0.0); - } - - public TalonSRXConfig(boolean inverted, NeutralMode neutralMode) { - this(inverted, neutralMode, 80); - } - - public void configure(WPI_TalonSRX motor) { - motor.setInverted(INVERTED); - motor.setNeutralMode(NEUTRAL_MODE); - motor.configContinuousCurrentLimit(PEAK_CURRENT_LIMIT_AMPS - 10, 0); - motor.configPeakCurrentLimit(PEAK_CURRENT_LIMIT_AMPS, 0); - motor.configPeakCurrentDuration(100, 0); - motor.enableCurrentLimit(true); - motor.configOpenloopRamp(OPEN_LOOP_RAMP_RATE); - } + public interface Algae { + SparkBaseConfig pivotMotorConfig = new SparkMaxConfig().inverted(false).smartCurrentLimit(50).openLoopRampRate(0.5).idleMode(IdleMode.kBrake); + SparkBaseConfig rollerMotorConfig = new SparkMaxConfig().inverted(false).smartCurrentLimit(50).openLoopRampRate(0.5).idleMode(IdleMode.kBrake); } - public static class VictorSPXConfig { - public final boolean INVERTED; - public final NeutralMode NEUTRAL_MODE; - public final double OPEN_LOOP_RAMP_RATE; + public interface Elevator { + SparkBaseConfig leftMotor = new SparkMaxConfig().inverted(false).smartCurrentLimit(100).openLoopRampRate(0.5).idleMode(IdleMode.kBrake); + SparkBaseConfig rightMotor = new SparkMaxConfig().inverted(false).smartCurrentLimit(100).openLoopRampRate(0.5).idleMode(IdleMode.kBrake); - public VictorSPXConfig( - boolean inverted, - NeutralMode neutralMode, - double openLoopRampRate) { - this.INVERTED = inverted; - this.NEUTRAL_MODE = neutralMode; - this.OPEN_LOOP_RAMP_RATE = openLoopRampRate; - } + EncoderConfig encoderConfig = new EncoderConfig().positionConversionFactor(Settings.Elevator.Encoders.POSITION_CONVERSION_FACTOR).velocityConversionFactor(Settings.Elevator.Encoders.VELOCITY_CONVERSION_FACTOR); + } - public VictorSPXConfig(boolean inverted, NeutralMode neutralMode) { - this(inverted, neutralMode, 0.0); - } + public interface Shooter { + SparkBaseConfig motorConfig = new SparkMaxConfig().inverted(false).smartCurrentLimit(50).openLoopRampRate(0.5).idleMode(IdleMode.kBrake); + } - public void configure(WPI_VictorSPX motor) { - motor.setInverted(INVERTED); - motor.setNeutralMode(NEUTRAL_MODE); - motor.configOpenloopRamp(OPEN_LOOP_RAMP_RATE); - } + public interface Funnel { + SparkBaseConfig motorConfig = new SparkMaxConfig().inverted(false).smartCurrentLimit(50).openLoopRampRate(0.5).idleMode(IdleMode.kBrake); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 6a6b7d5..237a683 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -14,7 +14,8 @@ public interface Gamepad { } public interface Shooter { - int SHOOTER_MOTOR = 0; + int MOTOR = 0; + int IR_SENSOR = 1; } public interface Elevator { @@ -24,8 +25,13 @@ public interface Elevator { } public interface Algae { - int ROLLER_ID = 0; // update ports later -- def won't be 0 - int PIVOT_ID = 1; // update ports later + int ROLLER = 0; + int PIVOT = 1; + int ENCODER = 2; + } + + public interface Funnel { + int MOTOR = 0; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f2f39a8..b86073a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -9,63 +9,32 @@ import edu.wpi.first.math.util.Units; -/*- - * File containing tunable settings for every subsystem on the robot. - * - * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable - * values that we can edit on Shuffleboard. - */ public interface Settings { - // checks the current RIO's serial number to determine which robot is running - public enum RobotType { - - AUNT_MARY("0000000"), - SIM(""); - - public final String serialNum; - - RobotType(String serialNum) { - this.serialNum = serialNum; - } - - public static RobotType fromString(String serialNum) { - for (RobotType robot : RobotType.values()) { - if (robot.serialNum.equals(serialNum.toUpperCase())) { - return robot; - } - } - - return RobotType.SIM; - } - } - double DT = 0.020; - public interface Robot { - double kG = 100.0; - } + double WIDTH_WITH_BUMPERS_METERS = Units.inchesToMeters(30); // TODO: find width + double LENGTH_WITH_BUMPERS_METERS = Units.inchesToMeters(30); // TODO: find length public interface Elevator { - double MIN_HEIGHT = 0.0; - double MAX_HEIGHT = 12.0; - double MAX_ACCELERATION = 2.0; - double MAX_VELOCITY = 3.0; - double ENCODER_CONVERSION_FACTOR = 0; + double MIN_HEIGHT_METERS = 0.0; // FROM THE FLOOR TO TOP OF CARRIAGE + double MAX_HEIGHT_METERS = 12.0; // FROM THE FLOOR TO TOP OF CARRIAGE + double MAX_VELOCITY_METERS_PER_SECOND = 3.0; + double MAX_ACCELERATION_METERS_PER_SECOND = 2.0; double MASS = 25.0; double GEARING = 1.0/9.0; double DRUM_RADIUS = Units.inchesToMeters(1.0); - double L1 = 1; - double L2 = 2; - double L3 = 3; - double L4 = 4; - - double POSITION_CONVERSION_FACTOR = 1.0; - double VELOCITY_CONVERSION_FACTOR = 1.0; + double L1_HEIGHT_METERS = 0; + double L2_HEIGHT_METERS = 0.25; + double L3_HEIGHT_METERS = 0.5; + double L4_HEIGHT_METERS = 0.75; - double SCALE_FACTOR = 0.5; + public interface Encoders { + double POSITION_CONVERSION_FACTOR = 1.0; + double VELOCITY_CONVERSION_FACTOR = 1.0; + } public interface PID { SmartNumber kP = new SmartNumber("kP",1.5); @@ -79,33 +48,47 @@ public interface FF { SmartNumber kA = new SmartNumber("kA", 0.27); SmartNumber kG = new SmartNumber("kG", 0.37); } + + public interface Simulation { + double SCALE_FACTOR = 0.5; + } } public interface Algae { - double RAISED_ANGLE = 0.0; // CHANGE - double PROCESSOR_ANGLE = 0.0; // CHANGE - double REEF_KNOCKOFF_ANGLE = 0.0; // CHANGE - double GROUND_PICKUP_ANGLE = 0.0; // CHANGE - double L2_ANGLE = 0.0; // CHANGE - double L3_ANGLE = 0.0; // CHANGE - double STOW_ANGLE = 0.0; // CHANGE - double ACQUIRE_SPEED = 0.0; // CHANGE - double DEACQUIRE_SPEED = 0.0; // CHANGE + // 0 degrees is as far into the robot as the arm can go + SmartNumber GROUND_PICKUP_ANGLE_DEGREES = new SmartNumber("Algae Mech/Ground Pickup Angle (deg)", 0); + SmartNumber L2_ANGLE_DEGREES = new SmartNumber("Algae Mech/L2 Angle (deg)", 0); + SmartNumber L3_ANGLE_DEGREES = new SmartNumber("Algae Mech/L3 Angle (deg)", 0); + SmartNumber PROCESSOR_ANGLE_DEGREES = new SmartNumber("Algae Mech/Processor Angle (deg)", 0); + SmartNumber ACQUIRE_SPEED = new SmartNumber("Algae Mech/Acquire Speed", 0.5); + SmartNumber DEACQUIRE_SPEED = new SmartNumber("Algae Mech/Deacquire Speed", 0.5); + + double ENCODER_OFFSET_DEGREES = 0; + + double MAX_ANGULAR_VELOCITY_RAD_PER_SECOND = 3.0; + double MAX_ANGULAR_ACCEL_RAD_PER_SECOND_PER_SECOND = 3.0; public interface PID { - double kP = 0.0; - double kI = 0.0; - double kD = 0.0; - double MAX_VELOCITY = 0.0; - double MAX_ACCELERATION = 0.0; + SmartNumber kP = new SmartNumber("Algae Mech/PID/kP", 0.0); + SmartNumber kI = new SmartNumber("Algae Mech/PID/kI", 0.0); + SmartNumber kD = new SmartNumber("Algae Mech/PID/kD", 0.0); } - public interface FF{ - double kS = 0.0; - double kV = 0.0; - double kA = 0.0; - double kG = 0.0; + SmartNumber kS = new SmartNumber("Algae Mech/FF/kS", 0.0); + SmartNumber kV = new SmartNumber("Algae Mech/FF/kV", 0.0); + SmartNumber kA = new SmartNumber("Algae Mech/FF/kA", 0.0); + SmartNumber kG = new SmartNumber("Algae Mech/FF/kG", 0.0); } } + + public interface Shooter { + SmartNumber ACQUIRE_SPEED = new SmartNumber("Shooter/Acquire Speed", 0.2); + SmartNumber SHOOT_SPEED = new SmartNumber("Shooter/Shoot Speed", 0.5); + } + + public interface Funnel { + SmartNumber ACQUIRE_SPEED = new SmartNumber("Funnel/Acquire Speed", 0.4); + SmartNumber DEACQUIRE_SPEED = new SmartNumber("Funnel/Deacquire Speed", 0.4); + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java index e25af01..a76c7f3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java @@ -1,7 +1,6 @@ -package com.stuypulse.robot.subsystems.Elevator; +package com.stuypulse.robot.subsystems.elevator; import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Settings.RobotType; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -10,7 +9,7 @@ public abstract class Elevator extends SubsystemBase { private static final Elevator instance; static { - if (Robot.ROBOT == RobotType.AUNT_MARY) { + if (Robot.isReal()) { instance = new ElevatorImpl(); } else { instance = new ElevatorSimu(); @@ -28,14 +27,8 @@ public Elevator() { } public abstract void setTargetHeight(double height); - public abstract double getTargetHeight(); - - public abstract double getHeight(); - - public abstract void stopElevator(); - - public abstract boolean atBottom(); + public abstract double getCurrentHeight(); public void periodic() { visualizer.update(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorImpl.java index 31573ae..23c1662 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorImpl.java @@ -1,17 +1,21 @@ -package com.stuypulse.robot.subsystems.Elevator; +package com.stuypulse.robot.subsystems.elevator; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -26,49 +30,34 @@ public class ElevatorImpl extends Elevator { private final SmartNumber targetHeight; - private final SparkMaxConfig leftConfig; - private final SparkMaxConfig rightConfig; + private final Controller controller; - private ElevatorFeedforward FF; - private ProfiledPIDController PID; + private boolean hasBeenReset; public ElevatorImpl() { leftMotor = new SparkMax(Ports.Elevator.LEFT, MotorType.kBrushless); - rightMotor = new SparkMax(Ports.Elevator.RIGHT, MotorType.kBrushless); - - rightConfig = new SparkMaxConfig(); - leftConfig = new SparkMaxConfig(); - - bumpSwitch = new DigitalInput(Ports.Elevator.SWITCH); - - targetHeight = new SmartNumber("Elevator/Target Height", 0); + Motors.Elevator.leftMotor.encoder.apply(Motors.Elevator.encoderConfig); + leftMotor.configure(Motors.Elevator.leftMotor, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - rightConfig.encoder.positionConversionFactor(Settings.Elevator.POSITION_CONVERSION_FACTOR); - leftConfig.encoder.positionConversionFactor(Settings.Elevator.POSITION_CONVERSION_FACTOR); - rightConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); - leftConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); + rightMotor = new SparkMax(Ports.Elevator.RIGHT, MotorType.kBrushless); + Motors.Elevator.rightMotor.encoder.apply(Motors.Elevator.encoderConfig); + rightMotor.configure(Motors.Elevator.rightMotor, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); leftEncoder = leftMotor.getEncoder(); rightEncoder = rightMotor.getEncoder(); - rightMotor.configure(leftConfig, SparkMax.ResetMode.kResetSafeParameters, SparkMax.PersistMode.kPersistParameters); - leftMotor.configure(rightConfig, SparkMax.ResetMode.kResetSafeParameters, SparkMax.PersistMode.kPersistParameters); + bumpSwitch = new DigitalInput(Ports.Elevator.SWITCH); - FF = new ElevatorFeedforward( - Settings.Elevator.FF.kS.getAsDouble(), - Settings.Elevator.FF.kG.getAsDouble(), - Settings.Elevator.FF.kV.getAsDouble() - ); + targetHeight = new SmartNumber("Elevator/Target Height", Settings.Elevator.MIN_HEIGHT_METERS); - PID = new ProfiledPIDController( - Settings.Elevator.PID.kP.getAsDouble(), - Settings.Elevator.PID.kI.getAsDouble(), - Settings.Elevator.PID.kD.getAsDouble(), - new TrapezoidProfile.Constraints( - Settings.Elevator.MAX_ACCELERATION, - Settings.Elevator.MAX_VELOCITY - ) - ); + MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCELERATION_METERS_PER_SECOND); + + controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() + .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) + .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) + .setSetpointFilter(motionProfile); + + hasBeenReset = false; } @Override @@ -76,8 +65,8 @@ public void setTargetHeight(double height) { targetHeight.set( SLMath.clamp( height, - Settings.Elevator.MIN_HEIGHT, - Settings.Elevator.MAX_HEIGHT + Settings.Elevator.MIN_HEIGHT_METERS, + Settings.Elevator.MAX_HEIGHT_METERS ) ); } @@ -88,40 +77,34 @@ public double getTargetHeight() { } @Override - public double getHeight() { + public double getCurrentHeight() { return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2.0; } - @Override - public void stopElevator() { - rightMotor.stopMotor(); - leftMotor.stopMotor(); - } - @Override - public boolean atBottom() { - return !bumpSwitch.get(); - } - - public void reset() { - leftEncoder.setPosition(0); - rightEncoder.setPosition(0); + private void setVoltage(double voltage) { + leftMotor.setVoltage(voltage); + rightMotor.setVoltage(voltage); } @Override public void periodic() { super.periodic(); - final double PIDOutput = PID.calculate(getHeight(), targetHeight.doubleValue()); - final double FFOutput = FF.calculate(PID.getSetpoint().velocity); - - if (atBottom() && (PIDOutput + FFOutput) < 0) { - stopElevator(); - reset(); - } else { - leftMotor.setVoltage(PIDOutput + FFOutput); - rightMotor.setVoltage(PIDOutput + FFOutput); + + if (bumpSwitch.get()) { + hasBeenReset = true; + leftMotor.getEncoder().setPosition(Settings.Elevator.MIN_HEIGHT_METERS); + rightMotor.getEncoder().setPosition(Settings.Elevator.MIN_HEIGHT_METERS); + } + + if (!hasBeenReset) { + setVoltage(-1); + } + else { + controller.update(getTargetHeight(), getCurrentHeight()); + setVoltage(controller.getOutput()); } SmartDashboard.putNumber("Elevator/Target Height", targetHeight.getAsDouble()); - SmartDashboard.putNumber("Elevator/Height", getHeight()); + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorSimu.java index 34f241d..54fd4e1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorSimu.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorSimu.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.subsystems.Elevator; +package com.stuypulse.robot.subsystems.elevator; import java.util.Optional; @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; - public class ElevatorSimu extends Elevator { private final ElevatorSim sim; private final SmartNumber targetHeight; @@ -37,10 +36,10 @@ public class ElevatorSimu extends Elevator { ElevatorSimu() { targetHeight = new SmartNumber("Elevator/Target Height", 0); - minHeight = Settings.Elevator.MIN_HEIGHT; - maxHeight = Settings.Elevator.MAX_HEIGHT; - maxAccel = new SmartNumber("Elevator/Max Acceleration",Settings.Elevator.MAX_ACCELERATION); - maxVel = new SmartNumber("Elevator/Max Velocity", Settings.Elevator.MAX_VELOCITY); + minHeight = Settings.Elevator.MIN_HEIGHT_METERS; + maxHeight = Settings.Elevator.MAX_HEIGHT_METERS; + maxAccel = new SmartNumber("Elevator/Max Acceleration",Settings.Elevator.MAX_ACCELERATION_METERS_PER_SECOND); + maxVel = new SmartNumber("Elevator/Max Velocity", Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND); gearbox = DCMotor.getNEO(2); motor = new PWMSparkMax(3); @@ -57,8 +56,8 @@ public class ElevatorSimu extends Elevator { Settings.Elevator.GEARING, Settings.Elevator.MASS, Settings.Elevator.DRUM_RADIUS, - Settings.Elevator.MIN_HEIGHT, - Settings.Elevator.MAX_HEIGHT, + Settings.Elevator.MIN_HEIGHT_METERS, + Settings.Elevator.MAX_HEIGHT_METERS, true, 0.0 ); @@ -76,7 +75,6 @@ public class ElevatorSimu extends Elevator { voltageOverride = Optional.empty(); - // encoder.setDistancePerPulse(256); } @@ -98,7 +96,6 @@ public double getTargetHeight() { return targetHeight.doubleValue(); } - @Override public boolean atBottom() { return sim.hasHitLowerLimit(); } @@ -108,25 +105,28 @@ public boolean elevatorTop() { } @Override - public double getHeight() { + public double getCurrentHeight() { return sim.getPositionMeters(); } - @Override public void stopElevator() { sim.setInputVoltage(0.0); } + public void elevatorIdleMode(IdleMode mode) {} + public void setVoltageOverride(double voltage) { voltageOverride = Optional.of(voltage); } + public void setConstraints(double maxVelocity, double maxAcceleration) { this.maxVel.set(maxVelocity); this.maxAccel.set(maxAcceleration); } + public double calculateVoltage() { final double FFOutput = FF.calculate(encoder.getDistance()); - final double PIDOutput = PID.calculate(getHeight(), targetHeight.doubleValue()); + final double PIDOutput = PID.calculate(getCurrentHeight(), targetHeight.doubleValue()); // Combine outputs and set motor voltage System.out.println("ff: " + FFOutput); System.out.println("pid: " + PIDOutput); @@ -149,7 +149,7 @@ public void periodic() { SmartDashboard.putNumber("Elevator/Target Height", getTargetHeight()); SmartDashboard.putNumber("Elevator/Current", sim.getCurrentDrawAmps()); - SmartDashboard.putNumber("Elevator/Height", getHeight()); + SmartDashboard.putNumber("Elevator/Height", getCurrentHeight()); } public void simulationPeriodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorVisualizer.java index 129cc7c..c6b4619 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorVisualizer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorVisualizer.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.subsystems.Elevator; +package com.stuypulse.robot.subsystems.elevator; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; @@ -158,11 +158,11 @@ public void update() { ElevatorSimu simu = ((ElevatorSimu) ElevatorSimu.getInstance()); System.out.println("distance: " + simu.getSim().getPositionMeters()); - innerBL.setPosition(2, simu.getHeight() + 2); - innerTR.setPosition(4, simu.getHeight() + 4); + innerBL.setPosition(2, simu.getCurrentHeight() + 2); + innerTR.setPosition(4, simu.getCurrentHeight() + 4); - outerBL.setPosition(1, 1 + simu.getHeight() * Settings.Elevator.SCALE_FACTOR); - outerTR.setPosition(5, 15 + simu.getHeight() * Settings.Elevator.SCALE_FACTOR); + outerBL.setPosition(1, 1 + simu.getCurrentHeight() * Settings.Elevator.Simulation.SCALE_FACTOR); + outerTR.setPosition(5, 15 + simu.getCurrentHeight() * Settings.Elevator.Simulation.SCALE_FACTOR); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/algae/Algae.java b/src/main/java/com/stuypulse/robot/subsystems/algae/Algae.java index f0240c1..55730c6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/algae/Algae.java +++ b/src/main/java/com/stuypulse/robot/subsystems/algae/Algae.java @@ -1,8 +1,5 @@ package com.stuypulse.robot.subsystems.algae; -import com.revrobotics.spark.SparkMax; -import com.stuypulse.robot.Robot; - import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Algae extends SubsystemBase { @@ -13,28 +10,22 @@ public abstract class Algae extends SubsystemBase { instance = new AlgaeImpl(); } - public Algae() { - - } - public static Algae getInstance() { return instance; } public abstract double getTargetAngle(); - public abstract double getCurrentAngle(); + public abstract void setTargetAngle(double targetAngle); - public abstract void acquireUnder(); + public abstract boolean hasAlgae(); + public abstract void acquireUnder(); public abstract void acquireOver(); public abstract void deacquireUnder(); - public abstract void deacquireOver(); public abstract void stopRollers(); - - public abstract void setTargetAngle(double targetAngle); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeImpl.java index b0ebe2a..b8def9a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeImpl.java @@ -1,128 +1,111 @@ package com.stuypulse.robot.subsystems.algae; -import com.revrobotics.RelativeEncoder; +import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.ArmFeedForward; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class AlgaeImpl extends Algae { - // variable declaration private SparkMax pivotMotor; private SparkMax rollerMotor; + private CANcoder pivotEncoder; - private RelativeEncoder pivotEncoder; + private Controller pivotController; - private ProfiledPIDController pivotPIDController; - private ArmFeedforward pivotFFController; + private SmartNumber targetAngle; private final AlgaeVisualizer visualizer; - private double targetAngle; - - private Constraints constraints; - - //private int rollerState; // -1 is deacquire, 1 is acquire/intake, 0 is not moving - - // constructor - public AlgaeImpl() { - pivotMotor = new SparkMax(com.stuypulse.robot.constants.Ports.Algae.PIVOT_ID, MotorType.kBrushless); - rollerMotor = new SparkMax(com.stuypulse.robot.constants.Ports.Algae.ROLLER_ID, MotorType.kBrushless); - + pivotMotor = new SparkMax(com.stuypulse.robot.constants.Ports.Algae.PIVOT, MotorType.kBrushless); + pivotMotor.configure(Motors.Algae.pivotMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - pivotEncoder = pivotMotor.getEncoder(); + rollerMotor = new SparkMax(com.stuypulse.robot.constants.Ports.Algae.ROLLER, MotorType.kBrushless); + rollerMotor.configure(Motors.Algae.rollerMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + pivotEncoder = new CANcoder(Ports.Algae.ENCODER); - constraints = new Constraints(Settings.Algae.PID.MAX_VELOCITY, Settings.Algae.PID.MAX_ACCELERATION); + MotionProfile motionProfile = new MotionProfile(Settings.Algae.MAX_ANGULAR_VELOCITY_RAD_PER_SECOND, Settings.Algae.MAX_ANGULAR_ACCEL_RAD_PER_SECOND_PER_SECOND); + + pivotController = new MotorFeedforward(Settings.Algae.FF.kS, Settings.Algae.FF.kV, Settings.Algae.FF.kA).position() + .add(new ArmFeedForward(Settings.Algae.FF.kG)) + .add(new PIDController(Settings.Algae.PID.kP, Settings.Algae.PID.kI, Settings.Algae.PID.kD)) + .setSetpointFilter(motionProfile); - pivotPIDController = new ProfiledPIDController(Settings.Algae.PID.kP, Settings.Algae.PID.kI, Settings.Algae.PID.kD, constraints); - pivotFFController = new ArmFeedforward(Settings.Algae.FF.kS, Settings.Algae.FF.kG, Settings.Algae.FF.kV, Settings.Algae.FF.kA); - targetAngle = 0; + targetAngle = new SmartNumber("Algae Mech/Target Angle", 0); visualizer = new AlgaeVisualizer(); } - // pivot + @Override public double getTargetAngle() { - return targetAngle; + return targetAngle.get(); } + @Override public double getCurrentAngle() { - // return Units.rotationsToDegrees(pivotEncoder.getPosition()); - return 135; + return Units.rotationsToDegrees(pivotEncoder.getPosition().getValueAsDouble()) - Settings.Algae.ENCODER_OFFSET_DEGREES; } - // setters - - // rollers - public void acquireUnder() { // only rollers - GROUND AND L2 - rollerMotor.set(Settings.Algae.ACQUIRE_SPEED); + @Override + public void setTargetAngle(double angle) { + this.targetAngle.set(angle); + } + + @Override + public void acquireUnder() { + rollerMotor.set(Settings.Algae.ACQUIRE_SPEED.get()); } - public void acquireOver() { // only rollers - L3 - rollerMotor.set(-Settings.Algae.ACQUIRE_SPEED); - // not sure if this should be negative or positive yet - // but it shoudl be opposite of Under - // Do we rename acquire speed to smth else - // Should probably rename aquire speed to smth else + @Override + public void acquireOver() { + rollerMotor.set(-Settings.Algae.ACQUIRE_SPEED.get()); } - public void deacquireUnder() { // only rollers - GROUND AND L2 - rollerMotor.set(Settings.Algae.DEACQUIRE_SPEED); + @Override + public void deacquireUnder() { + rollerMotor.set(-Settings.Algae.DEACQUIRE_SPEED.get()); } - public void deacquireOver() { // L3 - rollerMotor.set(-Settings.Algae.DEACQUIRE_SPEED); - // not sure if this should be negative or positive yet - // but it should be opposite of Under + @Override + public void deacquireOver() { + rollerMotor.set(Settings.Algae.DEACQUIRE_SPEED.get()); } + @Override public void stopRollers() { rollerMotor.set(0); } - - // pivot - - public void setTargetAngle(double targetAngle) { - this.targetAngle = targetAngle; - } - private double getCurrentPivotVelocity(){ - return pivotEncoder.getVelocity(); + @Override + public boolean hasAlgae() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'hasAlgae'"); } - // periodic - @Override public void periodic() { - - pivotMotor.setVoltage( - pivotPIDController.calculate( - getCurrentAngle(), - getTargetAngle() - ) - + - pivotFFController.calculate( - Math.toRadians(getTargetAngle()), - getCurrentPivotVelocity() - ) - ); + pivotController.update(getTargetAngle(), getCurrentAngle()); + pivotMotor.setVoltage(pivotController.getOutput()); - - // visualizer updating stuff - visualizer.update(); visualizer.updateBarAngle(); visualizer.updatePivotAngle(); - - + // SmartDashboard.putBoolean("Algae Mech/Has Algae", hasAlgae()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/Funnel.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/Funnel.java new file mode 100644 index 0000000..8e187ad --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/Funnel.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.subsystems.funnel; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Funnel extends SubsystemBase{ + + private static final Funnel instance; + + static { + instance = new FunnelImpl(); + } + + public static Funnel getInstance() { + return instance; + } + + public abstract void acquire(); + public abstract void deacquire(); + public abstract void stop(); +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/FunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/FunnelImpl.java new file mode 100644 index 0000000..46c89f7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/FunnelImpl.java @@ -0,0 +1,41 @@ +package com.stuypulse.robot.subsystems.funnel; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class FunnelImpl extends Funnel{ + + private SparkMax motor; + + public FunnelImpl() { + motor = new SparkMax(Ports.Funnel.MOTOR, MotorType.kBrushless); + motor.configure(Motors.Funnel.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void acquire() { + motor.set(Settings.Funnel.ACQUIRE_SPEED.get()); + } + + @Override + public void deacquire() { + motor.set(-Settings.Funnel.DEACQUIRE_SPEED.get()); + } + + @Override + public void stop() { + motor.set(0); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Funnel/Speed", motor.get()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 751ba72..5e37861 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -1,36 +1,21 @@ package com.stuypulse.robot.subsystems.shooter; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.stuypulse.robot.constants.Ports; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Shooter extends SubsystemBase { +public abstract class Shooter extends SubsystemBase { + private static final Shooter instance; static { - instance = new Shooter(); + instance = new ShooterImpl(); } public static Shooter getInstance() { return instance; } - private final SparkMax shooterMotor; - - public Shooter(){ - super(); - shooterMotor = new SparkMax(Ports.Shooter.SHOOTER_MOTOR, MotorType.kBrushless); - } - - public void shoot() { - shooterMotor.set(1); - } - - public void stop(){ - shooterMotor.set(0); - } + public abstract void acquire(); + public abstract void shoot(); + public abstract void stop(); - @Override - public void periodic() { - } + public abstract boolean hasCoral(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java new file mode 100644 index 0000000..c82bc12 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -0,0 +1,50 @@ +package com.stuypulse.robot.subsystems.shooter; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ShooterImpl extends Shooter{ + + private final SparkMax motor; + private final DigitalInput IR_Sensor; + + public ShooterImpl() { + motor = new SparkMax(Ports.Shooter.MOTOR, MotorType.kBrushless); + motor.configure(Motors.Shooter.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + IR_Sensor = new DigitalInput(Ports.Shooter.IR_SENSOR); + } + + @Override + public void acquire() { + motor.set(Settings.Shooter.ACQUIRE_SPEED.get()); + } + + @Override + public void shoot() { + motor.set(Settings.Shooter.SHOOT_SPEED.get()); + } + + @Override + public void stop() { + motor.set(0); + } + + @Override + public boolean hasCoral() { + return IR_Sensor.get(); + } + + @Override + public void periodic() { + SmartDashboard.putBoolean("Shooter/Has Coral", hasCoral()); + } +} diff --git a/src/main/java/com/stuypulse/robot/util/ArmFeedForward.java b/src/main/java/com/stuypulse/robot/util/ArmFeedForward.java new file mode 100644 index 0000000..ff494b7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/ArmFeedForward.java @@ -0,0 +1,42 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.control.Controller; + +/** + * A feedforward term to account for gravity for motorized arms that can move continuously (if not + * use `ArmFeedforward`) + * + *

The motor feedforward used in the context of an arm will not account for gravity that is + * acting on the arm. + * + *

Can be paired with MotorFeedforward or other controllers with .add + * + * @author Benjamin Irving Goldfisher XXII (myles.pasetsky@gmail.com) + */ + +public class ArmFeedForward extends Controller { + + /** voltage to hold arm horizontal */ + private final Number kG; + + /** + * Create arm feedforward + * + * @param kG term to hold arm vertical against gravity (volts) + */ + public ArmFeedForward(Number kG) { + this.kG = kG; + } + + /** + * Calculates voltage to hold arm at the setpoint angle + * + * @param setpoint setpoint + * @param measurement measurement + * @return kG * cos(setpoint) + */ + @Override + protected double calculate(double setpoint, double measurement) { + return kG.doubleValue() * Math.cos(measurement); + } +} diff --git a/src/main/java/com/stuypulse/robot/util/ElevatorFeedForward.java b/src/main/java/com/stuypulse/robot/util/ElevatorFeedForward.java new file mode 100644 index 0000000..6e83411 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/ElevatorFeedForward.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.control.Controller; + +/** + * A feedforward term to account for gravity for elevators + */ + +public class ElevatorFeedForward extends Controller { + + private final Number kG; + + public ElevatorFeedForward(Number kG) { + this.kG = kG; + } + + @Override + protected double calculate(double setpoint, double measurement) { + return kG.doubleValue(); + } +}