Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Lidar Align

Browse Source
This commit is contained in:
Shikhar
2026-02-25 20:21:47 -07:00
parent 3745cc2b18
commit f9174c23cd
5 changed files with 58 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/Robot Reveal Night.auto
+6
View File
@@ -10,6 +10,12 @@
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "Intake Retracted"
}
},
{
"type": "path",
"data": {
src/main/java/frc4388/robot/RobotContainer.java
+11 -6
View File
@@ -101,10 +101,10 @@ public class RobotContainer {
private Command LidarIntake = new SequentialCommandGroup(
// 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 InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true)
// RobotIntakeDown,
// new WaitCommand(1),
// new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
// new RunCommand(
// () -> m_robotSwerveDrive.driveWithInput(
@@ -123,14 +123,18 @@ public class RobotContainer {
new InstantCommand(() -> m_robotShooter.setShooterReady(), m_robotShooter)
);
private Command IntakeRetracted = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
);
private Command RobotShoot = new SequentialCommandGroup(
// TEST NEW AUTO ALIGN
//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),
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake),
new WaitCommand(2),
IntakeRetracted,
new WaitCommand(3),
new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter),
new InstantCommand(() -> m_robotShooter.setShooterNotReady(), m_robotShooter)
);
@@ -155,6 +159,7 @@ public class RobotContainer {
}, true);
NamedCommands.registerCommand("Robot Rev Up", RobotReadyToShoot);
NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
NamedCommands.registerCommand("Lidar Intake", LidarIntake);
NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown);
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+34 -21
View File
@@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.compute.FieldPositions;
@@ -14,15 +15,19 @@ import frc4388.utility.structs.Gains;
public class AutoAlign extends Command {
private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
private Lidar lidar;
private boolean isLidar;
private Pose2d targetpos;
private double targetRotation;
SwerveDrive swerveDrive;
Vision vision;
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) {
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Lidar lidar, boolean isLidar) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.targetpos = targetpos;
this.lidar = lidar;
this.isLidar = isLidar;
addRequirements(swerveDrive);
}
@@ -30,6 +35,7 @@ public class AutoAlign extends Command {
@Override
public void initialize() {
rotPID.initialize();
this.targetRotation = swerveDrive.getPose2d().getRotation().getDegrees();
//this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0));
}
@@ -40,36 +46,43 @@ public class AutoAlign extends Command {
@Override
public void execute() {
Rotation2d desiredHeading;
Pose2d robotPose = vision.getPose2d();
if (robotPose == null) return;
double desiredHeading;
// Pose2d robotPose = vision.getPose2d();
targetpos = new Pose2d(lidar.getClosestBall(), new Rotation2d(0));
// if (robotPose == null) return;
if (targetpos == null) return;
if (targetpos.getTranslation() == null) return;
double dx = targetpos.getX() - robotPose.getX();
double dy = targetpos.getY() - robotPose.getY();
double dx = targetpos.getX();// - robotPose.getX();
double dy = targetpos.getY();// - robotPose.getY();
if (!isLidar){
desiredHeading = new Rotation2d(Math.atan2(dy, dx)+ Math.PI);
desiredHeading = (Math.atan2(dy, dx)+ Math.PI) * (180. / Math.PI) + 180;
}else{
desiredHeading = new Rotation2d(Math.atan2(dx, dy));
desiredHeading = (Math.atan2(dx, dy)) * (180. / Math.PI);// + 180;
}
roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
if (roterr > 180) roterr -= 360;
if (roterr < -180) roterr += 360;
targetRotation = swerveDrive.getPose2d().getRotation().getDegrees() - desiredHeading;
SmartDashboard.putNumber("PID Rot Error", roterr);
System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr);
swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading);
// roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
// if (roterr > 180) roterr -= 360;
// if (roterr < -180) roterr += 360;
SmartDashboard.putNumber("Target Rotation!", targetRotation);
// System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr);
swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(targetRotation));
}
@Override
public final boolean isFinished() {
boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE;
if (finished) {
swerveDrive.softStop();
}
return finished;
// boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE;
// if (finished) {
// swerveDrive.softStop();
// }
// return finished;
return false;
}
private class PID {
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 = 91;
public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac";
public static final String GIT_DATE = "2026-02-25 17:17:52 MST";
public static final int GIT_REVISION = 96;
public static final String GIT_SHA = "3745cc2b1869e5850c93507277539c7cfed606c1";
public static final String GIT_DATE = "2026-02-25 18:59:11 MST";
public static final String GIT_BRANCH = "AutoTesting";
public static final String BUILD_DATE = "2026-02-25 17:44:49 MST";
public static final long BUILD_UNIX_TIME = 1772066689466L;
public static final String BUILD_DATE = "2026-02-25 20:19:51 MST";
public static final long BUILD_UNIX_TIME = 1772075991136L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+2 -2
View File
@@ -45,13 +45,13 @@ public class IntakeConstants {
public static final Slot0Configs ARM_PID = new Slot0Configs()
.withKP(0.1)
.withKP(0.03)
.withKI(0.0)
.withKD(0.0);
public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.05);
public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.03);
public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);