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

Merge branch 'shikhar-op-controls' of https://github.com/Team4388/2026KPopRobotHunters into shikhar-op-controls

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-27 18:57:00 -08:00
parent 1b1b0ac130 e299d3bd0a
commit 90c1f6c0fa
16 changed files with 1084 additions and 319 deletions
Download Patch File Download Diff File Expand all files Collapse all files
build.gradle
+3
View File
@@ -75,6 +75,9 @@ dependencies {
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
implementation("com.fazecast:jSerialComm:2.4.1")
def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
}
src/main/deploy/pathplanner/autos/Lidar Intake.auto
+19
View File
@@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lidar Intake"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Quick Shoot test.auto → src/main/deploy/pathplanner/autos/Robot Reveal Night.auto
+7 -1
View File
@@ -7,7 +7,13 @@
{
"type": "named",
"data": {
"name": "Robot Intake Down"
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "Intake Retracted"
}
},
{
src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto
+37
View File
@@ -0,0 +1,37 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Robot Rev Up"
}
},
{
"type": "path",
"data": {
"pathName": "Quick Shoot"
}
},
{
"type": "path",
"data": {
"pathName": "Test - Robot Align"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/Quick Shoot.path
+9 -25
View File
@@ -3,41 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 3.605428571428572,
"y": 5.107378917378918
"x": 3.6,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 2.6073542083177803,
"y": 5.107378917378918
"x": 3.35,
"y": 4.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.2557261904761914,
"y": 4.801630952380952
"x": 2.5,
"y": 4.0
},
"prevControl": {
"x": 2.5688571428571443,
"y": 5.4170952380952375
},
"nextControl": {
"x": 1.848588583599833,
"y": 4.00139496645156
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.2557261904761914,
"y": 4.035
},
"prevControl": {
"x": 2.2665238095238105,
"y": 4.304940476190477
"x": 2.75,
"y": 4.0
},
"nextControl": null,
"isLocked": false,
@@ -64,7 +48,7 @@
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 178.40885972880554
"rotation": 180.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Test - Robot Align.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.5,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 2.526567580029336,
"y": 4.248584319077823
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.5,
"y": 4.7
},
"prevControl": {
"x": 2.5175324328959037,
"y": 4.450615530161258
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 0.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}
src/main/java/frc4388/robot/RobotContainer.java
+47 -36
View File
@@ -11,7 +11,6 @@ import java.io.File;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -29,12 +28,13 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.alignment.AutoAlign;
import frc4388.robot.constants.Constants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
// Subsystems
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.shooter.Shooter;
@@ -65,6 +65,7 @@ public class RobotContainer {
// public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
/* Subsystems */
// public final Lidar m_lidar = new Lidar();
public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
//Testing of Colors
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
@@ -95,43 +96,50 @@ public class RobotContainer {
private Command RobotIntakeDown = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended))
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
);
// 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, m_lidar, 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(
new InstantCommand(()-> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake),
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter)
);
private Command IntakeRetracted = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
);
private Command RobotShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spinUpShooting()),
new RunCommand(
() -> {
m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldPositions.HUB_POSITION,
ShooterConstants.AIM_LEAD_TIME.get()
);
}, m_robotSwerveDrive),
new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)),
new WaitCommand(5),
new InstantCommand(()->m_robotShooter.allowShooting()),
new WaitCommand(10),
new InstantCommand(()->m_robotShooter.denyShooting())
// TEST NEW AUTO ALIGN
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
new InstantCommand(()-> m_robotShooter.spinUpShooting(), m_robotShooter),
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
new WaitCommand(2),
IntakeRetracted,
new WaitCommand(3),
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter),
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter)
);
// private Command RobotShoot = new SequentialCommandGroup(
// new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
// new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED),
// new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
// new WaitCommand(5),
// new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED),
// new InstantCommand(() -> System.out.println(m_robotLED.getMode()))
// );
// private Command RobotShoot = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter)
// );
// private Command RobotIntake = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake)
// );
public RobotContainer() {
configureButtonBindings();
@@ -150,7 +158,10 @@ public class RobotContainer {
m_robotShooter.io.updateGains();
}, 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);
@@ -364,7 +375,7 @@ public class RobotContainer {
autoChooser.onChange((filename) -> {
autoChooserUpdated = true;
// if (filename.equals("Taxi")) {
// if (filename.equals("Taxi%")) {
// autoCommand = new SequentialCommandGroup(
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0, -1),
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+119
View File
@@ -0,0 +1,119 @@
package frc4388.robot.commands.alignment;
import edu.wpi.first.math.geometry.Pose2d;
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.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;
import frc4388.utility.structs.Gains;
public class AutoAlign extends Command {
private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Lidar lidar;
private boolean isLidar;
private Pose2d targetpos;
private double targetRotation;
SwerveDrive swerveDrive;
Vision vision;
public AutoAlign(SwerveDrive swerveDrive, Vision vision, Lidar lidar, boolean isLidar) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.lidar = lidar;
this.isLidar = isLidar;
addRequirements(swerveDrive);
}
@Override
public void initialize() {
rotPID.initialize();
this.targetRotation = swerveDrive.getPose2d().getRotation().getDegrees();
//this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0));
}
double roterr;
double rotoutput;
@Override
public void execute() {
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();
if (!isLidar){
desiredHeading = (Math.atan2(dy, dx)+ Math.PI) * (180. / Math.PI) + 180;
}else{
desiredHeading = (Math.atan2(dx, dy)) * (180. / Math.PI);// + 180;
}
targetRotation = swerveDrive.getPose2d().getRotation().getDegrees() - 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;
return false;
}
private class PID {
protected Gains gains;
private double output = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(Gains gains, double tolerance) {
this.gains = gains;
}
// Called when the command is initially scheduled.
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
public double update(double error) {
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
return output;
}
}
}
src/main/java/frc4388/robot/constants/BuildConstants.java
+1 -1
View File
@@ -5,7 +5,7 @@ package frc4388.robot.constants;
*/
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 89;
public static final String GIT_SHA = "2ed8cbb8cb6070b2ac8a442fcbba1bbcab1bca11";
src/main/java/frc4388/robot/constants/Constants.java
+2 -2
View File
@@ -47,7 +47,7 @@ public final class Constants {
public static final class AutoConstants {
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
public static final Gains XY_GAINS = new Gains(8,0,0.0);
public static final Gains ROT_GAINS = new Gains(8,0,0.0);
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
@@ -61,7 +61,7 @@ public final class Constants {
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
public static final double XY_TOLERANCE = 0.07; // Meters
public static final double ROT_TOLERANCE = 5; // Degrees
public static final double ROT_TOLERANCE = 10; // Degrees
public static final double MIN_XY_PID_OUTPUT = 0.0;
public static final double MIN_ROT_PID_OUTPUT = 0.0;
src/main/java/frc4388/robot/subsystems/Lidar.java
+313
View File
@@ -0,0 +1,313 @@
package frc4388.robot.subsystems;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import java.util.Queue;
import org.littletonrobotics.junction.AutoLogOutput;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.highgui.HighGui;
import org.opencv.imgproc.Imgproc;
import edu.wpi.first.cscore.OpenCvLoader;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.subsystems.RPLidarA1.PolarPoint;
import frc4388.robot.subsystems.RPLidarA1.ScanListener;
import frc4388.utility.configurable.ConfigurableDouble;
import frc4388.utility.status.FaultA1M8;
public class Lidar extends SubsystemBase implements ScanListener {
// private final Spark m_motor;
private final RPLidarA1 lidar;
private ConfigurableDouble speed = new ConfigurableDouble("LiDAR speed", 0.2);
static
{
// This is so libopencv_javaVERSION.so (where version is the 3-digit opencv
// version) gets loaded.
try {
OpenCvLoader.forceLoad();
}
catch (Exception e) {
e.printStackTrace();
}
}
// private static final double m_Scan = 0.1;
public Lidar() {
// Spark motor = new Spark(0);
// this.m_motor = motor;
this.lidar = new RPLidarA1();
this.lidar.setListener(this);
// Thread processThread = new Thread(this::pointLoop);
// processThread.setDaemon(true);
// processThread.setName("RPLidar-Calc");
// processThread.start();
FaultA1M8.addDevice(lidar, "A1M8");
}
public Rotation2d getLatestBallAngle() {
return latestBallAngleDeg;
}
public boolean outOfBounds(Translation2d closestBall){
// Make sure robot doesn't go off the earth
return true;
}
@Override
public void periodic() {
this.lidar.setSpeed(speed.get());
SmartDashboard.putString("lidar state", this.lidar.getStatus().toString());
}
// Detection constriants: cluster detection
private static final double ANG_MAX_GAP = 3.; // Degrees
private static final double DIST_MAX_GAP = 0.04; // Meters
// Detection constraints: Circle detection
private static final double RADIUS_X_COEFF = Units.inchesToMeters(0);
private static final double RADIUS_Y_COEFF = Units.inchesToMeters(0);
private static final double RADIUS_OFFSET = Units.inchesToMeters(3);
private static final double RADIUS_TOLERANCE = Units.inchesToMeters(3);
private static boolean radiusInTolerance(double x, double y, double radius) {
double rad_at_position = RADIUS_X_COEFF*x + RADIUS_Y_COEFF*y + RADIUS_OFFSET;
return Math.abs(rad_at_position - radius) <= RADIUS_TOLERANCE;
}
// Window constants
private static final int WIDTH = 512;
private static final int HEIGHT = 512;
private static final int POINT_RAD = 2;
Translation2d closestBall;
Translation2d closestBallPrior = null;
@AutoLogOutput
public Translation2d getClosestBall() {
return closestBall;
}
private List<Point> point_group = new ArrayList<>();
private double last_ang = 0;
private double last_dist = 0;
private Rotation2d latestBallAngleDeg= new Rotation2d();
private boolean last_color = false;
Point LIDAR = new Point(WIDTH/2,WIDTH/2);
@Override
public void onScanComplete(List<PolarPoint> scan) {
System.out.println("SCAN: " + scan.size());
double scale = 0.009;
List<Translation2d> circlePoints = new ArrayList<>();
// Mat mat = Mat.zeros(WIDTH, HEIGHT, CvType.CV_8UC3);
for(PolarPoint point_polar : scan) {
if(!(point_polar.angle < 30 || point_polar.angle > 330)) {
continue;
}
double ang_rad = Math.toRadians(point_polar.angle);
double x = point_polar.distance * Math.cos(ang_rad);
double y = point_polar.distance * Math.sin(ang_rad);
// Point point_xy = new Point((WIDTH/2) + (x/scale), (HEIGHT/2) + (y/scale));
Point point_xy = new Point(x, y);
if(
Math.abs(last_ang - point_polar.angle) > ANG_MAX_GAP ||
Math.abs(last_dist - point_polar.distance) > DIST_MAX_GAP
) {
last_color = !last_color;
if (
point_group.size() >= 3
// point_group.size() <= POINT_MAX.get()
) {
// Get points
Point p1 = point_group.get(0);
Point p2 = point_group.get(point_group.size()/2);
Point p3 = point_group.get(point_group.size()-1);
// Simplify to var
double dx23 = p2.x - p3.x;
double dy23 = p2.y - p3.y;
double dx13 = p1.x - p3.x;
double dy13 = p1.y - p3.y;
double dx12 = p1.x - p2.x;
double dy12 = p1.y - p2.y;
// Calc Determinite
double D = p1.x*dy23 - p1.y*dx23 + (p2.x*p3.y - p3.x*p2.y);
// The points are in a straight line.
if(D == 0) {
continue;
}
// Square distances between each set of 2 points
double a_sq = dx23*dx23 + dy23*dy23;
double b_sq = dx13*dx13 + dy13*dy13;
double c_sq = dx12*dx12 + dy12*dy12;
// Calculate the radius
double radius = Math.sqrt(a_sq*b_sq*c_sq) / (2 * Math.abs(D));
// Square distances between each point and origin
double d1 = p1.x*p1.x + p1.y*p1.y;
double d2 = p2.x*p2.x + p2.y*p2.y;
double d3 = p3.x*p3.x + p3.y*p3.y;
// Calculate X and Y
double cx = (d1*dy23 - d2*dy13 + d3*dy12)/(2*D);
double cy = -(d1*dx23 - d2*dx13 + d3*dx12)/(2*D);
if(radiusInTolerance(cx, cy, radius)) {
circlePoints.add(new Translation2d(cx, cy));
}
// FitResult result = TaubinCircleFitter.fit(point_group, Lidar::getRadius);
// if(result.rmsError < ERROR_BOUND.get()) {
// circlePoints.add(result.center);
// Imgproc.circle(mat, result.center, (int) (result.radius/scale), new Scalar(255,255,255));
// }
}
point_group.clear();
}
point_group.add(point_xy);
last_ang = point_polar.angle;
last_dist = point_polar.distance;
// Point scaledPoint = new Point((WIDTH/2) + (point_xy.x / scale), (WIDTH/2) + (point_xy.y / scale));
// Imgproc.circle(mat, scaledPoint, POINT_RAD, new Scalar(127,127,127));
}
// for(Translation2d circle : circlePoints) {
// Point scaledPoint = new Point( (WIDTH/2) + (circle.getX() / scale), (WIDTH/2) + (circle.getY() / scale));
// Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(0,255,255));
// // System.out.println(circle.x + " - " + circle.y);
// }
closestBall = new Translation2d();
if (circlePoints.isEmpty()) {
closestBall = new Translation2d(Double.NaN, Double.NaN);
} else {
double minDist = Double.POSITIVE_INFINITY;
Translation2d best = null;
for (Translation2d circle : circlePoints) {
double dist = circle.getSquaredNorm(); // distance from 0,0
if (dist < minDist) {
minDist = dist;
best = circle;
}
}
closestBall = best;
}
if (closestBallPrior != null) {
if (closestBall.getDistance(closestBallPrior) < 0.1 && outOfBounds(closestBall)){
// Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale));
// Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180);
} else {
// Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getY() / scale));
// Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
}
}
closestBallPrior = closestBall;
// Imgproc.circle(mat, LIDAR, (int) (RADIUS_OFFSET / scale), new Scalar(255,255,255), -1);
// System.o
// showWindow(mat);
}
private static void showWindow(Mat img) {
// Display the image in a window titled "Original Image"
HighGui.imshow("Original Image", img);
// Wait for a key press to close the window
HighGui.waitKey(1);
}
// XYZ Position of the LiDAR on the robot
private static final Translation2d LiDAR_POS = new Translation2d(1, 0);
// Angle of the lidar unit
private static final double LiDAR_PITCH = 0; // Radians
private static final double LiDAR_ROLL = 0; // Radians
// Convert a LiDAR ball position to a field position
public static Translation2d lidarPosToField(Translation2d p, Pose2d pose) {
// Project the point tilted plane on to the XY plane
// Point should be relative to the XY plane, with (0,0) centered at the centerpoint of the lidar
double x = p.getX() * Math.cos(LiDAR_ROLL) + p.getY() * Math.sin(LiDAR_PITCH) * Math.sin(LiDAR_ROLL);
double y = p.getY() * Math.cos(LiDAR_PITCH);
// Translate the ball position to relative to the center of the robot
// Point should be relative to robot, wth (0,0) centered at center of robot
x -= LiDAR_POS.getX();
y -= LiDAR_POS.getY();
// Rotate the point by the robot's rotation
// Point should now be relative to robot, but rotated relative to the field.
double ang = -pose.getRotation().getRadians();
x = x*Math.cos(ang) - y*Math.sin(ang);
y = x*Math.sin(ang) + y*Math.cos(ang);
// Translate the point to the robot's field position
// Point should be relative to field. (0,0) should be relative to the field.
x += pose.getX();
y += pose.getY();
return new Translation2d(x, y);
}
}
src/main/java/frc4388/robot/subsystems/RPLidarA1.java
+431
View File
@@ -0,0 +1,431 @@
package frc4388.robot.subsystems;
import com.fazecast.jSerialComm.SerialPort;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.io.InputStream;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.concurrent.atomic.AtomicReference;
/**
* Robust RPLidar A1 / R1M8 Driver for FRC.
* Implements standard protocol with auto-reconnection and state monitoring.
*/
public class RPLidarA1 {
// --- Data Types ---
public static class PolarPoint {
public final double angle; // Degrees 0-360
public final double distance; // Meters
public PolarPoint(double angle, double distance) {
this.angle = angle;
this.distance = distance;
}
}
@FunctionalInterface
public interface ScanListener {
void onScanComplete(List<PolarPoint> scan);
}
public enum ConnectionStatus {
DISCONNECTED, // Port not found or closed
CONNECTING, // Attempting to open serial port
CONNECTED_IDLE, // Port open, but scan not started / no data yet
CONNECTED_DISABLED,// Robot is disabled, but sensor is connected
RECEIVING_DATA, // Actively receiving valid scan points
ERROR // Communication failure or timeout
}
// --- Protocol Constants ---
private static final byte SYNC_BYTE = (byte) 0xA5;
private static final byte SYNC_BYTE2 = (byte) 0x5A;
private static final byte CMD_STOP = (byte) 0x25;
private static final byte CMD_RESET = (byte) 0x40;
private static final byte CMD_SCAN = (byte) 0x20;
private static final byte CMD_GET_HEALTH = (byte) 0x52;
private static final int DESCRIPTOR_LEN = 7;
private static final int SCAN_PACKET_LEN = 5;
// --- Settings ---
private static final String PORT_DESC = "CP2102 USB to UART Bridge Controller";
private static final double WATCHDOG_TIMEOUT = 2.5; // Seconds before assuming link is dead
// --- Members ---
private final AtomicReference<ConnectionStatus> mStatus = new AtomicReference<>(ConnectionStatus.DISCONNECTED);
private SerialPort mSerialPort;
private InputStream mIn;
private OutputStream mOut;
private ScanListener mListener;
private final List<PolarPoint> mCurrentScan = new ArrayList<>();
private double mLastDataTimestamp = 0;
// private boolean mScanningActive = false;
public RPLidarA1() {
Thread driverThread = new Thread(this::runLoop);
driverThread.setDaemon(true);
driverThread.setName("RPLidar-Driver-Thread");
driverThread.start();
Thread pwmThread = new Thread(this::funnyDTR_PWM);
pwmThread.setDaemon(true);
pwmThread.setName("RPLidar-Driver-PWM");
pwmThread.start();
}
/** Sets the function to call whenever a full 360-degree rotation is parsed. */
public void setListener(ScanListener listener) {
this.mListener = listener;
}
// Set Speed of motor between 0 - 1
public void setSpeed(double speed) {
this.motor_percentage = speed;
}
public ConnectionStatus getStatus() {
return mStatus.get();
}
/** Signals the Lidar to stop the motor and laser. */
private void stop_motor() {
sendCmd(CMD_RESET);
Timer.delay(0.02);
sendCmd(CMD_STOP);
mSerialPort.setDTR();
}
private final static double TOGGLE_DELAY = 10;
private double motor_percentage = 0.5;
private boolean is_dtr = false;
// Control the speed of the motor like a PWM through the DTR serial pin
// This is "PWM", like we control the speed through the percentage.
// The rate of toggles is the resolution
private void funnyDTR_PWM() {
while (!Thread.interrupted()) {
try {
ConnectionStatus status = mStatus.get();
if (status == ConnectionStatus.RECEIVING_DATA) {
// If the motor is at full speed
if (motor_percentage >= 1) {
// Set the motor to on
mSerialPort.clearDTR();
// check again in a little bit
Thread.sleep(100);
}
// If the motor is at zero speed
if (motor_percentage <= 0) {
// Set the motor to on
mSerialPort.setDTR();
// check again in a little bit
Thread.sleep(100);
}
if (is_dtr) {
mSerialPort.clearDTR();
// Sleep for main part of motor pulse
Thread.sleep((long) (TOGGLE_DELAY * motor_percentage));
} else {
mSerialPort.setDTR();
// Sleep for gap of motor pulse
Thread.sleep((long) (TOGGLE_DELAY * (1 - motor_percentage)));
}
is_dtr = !is_dtr;
} else if(status == ConnectionStatus.CONNECTED_DISABLED) {
// Stop the motor
mSerialPort.setDTR();
// Sleep until we can check again
Thread.sleep(100);
} else { // When the motor is not ready
// Sleep until we can check again
Thread.sleep(100);
}
} catch (Exception e) {
continue;
}
}
}
private void runLoop() {
while (!Thread.interrupted()) {
try {
ConnectionStatus current = mStatus.get();
boolean robotEnabled = DriverStation.isEnabled();
switch (current) {
case DISCONNECTED:
case ERROR:
attemptConnection();
break;
case CONNECTING:
// Handled by attemptConnection
break;
case CONNECTED_DISABLED:
if (robotEnabled) {
mStatus.set(ConnectionStatus.CONNECTED_IDLE);
// On enable, set the last data time to now to avoid watchdog error
mLastDataTimestamp = Timer.getFPGATimestamp();
break;
}
// We have to check the health seperately because
// the connection check only ever occurs when
// the robot is recieving data
if (!getHealth()) {
mStatus.set(ConnectionStatus.ERROR);
}
break;
case CONNECTED_IDLE:
if (!robotEnabled) {
mStatus.set(ConnectionStatus.CONNECTED_DISABLED);
// On enable, set the last data time to now to avoid watchdog error
mLastDataTimestamp = Timer.getFPGATimestamp();
break;
}
if (initiateScanMode()) {
mStatus.set(ConnectionStatus.RECEIVING_DATA);
mLastDataTimestamp = Timer.getFPGATimestamp();
} else {
mStatus.set(ConnectionStatus.ERROR);
}
break;
case RECEIVING_DATA:
if (!robotEnabled) {
mStatus.set(ConnectionStatus.CONNECTED_DISABLED);
break;
}
processIncomingData();
checkWatchdog();
break;
}
Thread.sleep(200);
} catch (Exception e) {
continue;
}
}
}
private void attemptConnection() {
if (mSerialPort != null && mSerialPort.isOpen()) {
mSerialPort.closePort();
}
mStatus.set(ConnectionStatus.CONNECTING);
SerialPort[] ports = SerialPort.getCommPorts();
for (SerialPort p : ports) {
if (p.getPortDescription().contains(PORT_DESC)) {
mSerialPort = p;
break;
}
}
if (mSerialPort != null) {
mSerialPort.setComPortParameters(115200, 8, SerialPort.ONE_STOP_BIT, SerialPort.NO_PARITY);
mSerialPort.setFlowControl(SerialPort.FLOW_CONTROL_DISABLED);
if (mSerialPort.openPort()) {
mIn = mSerialPort.getInputStream();
mOut = mSerialPort.getOutputStream();
if (DriverStation.isEnabled()) {
mStatus.set(ConnectionStatus.CONNECTED_IDLE);
// On start, set the last data time to now to avoid watchdog error
mLastDataTimestamp = Timer.getFPGATimestamp();
} else {
mStatus.set(ConnectionStatus.CONNECTED_DISABLED);
stop_motor();
}
mStatus.set(ConnectionStatus.CONNECTED_IDLE);
// For A1: DTR False starts motor, DTR True stops it.
// mSerialPort.setDTR();
return;
}
}
mStatus.set(ConnectionStatus.DISCONNECTED);
Timer.delay(1.0); // Wait before retry
}
private boolean initiateScanMode() {
try {
// Clear buffer before starting
while (mIn.available() > 0) mIn.read();
mSerialPort.clearDTR(); // Start Motor
Thread.sleep(100);
sendCmd(CMD_SCAN);
// Wait for 7-byte descriptor
byte[] descriptor = new byte[DESCRIPTOR_LEN];
long start = System.currentTimeMillis();
while (mIn.available() < DESCRIPTOR_LEN) {
if (System.currentTimeMillis() - start > 1000) return false;
Timer.delay(0.01);
}
mIn.read(descriptor);
return descriptor[0] == SYNC_BYTE && descriptor[1] == SYNC_BYTE2;
} catch (Exception e) {
return false;
}
}
private void processIncomingData() {
try {
while (mIn.available() >= SCAN_PACKET_LEN) {
byte[] packet = new byte[SCAN_PACKET_LEN];
mIn.read(packet);
// Protocol validation based on provided Python logic
boolean newScan = (packet[0] & 0x1) != 0;
boolean invNewScan = ((packet[0] >> 1) & 0x1) != 0;
int checkBit = (packet[1] & 0x1);
if (newScan == invNewScan || checkBit != 1) {
// Out of sync - skip one byte to try and find sync again
return;
}
mLastDataTimestamp = Timer.getFPGATimestamp();
// Python logic: ((raw[1] >> 1) + (raw[2] << 7)) / 64.
int angleRaw = ((packet[1] & 0xFF) >> 1) + ((packet[2] & 0xFF) << 7);
double angle = angleRaw / 64.0;
// Python logic: (raw[3] + (raw[4] << 8)) / 4. (in mm)
int distRaw = (packet[3] & 0xFF) + ((packet[4] & 0xFF) << 8);
double distanceMeters = distRaw / 4000.0;
if (newScan && !mCurrentScan.isEmpty()) {
if (mListener != null) {
mListener.onScanComplete(new ArrayList<>(mCurrentScan));
}
mCurrentScan.clear();
}
if (distanceMeters > 0) {
mCurrentScan.add(new PolarPoint(angle, distanceMeters));
}
}
} catch (Exception e) {
mStatus.set(ConnectionStatus.ERROR);
}
}
private void checkWatchdog() {
if (Timer.getFPGATimestamp() - mLastDataTimestamp > WATCHDOG_TIMEOUT) {
// //
// stop_motor();
mStatus.set(ConnectionStatus.CONNECTED_IDLE);
// We have to check the health seperately because
// the connection check only ever occurs when
// the robot is recieving data
// if (!getHealth()) {
// DriverStation.reportWarning("RPLidar A1: Data timeout. Reconnecting...", false);
// mStatus.set(ConnectionStatus.ERROR);
// }
}
}
private void sendCmd(byte cmd) {
try {
if (mOut != null) {
mOut.write(new byte[]{SYNC_BYTE, cmd});
mOut.flush();
}
} catch (Exception e) {
mStatus.set(ConnectionStatus.ERROR);
}
}
/**
* Queries the device health status.
* @return true if the device is connected and returns a 'Good' health status, false otherwise.
*/
public boolean getHealth() {
if (mStatus.get() == ConnectionStatus.DISCONNECTED || mOut == null || mIn == null) {
return false;
}
try {
// Ensure the buffer is clear before sending request
while (mIn.available() > 0) mIn.read();
sendCmd(CMD_GET_HEALTH);
// Read 7-byte Descriptor
byte[] descriptor = new byte[DESCRIPTOR_LEN];
long startTime = System.currentTimeMillis();
while (mIn.available() < DESCRIPTOR_LEN) {
if (System.currentTimeMillis() - startTime > 500) return false;
Timer.delay(0.01);
}
mIn.read(descriptor);
return true;
// // Check if descriptor is valid and data type matches HEALTH (0x06)
// if (descriptor[0] != SYNC_BYTE || descriptor[1] != SYNC_BYTE2 || descriptor[6] != 0x06) {
// return false;
// }
// // Read 3-byte Health Payload
// byte[] healthPayload = new byte[3];
// while (mIn.available() < 3) {
// if (System.currentTimeMillis() - startTime > 1000) return false;
// Timer.delay(0.01);
// }
// mIn.read(healthPayload);
// Byte 0 is status: 0x00 = Good, 0x01 = Warning, 0x02 = Error
// return healthPayload[0] == 0;
} catch (Exception e) {
return false;
}
}
}
src/main/java/frc4388/robot/subsystems/TestRobot.java
-244
View File
@@ -1,244 +0,0 @@
package frc4388.robot.subsystems;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.configurable.ConfigurableDouble;
public class TestRobot extends SubsystemBase {
// TalonFX m_intakeMotor;
// TalonFX m_armMotor;
// TalonFX m_storageMotor;
TalonFX m_outerShooter;
TalonFX m_innerShooter;
ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0);
ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0);
ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0);
ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0);
ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0);
ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0);
VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0);
VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0);
public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second
public enum RobotStare {
IntakeDown,
Idle,
Shoot
}
public RobotStare robotStare = RobotStare.Idle;
public TestRobot(
// TalonFX intakeMotor,
// TalonFX armMotor,
// TalonFX storageMotor,
TalonFX outerShooter,
TalonFX innerShooter
) {
// m_intakeMotor = intakeMotor;
// m_armMotor = armMotor;
// m_storageMotor = storageMotor;
m_outerShooter = outerShooter;
m_innerShooter = innerShooter;
// m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG);
// m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG);
// m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG);
m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG);
m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG);
m_outerShooter.getConfigurator().apply(SHOOTER_PID);
m_innerShooter.getConfigurator().apply(SHOOTER_PID);
outerVelocity.Slot = 0;
innerVelocity.Slot = 0;
}
// public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
// public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true)
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
// public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static Slot0Configs SHOOTER_PID;
static {
SHOOTER_PID = new Slot0Configs();
SHOOTER_PID.kV = 0.12;
// SHOOTER_PID.kP = 0.11;
// SHOOTER_PID.kI = 0.48;
// SHOOTER_PID.kD = 0.01;
SHOOTER_PID.kP = 0.3;
SHOOTER_PID.kI = 0.0;
SHOOTER_PID.kD = 0.0;
}
ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12);
ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11);
ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48);
ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01);
@Override
public void periodic() {
SHOOTER_PID = new Slot0Configs();
SHOOTER_PID.kV = kV.get();
SHOOTER_PID.kP = kP.get();
SHOOTER_PID.kI = kI.get();
SHOOTER_PID.kD = kD.get();
m_outerShooter.getConfigurator().apply(SHOOTER_PID);
m_innerShooter.getConfigurator().apply(SHOOTER_PID);
SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond));
SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond));
switch (robotStare) {
case Idle:
// Move the arm to the up position
PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get());
// m_armMotor.setControl(armPosition);
// // Stop the intake motor
// m_intakeMotor.set(0);
// // Stop the storage motor
// m_storageMotor.set(0);
// Stop the outer shooter motor
m_outerShooter.set(0);
// Stop the inner shooter motor
m_innerShooter.set(0);
break;
case IntakeDown:
// Move the arm to the down sposition
PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get());
// m_armMotor.setControl(armPosition1);
// Move balls into the robot because the arm is down
VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get());
// m_intakeMotor.setControl(intakeVelocity);
// Move balls into the main box
VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get());
// m_storageMotor.setControl(storageVelocity);
// Stop the outer shooter motor
m_outerShooter.set(0);
// Stop the inner shooter motor
m_innerShooter.set(0);
break;
case Shoot:
// Move the arm to the up position
PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get());
// m_armMotor.setControl(armPosition2);
// // Stop the intake motor
// m_intakeMotor.set(0);
// // Move the balls into the
VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get());
// m_storageMotor.setControl(storageVelocity1);
// outerVelocity.
// m_outerShooter.setControl(outerVelocity);
double outerRPM = outerRate.get();
m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
// m_innerShooter.setControl(innerVelocity);
// m_innerShooter.set(innerRate.get());
double innerRPM = innerRate.get();
m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
// SmartDashboard.putNumber("Test", outerRate.get());
break;
default:
break;
}
}
}
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);
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+1 -8
View File
@@ -411,20 +411,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
}
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
// swerve drive is still going:
// stopModules(); // stop the swerve
// if (leftStick.getNorm() < 0.05) //if no imput
// return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * -speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rot));
// double
}
public double getGyroAngle() {
src/main/java/frc4388/utility/status/FaultA1M8.java
+39
View File
@@ -0,0 +1,39 @@
package frc4388.utility.status;
import frc4388.robot.subsystems.RPLidarA1;
import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus;
import frc4388.utility.status.Status.ReportLevel;
// Fault reporter for the RPLidar A1M8 Revolving lidar sensor
public class FaultA1M8 implements Queryable {
private String name;
private RPLidarA1 cam;
public static void addDevice(RPLidarA1 cam, String name) {
FaultA1M8 p = new FaultA1M8();
p.name = name;
p.cam = cam;
FaultReporter.register(p);
}
@Override
public String getName() {
return name;
}
@Override
public Status diagnosticStatus() {
Status s = new Status();
ConnectionStatus cam_ConnectionStatus = cam.getStatus();
if(cam_ConnectionStatus != ConnectionStatus.RECEIVING_DATA)
s.addReport(ReportLevel.ERROR, "Not Connected! Status = " + cam_ConnectionStatus);
s.addReport(ReportLevel.INFO, cam.getStatus().toString());
return s;
}
}