Aller au contenu principal

Robot Deep Space 2019

Le jeu FRC 2019 Deep Space comportait des cibles de vision au-dessus de nombreux buts. Vous trouverez ci-dessous des programmes d'exemple complets en Java et Labview qui mettent en œuvre une méthode simple pour conduire automatiquement vers un but dans Deep Space.

Ce sont des programmes très simples destinés uniquement à montrer le concept d'utilisation des données de suivi Limelight pour contrôler votre robot. Dans chaque programme, vous pouvez piloter votre robot avec une manette. Si vous maintenez le bouton 'A' enfoncé et que la Limelight détecte une cible valide (selon les paramètres de votre pipeline), le robot se dirigera automatiquement vers la cible. Veillez à ajuster les différentes constantes du code pour votre robot particulier. Certains robots tournent ou roulent plus facilement que d'autres, donc l'ajustement des constantes de contrôle proportionnel doit être fait au cas par cas. Assurez-vous que le robot se déplace correctement en utilisant la manette avant d'activer le comportement de recherche 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() {
  }

  /**
   * Cette fonction implémente une méthode simple pour générer des commandes de conduite et de direction
   * basées sur les données de suivi d'une caméra Limelight.
   */
  public void Update_Limelight_Tracking()
  {
        // Ces nombres doivent être ajustés pour votre robot ! Soyez prudent !
        final double STEER_K = 0.03;                    // intensité de rotation vers la cible
        final double DRIVE_K = 0.26;                    // intensité d'avancement vers la cible
        final double DESIRED_TARGET_AREA = 13.0;        // Surface de la cible lorsque le robot atteint le mur
        final double MAX_DRIVE = 0.7;                   // Limite de vitesse simple pour ne pas rouler trop vite

        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;

        // Commencer avec la direction proportionnelle
        double steer_cmd = tx * STEER_K;
        m_LimelightSteerCommand = steer_cmd;

        // essayer d'avancer jusqu'à ce que la zone cible atteigne notre zone désirée
        double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;

        // ne pas laisser le robot rouler trop vite vers le but
        if (drive_cmd > MAX_DRIVE)
        {
          drive_cmd = MAX_DRIVE;
        }
        m_LimelightDriveCommand = drive_cmd;
  }
}