From 84855339df905a4c05fa5864c00a7586406f7530 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 15 Dec 2023 14:06:10 -0800 Subject: [PATCH] feat: initial driving code, sans turret :D Co-authored-by: DaviTheDinosaur <119637962+davithedinosaur@users.noreply.github.com> --- .../bunnybotTank2023/RobotContainer.java | 65 ++++++----- .../commands/indexer/IndexerCommand.java | 2 +- .../commands/intake/IntakeCommand.java | 2 +- .../shooter/ShootSequenceCommand.java | 21 ++++ .../commands/turret/Turret.java | 4 + .../turret/TurretTrackTargetCommand.java | 28 +++++ .../bunnybotTank2023/io/vision/Limelight.java | 104 ++++++++++++++++-- .../io/vision/LimelightIO.java | 32 ++++++ .../io/vision/LimelightIOReal.java | 52 +++++++++ .../io/vision/TargetVisionIO.java | 18 --- .../utils/vision/LEDState.java | 2 +- .../utils/vision/ShootingParameters.java | 18 +++ .../utils/vision/TrackedTarget.java | 24 ++++ 13 files changed, 316 insertions(+), 56 deletions(-) create mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShootSequenceCommand.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIO.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java delete mode 100644 src/main/java/org/team1540/bunnybotTank2023/io/vision/TargetVisionIO.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/utils/vision/ShootingParameters.java create mode 100644 src/main/java/org/team1540/bunnybotTank2023/utils/vision/TrackedTarget.java diff --git a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java index 57209fb..8cc1ed3 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java +++ b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java @@ -9,20 +9,20 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes; +import org.team1540.bunnybotTank2023.commands.drivetrain.ArcadeDriveCommand; import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain; import org.team1540.bunnybotTank2023.commands.indexer.Indexer; import org.team1540.bunnybotTank2023.commands.indexer.IndexerCommand; import org.team1540.bunnybotTank2023.commands.indexer.IndexerIdleCommand; import org.team1540.bunnybotTank2023.commands.intake.Intake; import org.team1540.bunnybotTank2023.commands.intake.IntakeCommand; +import org.team1540.bunnybotTank2023.commands.shooter.ShootSequenceCommand; import org.team1540.bunnybotTank2023.commands.shooter.Shooter; -import org.team1540.bunnybotTank2023.commands.turret.Turret; -import org.team1540.bunnybotTank2023.commands.turret.TurretManualCommand; -import org.team1540.bunnybotTank2023.commands.turret.TurretSetpointCommand; -import org.team1540.bunnybotTank2023.commands.turret.TurretZeroSequenceCommand; +import org.team1540.bunnybotTank2023.commands.turret.*; import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOSim; import org.team1540.bunnybotTank2023.io.drivetrain.DrivetrainIOReal; import org.team1540.bunnybotTank2023.io.indexer.IndexerIOReal; @@ -30,6 +30,13 @@ import org.team1540.bunnybotTank2023.io.shooter.ShooterIOReal; import org.team1540.bunnybotTank2023.io.shooter.ShooterIOSim; import org.team1540.bunnybotTank2023.io.turret.TurretIOReal; +import org.team1540.bunnybotTank2023.io.vision.Limelight; +import org.team1540.bunnybotTank2023.io.vision.LimelightIO; +import org.team1540.bunnybotTank2023.io.vision.LimelightIOReal; + +import java.util.ArrayList; + +import static org.team1540.bunnybotTank2023.Constants.*; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -39,11 +46,12 @@ */ public class RobotContainer { // Subsystems - Drivetrain drivetrain; - Shooter shooter; - Indexer indexer; - Intake intake; - Turret turret; + final Drivetrain drivetrain; + final Shooter shooter; + final Indexer indexer; + final Intake intake; + final Turret turret; + final Limelight limelight; // Controllers CommandXboxController driver = new CommandXboxController(0); @@ -59,6 +67,7 @@ public RobotContainer() { indexer = new Indexer(new IndexerIOReal()); intake = new Intake(new IntakeIOReal()); turret = new Turret(new TurretIOReal()); + limelight = new Limelight(new LimelightIOReal()); } else { // Initialize subsystems with simulation IO drivetrain = new Drivetrain(new DrivetrainIOSim()); @@ -66,6 +75,7 @@ public RobotContainer() { indexer = null; intake = null; turret = null; + limelight = null; } setDefaultCommands(); configureButtonBindings(); @@ -74,31 +84,34 @@ public RobotContainer() { /** Use this method to define your trigger->command mappings. */ private void configureButtonBindings() { - copilot.a() - .whileTrue(new InstantCommand(() -> shooter.setVelocity(turretSetpoint.get())) - .andThen(new IndexerCommand(indexer))) - .onFalse(new InstantCommand(() -> { - shooter.stop(); - indexer.stop(); - })); + copilot.a().whileTrue( + Commands.parallel( + new IntakeCommand(intake), + new IndexerCommand(indexer) + ) + ); -// copilot.b().whileTrue(new InstantCommand(() -> intake.setFold(false)).andThen(new IndexerCommand(indexer))).whileFalse(new InstantCommand(() -> intake.setFold(true))); - copilot.b().whileTrue(Commands.parallel(new IntakeCommand(intake), new IndexerCommand(indexer))); - copilot.x().whileTrue(new TurretSetpointCommand(turret, Rotation2d.fromDegrees(turretSetpoint.get()))); copilot.y().whileTrue(new TurretZeroSequenceCommand(turret)); + copilot.a().onTrue(new InstantCommand( () -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true))); + copilot.rightTrigger().onTrue(new ShootSequenceCommand(shooter, indexer, 3000)); // copilot.x().onTrue(new InstantCommand(() -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true))); } private void setDefaultCommands() { -// drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver)); -// shooter.setDefaultCommand(new StartEndCommand( -// () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM), -// () -> {}, -// shooter -// )); + drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver)); + shooter.setDefaultCommand(new StartEndCommand( + () -> shooter.setVelocity(ShooterConstants.SHOOTER_IDLE_RPM), + () -> {}, + shooter + )); turret.setDefaultCommand(new TurretManualCommand(turret, copilot)); -// indexer.setDefaultCommand(new IndexerIdleCommand(indexer)); + indexer.setDefaultCommand(new IndexerIdleCommand(indexer)); + intake.setDefaultCommand(new StartEndCommand( + () -> intake.setMotorSpeed(0.3), + () -> {}, + intake + )); } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java index 27d2aef..c81f999 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/indexer/IndexerCommand.java @@ -13,7 +13,7 @@ public IndexerCommand(Indexer indexer) { @Override public void initialize() { indexer.setBottomSpeed(0.50); - indexer.setTopSpeed(1); + indexer.setTopSpeed(-0.2); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java index ba8ec38..ee0ca1b 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/intake/IntakeCommand.java @@ -12,7 +12,7 @@ public IntakeCommand(Intake intake) { @Override public void initialize() { intake.setFold(false); - intake.setMotorSpeed(0.5); + intake.setMotorSpeed(1); } @Override diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShootSequenceCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShootSequenceCommand.java new file mode 100644 index 0000000..064b225 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShootSequenceCommand.java @@ -0,0 +1,21 @@ +package org.team1540.bunnybotTank2023.commands.shooter; + +import edu.wpi.first.wpilibj2.command.*; +import org.team1540.bunnybotTank2023.commands.indexer.Indexer; + +public class ShootSequenceCommand extends SequentialCommandGroup { + public ShootSequenceCommand(Shooter shooter, Indexer indexer, double shooterVelocityRPM) { + addCommands( + new InstantCommand(() -> { + shooter.setVelocity(shooterVelocityRPM); + indexer.setBottomSpeed(0.5); + }), + new WaitUntilCommand(shooter::isAtSetpoint), + new InstantCommand(() -> indexer.setTopSpeed(1)), + new WaitCommand(0.1), + new InstantCommand(() -> indexer.setTopSpeed(-0.2)), + new WaitCommand(0.5) + ); + addRequirements(shooter, indexer); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java index 690e5db..4696f4e 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java @@ -50,6 +50,10 @@ public boolean isAtSetpoint() { return Math.abs(inputs.turretSetPointDegrees - averageFilter.getAverage()) < 0.5; } + public Rotation2d getPosition() { + return Rotation2d.fromDegrees(inputs.turretCurrentPositionDegrees); + } + public void resetToEncoder(Rotation2d position) { io.resetEncoder(position); } diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java new file mode 100644 index 0000000..c8a174c --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java @@ -0,0 +1,28 @@ +package org.team1540.bunnybotTank2023.commands.turret; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.team1540.bunnybotTank2023.io.vision.Limelight; + +public class TurretTrackTargetCommand extends CommandBase { + private final Turret turret; + private final Limelight limelight; + + public TurretTrackTargetCommand(Turret turret, Limelight limelight) { + this.turret = turret; + this.limelight = limelight; + addRequirements(turret); + } + + @Override + public void execute() { + Rotation2d turretSetpoint = Rotation2d.fromDegrees(limelight.getTx()).plus(turret.getPosition()); + turret.autoSetPosition(turretSetpoint); + } + + @Override + public void end(boolean interrupted) { + turret.stop(); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java b/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java index 8548369..fd6a5da 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java @@ -1,18 +1,104 @@ package org.team1540.bunnybotTank2023.io.vision; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import org.team1540.bunnybotTank2023.utils.vision.LEDState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; -public class Limelight implements TargetVisionIO { - private final NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); +import java.util.ArrayList; - @Override - public void updateInputs(VisionInputs inputs) { +public class Limelight extends SubsystemBase { + private final LimelightIO io; + private final LimelightIOInputsAutoLogged inputs = new LimelightIOInputsAutoLogged(); + private double[] target; + + private final double mountingAngleDegrees = 10.0; + private final double limelightLensHeightInches = 20.0;//TODO Find this + private final double goalHeightInches = 25.0;//TODO Find this + private static final double HORIZONTAL_FOV = Math.toRadians(63.3); + private static final double VERTICAL_FOV = Math.toRadians(49.7); + + public Limelight(LimelightIO io) { + this.io = io; } @Override - public void setLEDState(LEDState ledState) { + public void periodic() { + io.updateInputs(inputs); + Logger.getInstance().processInputs("Limelight", inputs); + + + if(getTv()) target = bestTargetCentral(); + + Logger.getInstance().recordOutput("Limelight/targetDistance", getDistance()); + Logger.getInstance().recordOutput("Limelight/targetX", getTx()); + Logger.getInstance().recordOutput("Limelight/targetY", getTy()); + Logger.getInstance().recordOutput("Limelight/validTargetsLength", io.getValidTargets().size()); + Logger.getInstance().recordOutput("Limelight/allTargetsLength", io.getAllTargets().length); + } + + public double getTa() { + return target[2]; + } + + public double getTx() { + if(getTv()) return target[3]; + return 0; + } + public double getTx(double[] target){ + return target[3]; + } + + public double getTy() { + if(getTv()) return target[4]; + return 0; + } + + public boolean getTv() { + return inputs.validTarget && io.getValidTargets().size() > 0; + } + + public double getDistance(){ + if(getTv()) return (goalHeightInches - limelightLensHeightInches)/Math.tan(Math.toRadians(getTy() + mountingAngleDegrees)); + return 0; + } + public double getDistance(double[] target){ + if(getTv()) return (goalHeightInches - limelightLensHeightInches)/Math.tan(Math.toRadians(target[4] + mountingAngleDegrees)); + return 0; + } + + private double[] bestTargetMinDist(){ + if (!getTv()) return null ; + ArrayList targets = io.getValidTargets(); + double[] minTarget = targets.get(0); + double min = getDistance(minTarget); + for (int i = 1; i < targets.size(); i++) { + if(getDistance(targets.get(i)) < min){ + min = getDistance(targets.get(i)); + minTarget = targets.get(i); + } + } + return minTarget; + } + + private double[] bestTargetCentral(){ + if (!getTv()) return null; + ArrayList targets = io.getValidTargets(); + double[] minTarget = targets.get(0); + double min = getTx(targets.get(0)); + for (int i = 1; i < targets.size(); i++) { + if(getTx(targets.get(i)) < min){ + min = getTx(targets.get(i)); + minTarget = targets.get(i); + } + } + return minTarget; + + } + + public boolean aimed(){ + return getTx() < 1; + } + public double getHorizontalFov() { + return HORIZONTAL_FOV; } -} +} \ No newline at end of file diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIO.java b/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIO.java new file mode 100644 index 0000000..1521181 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIO.java @@ -0,0 +1,32 @@ +package org.team1540.bunnybotTank2023.io.vision; + +import org.littletonrobotics.junction.AutoLog; + +import java.util.ArrayList; + +public interface LimelightIO { + public double[][] allTargets = {}; + public ArrayList validTargets = new ArrayList<>(); + + @AutoLog + public static class LimelightIOInputs{ + public boolean validTarget = false; + + public double captureLatencyMs = 0.0; + public double pipelineLatencyMs = 0.0; + public double parseLatencyMs = 0.0; + public double totalLatencyMs = 0.0; + public double timestampMs = 0.0; + public double pipeline = 0; + } + + default void updateInputs(LimelightIOInputs inputs) {} + + default double[][] getAllTargets() { + return new double[0][0]; + } + default ArrayList getValidTargets() { + return new ArrayList<>(); + } + +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java new file mode 100644 index 0000000..11964dc --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIOReal.java @@ -0,0 +1,52 @@ +package org.team1540.bunnybotTank2023.io.vision; + +import edu.wpi.first.wpilibj.DriverStation; +import org.team1540.bunnybotTank2023.utils.vision.LimelightHelpers; + +import java.util.ArrayList; + +public class LimelightIOReal implements LimelightIO{ + + public double[][] allTargets = {}; + public ArrayList validTargets = new ArrayList<>(); + + LimelightHelpers.LimelightResults limelightResults; + public LimelightIOReal(){ + LimelightHelpers.setPipelineIndex("limelight-rear",9); + LimelightHelpers.setLEDMode_PipelineControl(""); + } + + @Override + public void updateInputs(LimelightIOInputs inputs) { + validTargets.clear(); + limelightResults = LimelightHelpers.getLatestResults("limelight-rear"); + inputs.validTarget = limelightResults.targetingResults.valid; + inputs.captureLatencyMs = limelightResults.targetingResults.latency_capture; + inputs.pipelineLatencyMs = limelightResults.targetingResults.latency_pipeline; + inputs.parseLatencyMs = limelightResults.targetingResults.latency_jsonParse; + inputs.totalLatencyMs = inputs.captureLatencyMs + inputs.pipelineLatencyMs + inputs.parseLatencyMs; + inputs.timestampMs = limelightResults.targetingResults.timestamp_RIOFPGA_capture; + inputs.pipeline = limelightResults.targetingResults.pipelineID; + + allTargets = new double[limelightResults.targetingResults.targets_Detector.length][]; + ArrayList validTargets1 = new ArrayList<>(allTargets.length); + for (int i = 0; i < allTargets.length; i += 1) { + LimelightHelpers.LimelightTarget_Detector target = limelightResults.targetingResults.targets_Detector[i]; + double[] temp = new double[]{target.classID, target.confidence, target.ta, target.tx, target.ty}; + allTargets[i] = temp; + if(target.classID == (DriverStation.getAlliance() == DriverStation.Alliance.Blue?1.0:0)){ + validTargets1.add(temp); + } + } + validTargets = validTargets1; + } + + public double[][] getAllTargets(){ + return allTargets; + } + + @Override + public ArrayList getValidTargets() { + return validTargets; + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/vision/TargetVisionIO.java b/src/main/java/org/team1540/bunnybotTank2023/io/vision/TargetVisionIO.java deleted file mode 100644 index cfed686..0000000 --- a/src/main/java/org/team1540/bunnybotTank2023/io/vision/TargetVisionIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.team1540.bunnybotTank2023.io.vision; - -import org.littletonrobotics.junction.AutoLog; -import org.team1540.bunnybotTank2023.utils.vision.LEDState; - -public interface TargetVisionIO { - @AutoLog - class VisionInputs { - public double captureLatencyMs = 0; - public double pipelineLatencyMS = 0; - public double parseLatencyMs = 0; - public double totalLatencyMs = 0; - } - - void updateInputs(VisionInputs inputs); - - void setLEDState(LEDState ledState); -} diff --git a/src/main/java/org/team1540/bunnybotTank2023/utils/vision/LEDState.java b/src/main/java/org/team1540/bunnybotTank2023/utils/vision/LEDState.java index 49b5042..f12313a 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/utils/vision/LEDState.java +++ b/src/main/java/org/team1540/bunnybotTank2023/utils/vision/LEDState.java @@ -13,7 +13,7 @@ public enum LEDState { /* LEDs on */ ON(3); - private final int value; + public final int value; LEDState(int value) { this.value = value; diff --git a/src/main/java/org/team1540/bunnybotTank2023/utils/vision/ShootingParameters.java b/src/main/java/org/team1540/bunnybotTank2023/utils/vision/ShootingParameters.java new file mode 100644 index 0000000..2706017 --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/utils/vision/ShootingParameters.java @@ -0,0 +1,18 @@ +package org.team1540.bunnybotTank2023.utils.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + + +public class ShootingParameters { + public final Rotation2d turretAngle; + public final double distanceMeters; + + // probably incorrect math, might not use + public ShootingParameters(Translation2d fieldToTargetMeters, Pose2d robotPoseMeters) { + distanceMeters = fieldToTargetMeters.getDistance(robotPoseMeters.getTranslation()); + Translation2d deltaTranslation = robotPoseMeters.getTranslation().minus(fieldToTargetMeters); + turretAngle = Rotation2d.fromRadians(Math.atan2(deltaTranslation.getX(), deltaTranslation.getY())).plus(robotPoseMeters.getRotation()); + } +} diff --git a/src/main/java/org/team1540/bunnybotTank2023/utils/vision/TrackedTarget.java b/src/main/java/org/team1540/bunnybotTank2023/utils/vision/TrackedTarget.java new file mode 100644 index 0000000..1eb340f --- /dev/null +++ b/src/main/java/org/team1540/bunnybotTank2023/utils/vision/TrackedTarget.java @@ -0,0 +1,24 @@ +package org.team1540.bunnybotTank2023.utils.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.util.function.Supplier; + +public class TrackedTarget { + public final Translation2d robotToTarget; + public final Translation2d fieldToTarget; + + private final Supplier poseSupplier; + + public TrackedTarget(Rotation2d targetAngle, double distance, Supplier poseSupplier) { + robotToTarget = new Translation2d(distance, targetAngle); + fieldToTarget = poseSupplier.get().getTranslation().plus(robotToTarget); + this.poseSupplier = poseSupplier; + } + + public ShootingParameters getShootingParameters() { + return new ShootingParameters(fieldToTarget, poseSupplier.get()); + } +}