Aller au contenu principal

Robot Deep Space 2019

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

Ces programmes sont très simples et ne visent qu'à montrer le concept d'utilisation des données de suivi de Limelight pour contrôler votre robot. Dans chaque programme, vous pouvez piloter votre robot avec une manette de jeu. Si vous maintenez le bouton 'A' enfoncé, et que le 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 se déplacent 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 de jeu avant d'activer le comportement de recherche du 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;                    // à quel point tourner vers la cible
        final double DRIVE_K = 0.26;                    // à quel point avancer 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 conduire 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 par 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 souhaitée
        double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;

        // ne pas laisser le robot avancer trop vite vers l'objectif
        if (drive_cmd > MAX_DRIVE)
        {
          drive_cmd = MAX_DRIVE;
        }
        m_LimelightDriveCommand = drive_cmd;
  }
}