Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Lidar Test

Browse Source
This commit is contained in:
SHikhar
2026-02-25 17:45:22 -07:00
parent b3b32b00fe
commit 4ca55e0225
3 changed files with 24 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+14 -11
View File
@@ -103,16 +103,19 @@ public class RobotContainer {
// Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
RobotIntakeDown,
new WaitCommand(1),
new RunCommand(
() -> m_robotSwerveDrive.driveWithInput(
m_lidar.getClosestBall(),
new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
false
),
m_robotSwerveDrive
)
.withTimeout(5.0)
.andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true)
// new RunCommand(
// () -> m_robotSwerveDrive.driveWithInput(
// m_lidar.getClosestBall(),
// new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
// false
// ),
// m_robotSwerveDrive
// )
// .withTimeout(5.0)
// .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
);
private Command RobotReadyToShoot = new SequentialCommandGroup(
@@ -122,7 +125,7 @@ public class RobotContainer {
private Command RobotShoot = new SequentialCommandGroup(
// TEST NEW AUTO ALIGN
new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0))),
new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter),
new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter),
new WaitCommand(3),