Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

updated limealign and drivetolimedistance

Browse Source
This commit is contained in:
aarav18
2023-03-16 23:08:08 -06:00
parent 94a092b7b5
commit 5ae5db7847
5 changed files with 50 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+8 -11
View File
@@ -92,21 +92,18 @@ public class RobotContainer {
private ConditionalCommand alignToPole = new ConditionalCommand(
new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight),
new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30)
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw()),
new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToTape())
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)),
emptyCommand.asProxy(),
() -> m_robotLimeLight.numTargets() <= 2
() -> m_robotLimeLight.getNumTapes() <= 2
);
// TODO: make
private ConditionalCommand alignToShelf = new ConditionalCommand(
new SequentialCommandGroup(
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)),
emptyCommand.asProxy(),
() -> true
);
private SequentialCommandGroup alignToShelf = new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw()),
new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToApril()) // TODO: find distance
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
public SequentialCommandGroup place = null;