Skip to content
This repository has been archived by the owner on Jan 16, 2024. It is now read-only.

Commit

Permalink
feat: limelight
Browse files Browse the repository at this point in the history
  • Loading branch information
WeilSimon committed Dec 5, 2023
1 parent 35d842c commit 4a6d945
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 3 deletions.
10 changes: 10 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
import frc.robot.subsystems.drive.ModuleIOSim;
import frc.robot.subsystems.drive.ModuleIOSparkMax;
import frc.robot.subsystems.shooter.*;
import frc.robot.util.Limelight;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;

Expand All @@ -47,6 +48,8 @@ public class RobotContainer {
private final Flywheel flywheel;
private final Shooter shooter;

private final Limelight limelight = new Limelight("limelight");

// Controller
private final CommandXboxController controller = new CommandXboxController(0);

Expand Down Expand Up @@ -174,6 +177,13 @@ private void configureButtonBindings() {
.whileTrue(
Commands.startEnd(
() -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
controller.x()
.whileTrue(DriveCommands.LimelightRotDrive(
drive,
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
limelight))
.whileTrue(new ShooterCommands.Shoot(shooter,limelight));
}

/**
Expand Down
37 changes: 37 additions & 0 deletions src/main/java/frc/robot/commands/DriveCommands.java
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.subsystems.drive.Drive;
import frc.robot.util.Limelight;

import java.util.function.DoubleSupplier;

public class DriveCommands {
Expand Down Expand Up @@ -67,4 +69,39 @@ public static Command joystickDrive(
},
drive);
}

public static Command LimelightRotDrive(
Drive drive,
DoubleSupplier xSupplier,
DoubleSupplier ySupplier,
Limelight limelight) {
return Commands.run(
() -> {
// Apply deadband
double linearMagnitude =
MathUtil.applyDeadband(
Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
Rotation2d linearDirection =
new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
double omega = MathUtil.applyDeadband(limelight.getTx() * limelight.getHorizontalFov()/(2*100), 0.01);
// Square values
linearMagnitude = linearMagnitude * linearMagnitude;
omega = Math.copySign(omega * omega, omega);

// Calcaulate new linear velocity
Translation2d linearVelocity =
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
.getTranslation();

// Convert to field relative speeds & send command
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
drive.getRotation()));
},
drive);
}
}
13 changes: 10 additions & 3 deletions src/main/java/frc/robot/commands/ShooterCommands.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.util.Limelight;

public class ShooterCommands {
public static class Shoot extends Command {
Expand All @@ -10,6 +11,8 @@ public static class Shoot extends Command {
double mainRPM;
double secondaryRPM;

Limelight limelight;

enum State {
INIT,
SPINUP,
Expand All @@ -19,24 +22,28 @@ enum State {

State state = State.SPINUP;

public Shoot(Shooter shooter) {
public Shoot(Shooter shooter, Limelight limelight) {
this.shooter = shooter;
addRequirements(shooter);

this.limelight = limelight;
}

@Override
public void execute() {

switch (state) {
case INIT -> {
shooter.setLoadingVoltage(-8);
state = State.SPINUP;
}
case SPINUP -> {
//TODO calculation based on distance;

shooter.setMainVelocity(mainRPM);
shooter.setSecondaryVelocity(secondaryRPM);
if (shooter.getMainVelocityRPM() == mainRPM
&& shooter.getSecondaryVelocityRPM() == secondaryRPM) {
&& shooter.getSecondaryVelocityRPM() == secondaryRPM
&& limelight.aimed()) {
shooter.setLoadingVoltage(10);
state = State.SHOOTING;
}
Expand Down
61 changes: 61 additions & 0 deletions src/main/java/frc/robot/util/Limelight.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,61 @@
package frc.robot.util;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;

public class Limelight {
private double tv, tx, ty, ta;

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 final NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
//private final AHRS navx;
public final String name;
private double[] data;
// private final PoseZeroFilter zeroFilter = new PoseZeroFilter(50, 48);
// private final PoseMedianFilter medianFilter = new PoseMedianFilter(10);
private double latency;
private static double HORIZONTAL_FOV = Math.toRadians(63.3);
private static double VERTICAL_FOV = Math.toRadians(49.7);

public Limelight(String name) {
this.name = name;

}

public void update(){
tv = table.getEntry("tv").getDouble(0);
tx = table.getEntry("tx").getDouble(0);
ty = table.getEntry("ty").getDouble(0);
ta = table.getEntry("ta").getDouble(0);
}

public double getTa() {
return ta;
}

public double getTx() {
return tx;
}

public double getTy() {
return ty;
}

public double getTv() {
return tv;
}

public double getDistance(){
return (goalHeightInches - limelightLensHeightInches)/Math.tan(Math.toRadians(ty + mountingAngleDegrees));
}

public boolean aimed(){
return tx < 1;
}

public double getHorizontalFov() {
return HORIZONTAL_FOV;
}
}

0 comments on commit 4a6d945

Please sign in to comment.