mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Merge pull request #5 from Team4388/AutoTesting
Merge autos for reveal night
This commit is contained in:
Michael Mikovsky
@@ -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"
|
||||
}
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lidar Intake"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
+7
-1
@@ -7,7 +7,13 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Robot Intake Down"
|
||||
"name": "Robot Rev Up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Intake Retracted"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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.setShooterReady(), 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.setShooterShoot(), m_robotShooter),
|
||||
new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter),
|
||||
new WaitCommand(2),
|
||||
IntakeRetracted,
|
||||
new WaitCommand(3),
|
||||
new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter),
|
||||
new InstantCommand(() -> m_robotShooter.setShooterNotReady(), 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);
|
||||
|
||||
|
||||
@@ -355,7 +366,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),
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -40,6 +40,10 @@ public class Intake extends SubsystemBase {
|
||||
return mode;
|
||||
}
|
||||
|
||||
public void rollerStop(){
|
||||
io.setRollerOutput(state, 0);
|
||||
}
|
||||
|
||||
|
||||
// public enum FieldZone {
|
||||
// // The robot should aim at the hub
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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() {
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user