diff --git a/build.gradle b/build.gradle index bdf2b90..489a591 100644 --- a/build.gradle +++ b/build.gradle @@ -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" } diff --git a/src/main/deploy/pathplanner/autos/Lidar Intake.auto b/src/main/deploy/pathplanner/autos/Lidar Intake.auto new file mode 100644 index 0000000..0e1376a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Lidar Intake.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lidar Intake" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Quick Shoot test.auto b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto similarity index 76% rename from src/main/deploy/pathplanner/autos/Quick Shoot test.auto rename to src/main/deploy/pathplanner/autos/Robot Reveal Night.auto index 1828ee3..ddb4f8a 100644 --- a/src/main/deploy/pathplanner/autos/Quick Shoot test.auto +++ b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto @@ -7,7 +7,13 @@ { "type": "named", "data": { - "name": "Robot Intake Down" + "name": "Robot Rev Up" + } + }, + { + "type": "named", + "data": { + "name": "Intake Retracted" } }, { diff --git a/src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto b/src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto new file mode 100644 index 0000000..6c7bc18 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test- Robot Align Path.auto @@ -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 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Quick Shoot.path b/src/main/deploy/pathplanner/paths/Quick Shoot.path index 11d6ce4..36bd07b 100644 --- a/src/main/deploy/pathplanner/paths/Quick Shoot.path +++ b/src/main/deploy/pathplanner/paths/Quick Shoot.path @@ -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 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test - Robot Align.path b/src/main/deploy/pathplanner/paths/Test - Robot Align.path new file mode 100644 index 0000000..0b4c861 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Test - Robot Align.path @@ -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 +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5a49a3e..ec732cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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), diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java new file mode 100644 index 0000000..770280d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -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; + } + } + +} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 550c761..5c2a083 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -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; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java new file mode 100644 index 0000000..fdbf2a6 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -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_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 scan) { + + System.out.println("SCAN: " + scan.size()); + + double scale = 0.009; + + List 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); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java b/src/main/java/frc4388/robot/subsystems/RPLidarA1.java new file mode 100644 index 0000000..fa5aa32 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/RPLidarA1.java @@ -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 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 mStatus = new AtomicReference<>(ConnectionStatus.DISCONNECTED); + private SerialPort mSerialPort; + private InputStream mIn; + private OutputStream mOut; + private ScanListener mListener; + + private final List 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; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java deleted file mode 100644 index 4bf3dba..0000000 --- a/src/main/java/frc4388/robot/subsystems/TestRobot.java +++ /dev/null @@ -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; - } - - - - } - -} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index fc05d3a..c1a90ad 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -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 diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index b46648a..5a9e1c2 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -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); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 2ee10d3..a6c772e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -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() { diff --git a/src/main/java/frc4388/utility/status/FaultA1M8.java b/src/main/java/frc4388/utility/status/FaultA1M8.java new file mode 100644 index 0000000..aedb866 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultA1M8.java @@ -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; + } +}