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

Merge branch 'master' of https://github.com/Team4388/2026KPopRobotHunters

Browse Source
This commit is contained in:
C4llSqin
2026-01-13 18:19:47 -07:00
parent 5e86771288 6280e58215
commit 24fdd610c9
13 changed files with 117 additions and 323 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java
-48
View File
@@ -1,48 +0,0 @@
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class DriveUntilLiDAR extends Command {
private final SwerveDrive swerveDrive;
private final Translation2d leftStick;
private final Translation2d rightStick;
private final LiDAR m_lidar;
private final double mindistance;
public DriveUntilLiDAR(
SwerveDrive swerveDrive,
Translation2d leftStick,
Translation2d rightStick,
LiDAR lidar,
double mindistance) {
addRequirements(swerveDrive);
this.swerveDrive = swerveDrive;
this.leftStick = leftStick;
this.rightStick = rightStick;
this.m_lidar = lidar;
this.mindistance = mindistance;
}
@Override
public void initialize() {
}
@Override
public void execute() {
swerveDrive.driveFine(leftStick, rightStick, 0.3);
}
@Override
public boolean isFinished() {
if (Math.abs(m_lidar.getDistance()) < mindistance) {
swerveDrive.softStop();
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/alignment/LidarAlign.java
-107
View File
@@ -1,107 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.lidar.LiDAR;
import frc4388.robot.subsystems.swerve.SwerveDrive;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class LidarAlign extends Command {
private SwerveDrive swerveDrive;
private LiDAR lidar;
private int currentFinderTick;
// private int tickFoundPipe;
private boolean foundReef;
private boolean headedRight;
private double speed;
private int bounces;
private double additionalDistance = 0;
// private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */
public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerveDrive = swerveDrive;
this.lidar = lidar;
addRequirements(swerveDrive, lidar);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.currentFinderTick = 0;
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
//this.headedRight = (DriveToReef.tagRelativeXError < 0);
this.additionalDistance = 0;
this.bounces = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (lidar.withinDistance()) {
swerveDrive.softStop();
foundReef = true;
return;
}
if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
headedRight = !headedRight;
currentFinderTick *= -1;
bounces++;
additionalDistance += 5;
if (bounces == 5) return;
}
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
SmartDashboard.putNumber("Rel Angle", relAngle);
SmartDashboard.putNumber("heading", currentHeading);
if (!headedRight) {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
} else {
swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
}
currentFinderTick++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (lidar.getDistance() < 4) {
swerveDrive.stopModules();
return true;
} else if (foundReef && lidar.withinDistance()) { // spot on
swerveDrive.stopModules();
return true;
} else if (foundReef && !lidar.withinDistance()) { // over shot
speed = speed / 2;
headedRight = !headedRight;
currentFinderTick = 0;
foundReef = false;
return false;
} else if (bounces >= 3) {
swerveDrive.stopModules();
return true;
} else {
return false;
}
}
}