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),
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 Pose2d targetpos;
private boolean isLidar;
SwerveDrive swerveDrive;
Vision vision;
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) {
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.targetpos = targetpos;
this.isLidar = isLidar;
addRequirements(swerveDrive);
}
@@ -39,8 +40,6 @@ public class AutoAlign extends Command {
@Override
public void execute() {
Pose2d robotPose = vision.getPose2d();
if (robotPose == null) return;
@@ -54,9 +53,9 @@ public class AutoAlign extends Command {
if (roterr < -180) roterr += 360;
SmartDashboard.putNumber("PID Rot Error", roterr);
System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr);
swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading);
}
}
@Override
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_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 87;
public static final String GIT_SHA = "34a7ed050d2967603ebf12cd0bad0b7264aeae01";
public static final String GIT_DATE = "2026-02-24 17:49:26 MST";
public static final int GIT_REVISION = 91;
public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac";
public static final String GIT_DATE = "2026-02-25 17:17:52 MST";
public static final String GIT_BRANCH = "AutoTesting";
public static final String BUILD_DATE = "2026-02-24 18:48:38 MST";
public static final long BUILD_UNIX_TIME = 1771984118729L;
public static final String BUILD_DATE = "2026-02-25 17:44:49 MST";
public static final long BUILD_UNIX_TIME = 1772066689466L;
public static final int DIRTY = 1;
private BuildConstants(){}