This repository has been archived by the owner on Jan 16, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: initial driving code, sans turret :D
Co-authored-by: DaviTheDinosaur <[email protected]>
- Loading branch information
1 parent
2eaa1ef
commit 8485533
Showing
13 changed files
with
316 additions
and
56 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
21 changes: 21 additions & 0 deletions
21
src/main/java/org/team1540/bunnybotTank2023/commands/shooter/ShootSequenceCommand.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
28 changes: 28 additions & 0 deletions
28
src/main/java/org/team1540/bunnybotTank2023/commands/turret/TurretTrackTargetCommand.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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(); | ||
| } | ||
| } |
104 changes: 95 additions & 9 deletions
104
src/main/java/org/team1540/bunnybotTank2023/io/vision/Limelight.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<double[]> 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<double[]> 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; | ||
| } | ||
| } | ||
| } |
32 changes: 32 additions & 0 deletions
32
src/main/java/org/team1540/bunnybotTank2023/io/vision/LimelightIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<double[]> 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<double[]> getValidTargets() { | ||
| return new ArrayList<>(); | ||
| } | ||
|
|
||
| } |
Oops, something went wrong.