Hi everyone,
I’m looking for suggestions on how to test and debug Limelight code remotely when I am at home and cannot access the robot or the Limelight.
If there are any tools I can use to simulate a robot with limelight vision please let me know!
Additionally, this is the code I am hoping to test!
RobotContainer.java
package frc.robot;
import java.lang.reflect.Method;
import java.util.Arrays;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.*;
import edu.wpi.first.wpilibj2.command.*;
import frc.robot.commands.*;
import frc.robot.commands.LimeLightVision.*;
import frc.robot.subsystems.*;
import frc.robot.Constants.*;
public class RobotContainer {
//subsystems
private final CANDrivetrain m_drivetrain = new CANDrivetrain();
private final LimeLight m_limelight = new LimeLight();
//private final CANLauncher m_launcher = new CANLauncher();
//commands
private final AlignWithTag m_align = new AlignWithTag(m_drivetrain, m_limelight);
private final MoveWithTag m_move = new MoveWithTag(m_limelight, m_drivetrain);
//shuffleboard
private final SendableChooser<Command> autoChooser = new SendableChooser<>();
private final Joystick m_driver = new Joystick(Constants.OIConstants.kDriverControllerPort);
private final Joystick m_operator = new Joystick(Constants.OIConstants.kOperatorControllerPort);
public RobotContainer() {
m_drivetrain.setDefaultCommand(
new RunCommand(
() -> m_drivetrain.arcadeDrive(
-m_driver.getY() * Constants.OIConstants.kDriverSpeedMultiplier,
-m_driver.getX() * Constants.OIConstants.kDriverSpeedMultiplier),
m_drivetrain));
configureBindings();
populateAutoCommands();
}
private void configureBindings() {
//Move with the vision target
final JoystickButton autoMoveButton = new JoystickButton(m_driver, OIConstants.kDriverAutoMoveButton);
autoMoveButton.onTrue(m_move);
autoMoveButton.onFalse(
new RunCommand(
() -> m_drivetrain.arcadeDrive(
-m_driver.getY() * Constants.OIConstants.kDriverSpeedMultiplier,
-m_driver.getX() * Constants.OIConstants.kDriverSpeedMultiplier),
m_drivetrain));
//Align with the vision target
final JoystickButton autoAlignButton = new JoystickButton(m_driver, OIConstants.kDriverAlignmentButton);
autoAlignButton.onTrue(m_align);
autoAlignButton.onFalse(
new RunCommand(
() -> m_drivetrain.arcadeDrive(
-m_driver.getY() * Constants.OIConstants.kDriverSpeedMultiplier,
-m_driver.getX() * Constants.OIConstants.kDriverSpeedMultiplier),
m_drivetrain));
}
// Commented out the following as we do not have a launcher yet.
/*
m_operatorController
.a()
.whileTrue(
new PrepareLaunch(m_launcher)
.withTimeout(LauncherConstants.kLauncherDelay)
.andThen(new LaunchNote(m_launcher))
.handleInterrupt(() -> m_launcher.stop()));
m_operatorController.leftBumper().whileTrue(m_launcher.getIntakeCommand());
*/
private void populateAutoCommands() {
Method[] methods = Autos.class.getDeclaredMethods();
Arrays.stream(methods)
.filter(method -> Command.class.isAssignableFrom(method.getReturnType()) && method.getParameterCount() == 1)
.forEach(method -> {
try {
Command command = (Command) method.invoke(null, m_drivetrain);
autoChooser.addOption(method.getName(), command);
} catch (Exception e) {
e.printStackTrace();
}
});
SmartDashboard.putData("Auto Mode", autoChooser);
}
public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
public String getSelectedAutoName() {
return autoChooser.getSelected().getName();
}
}
LimeLight.java
package frc.robot.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.networktables.NetworkTableInstance;
public class LimeLight extends SubsystemBase {
public boolean m_ValidTarget = false;
public double tx;
public double ty;
public double tv;
public double ta;
public LimeLight(){
//setup for calibration
}
@Override
public void periodic() {
updateLimeLightTracking();
}
public void updateLimeLightTracking() {
final double TARGET = 13.0;
tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0);
if (tv < 1.0)
{
m_ValidTarget = false;
return;
}
else{
m_ValidTarget = true;
}
SmartDashboard.putNumber("LimeLight TV", tv);
SmartDashboard.putNumber("LimeLight TX", tx);
SmartDashboard.putNumber("LimeLight TY", ty);
SmartDashboard.putNumber("LimeLight TA", ta);
SmartDashboard.putBoolean("Target Aquired", m_ValidTarget);
}
}
AlignWithTag.java
package frc.robot.commands.LimeLightVision;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.CANDrivetrain;
import frc.robot.subsystems.LimeLight;
public class AlignWithTag extends Command {
private final CANDrivetrain drivetrain;
private final LimeLight limelight;
private static final double Kp = -0.1;
private static final double min_command = 0.05;
// Constructor
public AlignWithTag(CANDrivetrain drivetrain, LimeLight limelight) {
this.drivetrain = drivetrain;
this.limelight = limelight;
addRequirements(drivetrain);
addRequirements(limelight);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
// Initialization code, if any
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
limelight.updateLimeLightTracking();
double heading_error = -limelight.tx;
System.out.println("Executing with error " + heading_error);
double steering_adjust = 0.0;
if (Math.abs(heading_error) > 1.0) {
steering_adjust = Kp * heading_error + (heading_error > 0 ? -min_command : min_command);
}
drivetrain.arcadeDrive(0, steering_adjust);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.arcadeDrive(0, 0); //Stop the drivetrain when the command ends
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false; // Modify this as needed based on your command's ending condition
}
}
MoveWithTag.java
package frc.robot.commands.LimeLightVision;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.LimeLight;
import frc.robot.Constants.DrivetrainConstants;
import frc.robot.subsystems.CANDrivetrain;
public class MoveWithTag extends Command{
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final LimeLight m_LimeLight;
private final CANDrivetrain m_Drivetrain;
/**
* Creates a new AutoDrive.
*
* @param subsystem The subsystem used by this command.
*/
public MoveWithTag(LimeLight subsystem1, CANDrivetrain subsystem2) {
m_LimeLight = subsystem1;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(subsystem1);
m_Drivetrain = subsystem2;
addRequirements(subsystem2);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// Check if there's a valid target
if (!m_LimeLight.m_ValidTarget) {
return; // No valid target, do nothing
}
// Calculate the necessary adjustments for alignment
double turnAdjust = 0.0;
double moveAdjust = 0.0;
// Determine turn direction based on horizontal offset (tx)
if (Math.abs(m_LimeLight.tx) > DrivetrainConstants.k_AutoCorrectTurn) {
turnAdjust = m_LimeLight.tx > 0 ? -DrivetrainConstants.k_AutoCorrectSpeed : DrivetrainConstants.k_AutoCorrectSpeed;
}
// Determine move direction based on target area (ta)
if (Math.abs(m_LimeLight.ta - DrivetrainConstants.k_AutoCorrectDist) > 0.1) {
moveAdjust = m_LimeLight.ta > DrivetrainConstants.k_AutoCorrectDist ? -DrivetrainConstants.k_AutoCorrectSpeed : DrivetrainConstants.k_AutoCorrectSpeed;
}
// Command the drivetrain to adjust position and orientation
m_Drivetrain.arcadeDrive(moveAdjust, turnAdjust);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}