Robot Deep Space 2019

El juego FRC de 2019 Deep Space tenía objetivos de visión sobre muchas de las metas. A continuación puedes encontrar programas de ejemplo completos en Java y Labview que implementan un método simple para conducir automáticamente hacia una meta en Deep Space.

Estos son programas muy simples y solo pretenden mostrar el concepto de usar datos de seguimiento de Limelight para controlar tu robot. En cada programa, puedes conducir tu robot con un mando. Si mantienes presionado el botón 'A', y el Limelight ve un objetivo válido (dependiendo de la configuración en tu pipeline) entonces el robot conducirá automáticamente hacia el objetivo. Ten cuidado de ajustar las diversas constantes en el código para tu robot en particular. Algunos robots giran o conducen más fácilmente que otros, por lo que el ajuste de las constantes de control proporcional debe hacerse caso por caso. Asegúrate de que el robot conduce correctamente usando el mando antes de habilitar el comportamiento de búsqueda del Limelight.

Java
LabView

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.networktables.*;


public class Robot extends TimedRobot {

  private static final String kDefaultAuto = "Default";
  private static final String kCustomAuto = "My Auto";
  private String m_autoSelected;
  private final SendableChooser<String> m_chooser = new SendableChooser<>();

  private VictorSP m_Left0 = new VictorSP(0);
  private VictorSP m_Left1 = new VictorSP(1);
  private VictorSP m_Right0 = new VictorSP(2);
  private VictorSP m_Right1 = new VictorSP(3);
  private SpeedControllerGroup m_LeftMotors = new SpeedControllerGroup(m_Left0,m_Left1);
  private SpeedControllerGroup m_RightMotors = new SpeedControllerGroup(m_Right0,m_Right1);
  private DifferentialDrive m_Drive = new DifferentialDrive(m_LeftMotors,m_RightMotors);

  private XboxController m_Controller = new XboxController(0);

  private boolean m_LimelightHasValidTarget = false;
  private double m_LimelightDriveCommand = 0.0;
  private double m_LimelightSteerCommand = 0.0;

  @Override
  public void robotInit() {
        m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
        m_chooser.addOption("My Auto", kCustomAuto);
        SmartDashboard.putData("Auto choices", m_chooser);
  }

  @Override
  public void robotPeriodic() {
  }

  @Override
  public void autonomousInit() {
        m_autoSelected = m_chooser.getSelected();
  }

  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void teleopPeriodic() {

        Update_Limelight_Tracking();

        double steer = m_Controller.getX(Hand.kRight);
        double drive = -m_Controller.getY(Hand.kLeft);
        boolean auto = m_Controller.getAButton();

        steer *= 0.70;
        drive *= 0.70;

        if (auto)
        {
          if (m_LimelightHasValidTarget)
          {
                m_Drive.arcadeDrive(m_LimelightDriveCommand,m_LimelightSteerCommand);
          }
          else
          {
                m_Drive.arcadeDrive(0.0,0.0);
          }
        }
        else
        {
          m_Drive.arcadeDrive(drive,steer);
        }
  }

  @Override
  public void testPeriodic() {
  }

  /**
   * Esta función implementa un método simple para generar comandos de conducción y dirección
   * basados en los datos de seguimiento de una cámara Limelight.
   */
  public void Update_Limelight_Tracking()
  {
        // ¡Estos números deben ser ajustados para tu Robot! ¡Ten cuidado!
        final double STEER_K = 0.03;                    // qué tan fuerte girar hacia el objetivo
        final double DRIVE_K = 0.26;                    // qué tan fuerte conducir hacia adelante hacia el objetivo
        final double DESIRED_TARGET_AREA = 13.0;        // Área del objetivo cuando el robot alcanza la pared
        final double MAX_DRIVE = 0.7;                   // Límite de velocidad simple para no conducir demasiado rápido

        double tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
        double tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
        double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
        double ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0);

        if (tv < 1.0)
        {
          m_LimelightHasValidTarget = false;
          m_LimelightDriveCommand = 0.0;
          m_LimelightSteerCommand = 0.0;
          return;
        }

        m_LimelightHasValidTarget = true;

        // Comenzar con dirección proporcional
        double steer_cmd = tx * STEER_K;
        m_LimelightSteerCommand = steer_cmd;

        // intentar conducir hacia adelante hasta que el área objetivo alcance nuestra área deseada
        double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;

        // no dejar que el robot conduzca demasiado rápido hacia la meta
        if (drive_cmd > MAX_DRIVE)
        {
          drive_cmd = MAX_DRIVE;
        }
        m_LimelightDriveCommand = drive_cmd;
  }
}