generated from StuyPulse/Phil
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #8 from StuyPulse/automations
Automations
- Loading branch information
Showing
26 changed files
with
334 additions
and
438 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
13 changes: 0 additions & 13 deletions
13
src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl1.java
This file was deleted.
Oops, something went wrong.
20 changes: 0 additions & 20 deletions
20
src/main/java/com/stuypulse/robot/commands/funnel/FunnelAcquire.java
This file was deleted.
Oops, something went wrong.
20 changes: 0 additions & 20 deletions
20
src/main/java/com/stuypulse/robot/commands/funnel/FunnelDeacquire.java
This file was deleted.
Oops, something went wrong.
29 changes: 29 additions & 0 deletions
29
src/main/java/com/stuypulse/robot/commands/funnel/FunnelDefaultCommand.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,29 @@ | ||
| package com.stuypulse.robot.commands.funnel; | ||
|
|
||
| import com.stuypulse.robot.subsystems.funnel.Funnel; | ||
| import com.stuypulse.robot.subsystems.shooter.Shooter; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
|
|
||
| public class FunnelDefaultCommand extends Command{ | ||
|
|
||
| private final Funnel funnel; | ||
|
|
||
| public FunnelDefaultCommand() { | ||
| this.funnel = Funnel.getInstance(); | ||
| addRequirements(funnel); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| if (Shooter.getInstance().hasCoral()) { | ||
| funnel.stop(); | ||
| } | ||
| else if (funnel.shouldReverse()) { | ||
| funnel.reverse(); | ||
| } | ||
| else { | ||
| funnel.acquire(); | ||
| } | ||
| } | ||
| } |
20 changes: 0 additions & 20 deletions
20
src/main/java/com/stuypulse/robot/commands/funnel/FunnelStop.java
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
64 changes: 64 additions & 0 deletions
64
src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,64 @@ | ||
| package com.stuypulse.robot.commands.swerve; | ||
|
|
||
| import java.util.function.Supplier; | ||
|
|
||
| import com.stuypulse.robot.constants.Settings.Driver.Drive; | ||
| import com.stuypulse.robot.constants.Settings.Swerve.Alignment; | ||
| import com.stuypulse.robot.subsystems.odometry.Odometry; | ||
| import com.stuypulse.robot.subsystems.swerve.SwerveDrive; | ||
| import com.stuypulse.stuylib.control.angle.AngleController; | ||
| import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; | ||
| import com.stuypulse.stuylib.input.Gamepad; | ||
| import com.stuypulse.stuylib.math.Angle; | ||
| import com.stuypulse.stuylib.streams.vectors.VStream; | ||
| import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; | ||
| import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; | ||
| import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
|
|
||
| public class SwerveDriveDriveAligned extends Command { | ||
|
|
||
| private final SwerveDrive swerve; | ||
| private final VStream drive; | ||
| private final Odometry odometry; | ||
|
|
||
| private final AngleController controller; | ||
|
|
||
| private final Supplier<Rotation2d> targetAngle; | ||
|
|
||
| public SwerveDriveDriveAligned(Gamepad driver, Supplier<Rotation2d> targetAngle) { | ||
| swerve = SwerveDrive.getInstance(); | ||
| odometry = Odometry.getInstance(); | ||
|
|
||
| this.targetAngle = targetAngle; | ||
|
|
||
| drive = VStream.create(driver::getLeftStick) | ||
| .filtered( | ||
| new VDeadZone(Drive.DEADBAND), | ||
| x -> x.clamp(1), | ||
| x -> x.pow(Drive.POWER.get()), | ||
| x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), | ||
| new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), | ||
| new VLowPassFilter(Drive.RC.get())); | ||
|
|
||
| controller = new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) | ||
| .setOutputFilter(x -> -x); | ||
|
|
||
| addRequirements(swerve); | ||
| } | ||
|
|
||
| public SwerveDriveDriveAligned(Gamepad driver, Rotation2d targetAngle) { | ||
| this(driver, () -> targetAngle); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| swerve.drive( | ||
| drive.get(), | ||
| controller.update( | ||
| Angle.fromRotation2d(targetAngle.get()), | ||
| Angle.fromRotation2d(odometry.getPose().getRotation()))); | ||
| } | ||
| } |
11 changes: 11 additions & 0 deletions
11
...ava/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedToNearestCoralStation.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,11 @@ | ||
| package com.stuypulse.robot.commands.swerve; | ||
|
|
||
| import com.stuypulse.robot.constants.Field; | ||
| import com.stuypulse.stuylib.input.Gamepad; | ||
|
|
||
| public class SwerveDriveDriveAlignedToNearestCoralStation extends SwerveDriveDriveAligned{ | ||
|
|
||
| public SwerveDriveDriveAlignedToNearestCoralStation(Gamepad driver) { | ||
| super(driver, () -> Field.getClosestCoralStationTargetPose().getRotation()); | ||
| } | ||
| } |
9 changes: 9 additions & 0 deletions
9
src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranch.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,9 @@ | ||
| package com.stuypulse.robot.commands.swerve; | ||
|
|
||
| import com.stuypulse.robot.constants.Field; | ||
|
|
||
| public class SwerveDrivePIDToNearestBranch extends SwerveDrivePIDToPose{ | ||
| public SwerveDrivePIDToNearestBranch() { | ||
| super(() -> Field.getClosestBranch().getTargetPose()); | ||
| } | ||
| } |
Oops, something went wrong.