Pular para o conteúdo principal

Robô Deep Space 2019

O jogo FRC 2019 Deep Space tinha alvos de visão acima de muitos dos objetivos. Abaixo você encontrará programas de exemplo completos em Java e Labview que implementam um método simples para dirigir automaticamente em direção a um objetivo no Deep Space.

Estes são programas muito simples e destinados apenas a mostrar o conceito de usar dados de rastreamento do limelight para controlar seu robô. Em cada programa, você pode dirigir seu robô com um gamepad. Se você mantiver o botão 'A' pressionado, e o limelight detectar um alvo válido (dependendo das configurações em seu pipeline), o robô dirigirá automaticamente em direção ao alvo. Tome cuidado para ajustar as várias constantes no código para seu robô específico. Alguns robôs giram ou dirigem mais facilmente que outros, então o ajuste das constantes de controle proporcional deve ser feito caso a caso. Certifique-se de que o robô dirige corretamente usando o controle gamepad antes de habilitar o comportamento de busca do limelight.

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 função implementa um método simples de gerar comandos de direção e esterçamento
   * baseado nos dados de rastreamento de uma câmera limelight.
   */
  public void Update_Limelight_Tracking()
  {
        // Estes números devem ser ajustados para seu Robô! Seja cuidadoso!
        final double STEER_K = 0.03;                    // quão forte deve virar em direção ao alvo
        final double DRIVE_K = 0.26;                    // quão forte deve dirigir em direção ao alvo
        final double DESIRED_TARGET_AREA = 13.0;        // Área do alvo quando o robô alcança a parede
        final double MAX_DRIVE = 0.7;                   // Limite simples de velocidade para não dirigirmos muito 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;

        // Começa com esterçamento proporcional
        double steer_cmd = tx * STEER_K;
        m_LimelightSteerCommand = steer_cmd;

        // tenta dirigir para frente até que a área do alvo alcance nossa área desejada
        double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;

        // não deixa o robô dirigir muito rápido em direção ao objetivo
        if (drive_cmd > MAX_DRIVE)
        {
          drive_cmd = MAX_DRIVE;
        }
        m_LimelightDriveCommand = drive_cmd;
  }
}