generated from StuyPulse/Phil
-
Notifications
You must be signed in to change notification settings - Fork 7
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Add SwerveDriveToFerry * Make amp motion uninterruptable (very scuffed and maybe has unintended consequences) * Move amp score in slightly for lab * Fix constant x ferry command * Added AmpScoreThenStop to AmpScoreRoutine * Add ferry cutoff --------- Co-authored-by: Prog694 <prog692@gmail.com> Co-authored-by: Kalimul Kaif <kalimul.kaif@stuypulse.com>
- Loading branch information
3 people
authored
May 31, 2024
1 parent
9d87418
commit 1eae6ec
Showing
9 changed files
with
167 additions
and
7 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
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
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
37 changes: 37 additions & 0 deletions
37
src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.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,37 @@ | ||
|
|
||
| package com.stuypulse.robot.commands.amper; | ||
|
|
||
| import com.stuypulse.robot.subsystems.amper.Amper; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.InstantCommand; | ||
|
|
||
| public class AmperScoreThenStop extends InstantCommand { | ||
|
|
||
| private final Amper amper; | ||
|
|
||
| public static Command scoreThenStop() { | ||
| return new AmperScoreThenStop(); | ||
| } | ||
|
|
||
| public AmperScoreThenStop() { | ||
| amper = Amper.getInstance(); | ||
| addRequirements(amper); | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| amper.amp(); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return !amper.hasNote(); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| amper.stopRoller(); | ||
| } | ||
|
|
||
| } |
65 changes: 65 additions & 0 deletions
65
src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.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,65 @@ | ||
| /************************ PROJECT IZZI *************************/ | ||
| /* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ | ||
| /* Use of this source code is governed by an MIT-style license */ | ||
| /* that can be found in the repository LICENSE file. */ | ||
| /***************************************************************/ | ||
|
|
||
| package com.stuypulse.robot.commands.conveyor; | ||
|
|
||
| import com.stuypulse.robot.commands.amper.AmperToHeight; | ||
| import com.stuypulse.robot.constants.Settings; | ||
| import com.stuypulse.robot.constants.Settings.Amper.Lift; | ||
| import com.stuypulse.robot.subsystems.amper.Amper; | ||
| import com.stuypulse.robot.subsystems.conveyor.Conveyor; | ||
| import com.stuypulse.robot.subsystems.intake.Intake; | ||
| import com.stuypulse.robot.subsystems.shooter.Shooter; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
|
|
||
| public class ConveyorToAmpUnsafe extends Command { | ||
| public static Command withCheckLift() { | ||
| return AmperToHeight.untilBelow(Lift.MIN_HEIGHT, Lift.MAX_HEIGHT_ERROR) | ||
| .andThen(new ConveyorToAmp()); | ||
| } | ||
|
|
||
| private final Conveyor conveyor; | ||
| private final Shooter shooter; | ||
| private final Intake intake; | ||
| private final Amper amper; | ||
|
|
||
| public ConveyorToAmpUnsafe() { | ||
| conveyor = Conveyor.getInstance(); | ||
| shooter = Shooter.getInstance(); | ||
| intake = Intake.getInstance(); | ||
| amper = Amper.getInstance(); | ||
|
|
||
| addRequirements(conveyor, intake); | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| shooter.setTargetSpeeds(Settings.Shooter.HANDOFF); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| if (shooter.atTargetSpeeds()) { | ||
| conveyor.toAmp(); | ||
| intake.acquire(); | ||
| amper.fromConveyor(); | ||
| } | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return amper.hasNote(); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| conveyor.stop(); | ||
| // shooter.stop(); | ||
| intake.stop(); | ||
| amper.stopRoller(); | ||
| } | ||
| } |
41 changes: 41 additions & 0 deletions
41
src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.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,41 @@ | ||
| /************************ PROJECT IZZI *************************/ | ||
| /* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ | ||
| /* Use of this source code is governed by an MIT-style license */ | ||
| /* that can be found in the repository LICENSE file. */ | ||
| /***************************************************************/ | ||
|
|
||
| package com.stuypulse.robot.commands.swerve; | ||
|
|
||
|
|
||
| import com.stuypulse.robot.Robot; | ||
| import com.stuypulse.robot.constants.Field; | ||
| import com.stuypulse.robot.subsystems.odometry.Odometry; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
|
|
||
| public class SwerveDriveToFerry extends SwerveDriveToPose { | ||
|
|
||
| public SwerveDriveToFerry() { | ||
| super(() -> { | ||
| Pose2d robot = Odometry.getInstance().getPose(); | ||
|
|
||
| Translation2d target = Robot.isBlue() | ||
| ? new Translation2d(0, Field.WIDTH - 1.5) | ||
| : new Translation2d(0, 1.5); | ||
|
|
||
| Rotation2d targetAngle = new Translation2d(Field.CONST_FERRY_X, robot.getY()) | ||
| .minus(target) | ||
| .getAngle(); | ||
|
|
||
| // use robot's y, put in x and rotation | ||
|
|
||
| return new Pose2d( | ||
| Field.CONST_FERRY_X, | ||
| Robot.isBlue() ? Math.max(robot.getY(), Field.FERRY_CUTOFF) : Math.min(robot.getY(), Field.WIDTH - Field.FERRY_CUTOFF), | ||
| targetAngle | ||
| ); | ||
| }); | ||
| } | ||
| } |
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
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
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