Visée et Ajustement de Distance Avec Swerve

Voir le projet d'exemple complet ici

 // contrôle de rotation proportionnel simple avec Limelight.
  // le "contrôle proportionnel" est un algorithme de contrôle dans lequel la sortie est proportionnelle à l'erreur.
  // dans ce cas, nous allons retourner une vitesse angulaire qui est proportionnelle à la 
  // valeur "tx" du Limelight.
  double limelight_aim_proportional()
  {    
    // kP (constante de proportionnalité)
    // c'est un nombre ajusté manuellement qui détermine l'agressivité de notre boucle de contrôle proportionnel
    // s'il est trop élevé, le robot oscillera.
    // s'il est trop bas, le robot n'atteindra jamais sa cible
    // si le robot ne tourne jamais dans la bonne direction, kP doit être inversé.
    double kP = .035;

    // tx varie de (-hfov/2) à (hfov/2) en degrés. Si votre cible est sur le bord le plus à droite de 
    // votre flux limelight 3, tx devrait retourner environ 31 degrés.
    double targetingAngularVelocity = LimelightHelpers.getTX("limelight") * kP;

    // convertir en radians par seconde pour notre méthode de conduite
    targetingAngularVelocity *= Drivetrain.kMaxAngularSpeed;

    // inverser car tx est positif quand la cible est à droite du réticule
    targetingAngularVelocity *= -1.0;

    return targetingAngularVelocity;
  }

  // contrôle de distance proportionnel simple avec la valeur "ty" du Limelight
  // cela fonctionne mieux si la hauteur de montage de votre Limelight et la hauteur de montage de la cible sont différentes.
  // si votre limelight et votre cible sont montés à des hauteurs identiques ou similaires, utilisez "ta" (aire) pour l'ajustement de distance plutôt que "ty"
  double limelight_range_proportional()
  {    
    double kP = .1;
    double targetingForwardSpeed = LimelightHelpers.getTY("limelight") * kP;
    targetingForwardSpeed *= Drivetrain.kMaxSpeed;
    targetingForwardSpeed *= -1.0;
    return targetingForwardSpeed;
  }

  private void drive(boolean fieldRelative) {
    // Obtenir la vitesse x. Nous inversons ceci car les manettes Xbox retournent
    // des valeurs négatives quand nous poussons vers l'avant.
    var xSpeed =
        -m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
            * Drivetrain.kMaxSpeed;

    // Obtenir la vitesse y ou vitesse latérale/strafe. Nous inversons ceci car
    // nous voulons une valeur positive quand nous tirons vers la gauche. Les manettes Xbox
    // retournent des valeurs positives quand vous tirez vers la droite par défaut.
    var ySpeed =
        -m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
            * Drivetrain.kMaxSpeed;

    // Obtenir le taux de rotation angulaire. Nous inversons ceci car nous voulons une
    // valeur positive quand nous tirons vers la gauche (rappelez-vous, le sens antihoraire est positif en
    // mathématiques). Les manettes Xbox retournent des valeurs positives quand vous tirez vers
    // la droite par défaut.
    var rot =
        -m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
            * Drivetrain.kMaxAngularSpeed;

    // pendant que le bouton A est pressé, remplacer certaines valeurs de conduite avec la sortie de nos méthodes limelight
    if(m_controller.getAButton())
    {
        final var rot_limelight = limelight_aim_proportional();
        rot = rot_limelight;

        final var forward_limelight = limelight_range_proportional();
        xSpeed = forward_limelight;

        // lors de l'utilisation du Limelight, désactiver la conduite relative au terrain.
        fieldRelative = false;
    }

    m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
  }