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 // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
RobotIntakeDown, RobotIntakeDown,
new WaitCommand(1), new WaitCommand(1),
new RunCommand( new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
() -> m_robotSwerveDrive.driveWithInput( new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true)
m_lidar.getClosestBall(),
new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), // new RunCommand(
false // () -> m_robotSwerveDrive.driveWithInput(
), // m_lidar.getClosestBall(),
m_robotSwerveDrive // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
) // false
.withTimeout(5.0) // ),
.andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) // m_robotSwerveDrive
// )
// .withTimeout(5.0)
// .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
); );
private Command RobotReadyToShoot = new SequentialCommandGroup( private Command RobotReadyToShoot = new SequentialCommandGroup(
@@ -122,7 +125,7 @@ public class RobotContainer {
private Command RobotShoot = new SequentialCommandGroup( private Command RobotShoot = new SequentialCommandGroup(
// TEST NEW AUTO ALIGN // 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.setShooterShoot(), m_robotShooter),
new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter),
new WaitCommand(3), new WaitCommand(3),
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+5 -6
View File
@@ -15,14 +15,15 @@ public class AutoAlign extends Command {
private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos; private Pose2d targetpos;
private boolean isLidar;
SwerveDrive swerveDrive; SwerveDrive swerveDrive;
Vision vision; Vision vision;
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) {
this.swerveDrive = swerveDrive; this.swerveDrive = swerveDrive;
this.vision = vision; this.vision = vision;
this.targetpos = targetpos; this.targetpos = targetpos;
this.isLidar = isLidar;
addRequirements(swerveDrive); addRequirements(swerveDrive);
} }
@@ -39,8 +40,6 @@ public class AutoAlign extends Command {
@Override @Override
public void execute() { public void execute() {
Pose2d robotPose = vision.getPose2d(); Pose2d robotPose = vision.getPose2d();
if (robotPose == null) return; if (robotPose == null) return;
@@ -54,9 +53,9 @@ public class AutoAlign extends Command {
if (roterr < -180) roterr += 360; if (roterr < -180) roterr += 360;
SmartDashboard.putNumber("PID Rot Error", roterr); SmartDashboard.putNumber("PID Rot Error", roterr);
System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr);
swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading); swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading);
} }
@Override @Override
public final boolean isFinished() { public final boolean isFinished() {
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = ""; public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified"; public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 87; public static final int GIT_REVISION = 91;
public static final String GIT_SHA = "34a7ed050d2967603ebf12cd0bad0b7264aeae01"; public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac";
public static final String GIT_DATE = "2026-02-24 17:49:26 MST"; public static final String GIT_DATE = "2026-02-25 17:17:52 MST";
public static final String GIT_BRANCH = "AutoTesting"; public static final String GIT_BRANCH = "AutoTesting";
public static final String BUILD_DATE = "2026-02-24 18:48:38 MST"; public static final String BUILD_DATE = "2026-02-25 17:44:49 MST";
public static final long BUILD_UNIX_TIME = 1771984118729L; public static final long BUILD_UNIX_TIME = 1772066689466L;
public static final int DIRTY = 1; public static final int DIRTY = 1;
private BuildConstants(){} private BuildConstants(){}