Mecanum TeleOp¶
Physique de Mécanum¶
Le :terme:` châssis Mecanum <Mecanum Wheel>` est un type de transmission très populaire en FTC. La transmission Mecanum permet un mouvement holonomique. Cela signifie que la transmission est capable de se déplacer dans n’importe quelle direction tout en tournant : vers l’avant, vers l’arrière, d’un côté à l’autre, en translation, tout en tournant, etc. Voici une vidéo intéressante <https://www.youtube.com/watch?v=pP8ajNMx84k>`_ démontrant ce type de mouvement.
Note
Les kits de transmission Mecanum COTS les plus courants sont le goBILDA Strafer Chassis Kit et le REV Mecanum Drivetrain Kit.
Les Roues Mecanum ont des rouleaux à un angle de 45° par rapport au reste de la roue. Comme ils sont en contact avec le sol et non avec quelque chose de solide comme dans une roue de traction, au lieu de créer une force parallèle à l’orientation de la roue, celle-ci en crée une à 45 degrés de la parallèle. Selon la façon dont les roues sont entraînées, les composantes X ou Y des vecteurs de force peuvent s’annuler, ce qui permet un mouvement dans n’importe quelle direction.
Utilisation du vecteur pour créer un mouvement omnidirectionnel¶
Une configuration de chassis mécanum standard possède 4 roues mécanum orientées en forme de « X ». Cela signifie que les rouleaux sont orientés vers le centre lorsqu’on les regarde d’en haut. Cette configuration permet d’additionner les vecteurs de force générés par les rouleaux décalés et d’obtenir un mouvement dans n’importe quelle direction. Il est important de noter qu’en raison du frottement, un mouvement parfait n’est pas possible dans toutes les directions, de sorte qu’un chassis mecanum pourra rouler légèrement plus vite en avant/en arrière que dans n’importe quelle autre direction. La combinaison de la translation et de la rotation entraînera également un mouvement plus lent.
Dans l’image ci-dessus, les vecteurs 1, 2, 3 et 4 sont les vecteurs de force créés par le roues mecanum lorsque le châssis reçoit l’instruction de se diriger vers le haut de l’image. Tous les moteurs sont en marche avant. Les lignes bleues et rouges représentent respectivement les composantes X et Y de ces vecteurs. Voici quelques exemples de la façon dont les roues doivent être actionnées pour réaliser différents mouvements :
Attention
Il est fortement conseillé de ne pas coder en dur ces mouvements ; il existe une bien meilleure méthode, décrite ci-dessous, qui permet un véritable mouvement holonomique et qui est beaucoup plus élégante.
Calcul des équations de contrôle pour mécanum¶
Avant de penser au mecanum, imaginez un scénario dans lequel vous avez un tank à 2 moteurs que vous voulez contrôler en utilisant l’axe Y du manche gauche pour les mouvements avant/arrière, et l’axe X du manche droit pour la rotation. Les moteurs sont configurés de façon à ce que le moteur droit soit positif dans le sens des aiguilles d’une montre lorsque le corps est tourné vers l’extérieur, et le moteur gauche dans le sens inverse. Pour contrôler uniquement les mouvements avant/arrière, il suffit de régler les puissances du moteur sur la valeur du manche Y (inverser le signe puisque Y est inversé) :
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
leftMotor.setPower(y);
rightMotor.setPower(y);
Bien que l’ajout d’une rotation puisse sembler difficile au premier abord, c’est en fait très simple. Tout ce que vous avez à faire est de soustraire la valeur du manche X de droite des roues de droite, et de l’ajouter à celle de gauche :
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double rx = gamepad1.right_stick_x;
leftMotor.setPower(y + rx);
rightMotor.setPower(y - rx);
Ici, si le manche gauche est pressé vers le haut, les deux moteurs seront alimentés par une valeur positive, ce qui fera avancer le robot. S’il est poussé vers le bas, les deux moteurs recevront une valeur négative, ce qui fera reculer le robot. Un principe similaire s’applique à la rotation : si le manche droit est poussé vers la droite, les roues gauches tourneront vers l’avant tandis que les roues droites tourneront vers l’arrière, ce qui provoquera une rotation. L’inverse s’applique à la poussée du manche vers la gauche. Si les deux manche sont poussés en même temps, disons que le manche Y gauche est à 1 et que le manche X droit est également à 1, la valeur des roues gauches sera \(1+1=2\) (qui est ramenée à 1 dans le SDK) et les roues droites seront \(1-1=0\), ce qui provoque une courbe vers la droite.
L’application d’un mouvement omnidirectionnel avec des roues mecanum fonctionne selon le même principe que l’ajout d’une rotation dans l’exemple d’un système de type tank. Les valeurs X du manche gauche seront ajoutées ou soustraites à chaque roue en fonction de la façon dont cette roue doit tourner pour obtenir le mouvement désiré. La seule différence avec le tournage est qu’au lieu que les roues du même côté soient du même signe, les roues en diagonale seront du même signe.
Nous voulons que la valeur X du manche gauche soit positive pour correspondre à un déplacement latéral vers la droite. Si nous nous référons à l’image de vectorisation, cela signifie que l’avant gauche et l’arrière droit doivent tourner vers l’avant, tandis que l’arrière gauche et l’avant droit doivent tourner vers l’arrière. Nous devons donc ajouter la valeur x à l’avant gauche et à l’arrière droit et la soustraire de l’arrière droit et de l’avant gauche :
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;
frontLeftMotor.setPower(y + x + rx);
backLeftMotor.setPower(y - x + rx);
frontRightMotor.setPower(y - x - rx);
backRightMotor.setPower(y + x - rx);
Important
La plupart des moteurs FTC tournent dans le sens inverse des aiguilles d’une montre lorsqu’ils sont alimentés par défaut, à l’exception des NeveRest. Si votre transmission utilise un nombre pair d’engrenages, le sens de rotation des moteurs sera inversé.
Sur la plupart des transmissions, vous devrez inverser le côté gauche pour obtenir une puissance positive afin d’avancer avec la plupart des moteurs, et inverser le côté droit avec les NeveRest. La présence d’un engrenage entre la boîte de vitesses du moteur et la roue peut modifier cette situation, ce qui est le cas pour le goBILDA Strafer et le REV Mecanum Drivetrain Kit.
Il s’agit du même exemple que celui du système de type tank, mais avec 4 moteurs et l’ajout de la composante de déplacement latéral. Comme dans l’exemple du tank, la composante Y est ajoutée à toutes les roues, et la composante X droite (rx) est ajoutée aux roues gauches et soustraite aux roues droites. Nous avons maintenant ajouté une composante X gauche (x) qui nous permet de nous diriger vers la droite. Cependant, en faisant cela, nous avons en fait autorisé le déplacement latéral dans n’importe quelle direction. Si vous y réfléchissez, le fait d’appuyer sur le joystick gauche vers la gauche fera la même chose en sens inverse, ce qui est nécessaire pour virer à gauche. Si l’on appuie sur le joystick à 45 degrés, les composantes x et y du joystick sont égales. Les deux moteurs diagonaux s’annulent alors, ce qui permet de se déplacer en diagonale. Ce même effet s’applique à tous les angles du joystick.
Maintenant que nous avons un programme de conduite mécanum qui fonctionne, il y a plusieurs choses à faire pour l’améliorer. La première d’entre elles serait de multiplier la valeur X de gauche par quelque chose pour contrecarrer le déplacement latéral imparfait. Cela rendra la conduite plus précise sur les directions non alignées sur l’axe, et rendra la conduite centrée sur le champ plus précise. Dans ce tutoriel, nous utiliserons la version 1.1, mais c’est une question de préférence pour le pilote.
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
L’autre amélioration que nous pouvons apporter est une mise à l’échelle des valeurs dans la plage de -1 à 1.
Étant donné que le SDK se contente d’écrêter (limiter) les puissances à cette plage, nous pouvons perdre le rapport que nous recherchons à moins que nous ne ramenions de manière proactive tous les nombres dans cette plage tout en conservant notre rapport calculé. Par exemple, si nous calculons des valeurs de 0,4, 0,1, 1,1 et 1,4, elles seront ramenées à 0,4, 0,1, 1,0 et 1,0, ce qui ne correspond pas au même rapport. Au lieu de cela, nous devons les diviser par la valeur absolue de la plus grande puissance lorsqu’elle est supérieure à 1 :
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
Veillez à définir les puissances de votre moteur et à mettre à jour chaque boucle dans un mode opmode !
Exemple de Code Final Centré sur le Robot¶
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@TeleOp
public class MecanumTeleOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
// Declare our motors
// Make sure your ID's match your configuration
DcMotor frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor");
DcMotor backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor");
DcMotor frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor");
DcMotor backRightMotor = hardwareMap.dcMotor.get("backRightMotor");
// Reverse the right side motors. This may be wrong for your setup.
// If your robot moves backwards when commanded to go forwards,
// reverse the left side instead.
// See the note about this earlier on this page.
frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
frontLeftMotor.setPower(frontLeftPower);
backLeftMotor.setPower(backLeftPower);
frontRightMotor.setPower(frontRightPower);
backRightMotor.setPower(backRightPower);
}
}
}
Centré sur le terrain¶
Dans le cas d’une conduite mécanique centrée sur le champ, le manche de translation contrôle la direction du robot par rapport au terrain, et non par rapport au châssis du robot. Certains conducteurs préfèrent cette solution, qui facilite certaines manœuvres d’évitement, car il est plus facile de tourner tout en se déplaçant dans une direction donnée. Pour ce faire, les composantes x/y des manches sont tournées dans le sens inverse de l’angle du robot, qui est donné par l’IMU.
Les Control Hubs (et les anciens modèles d’Expansion Hubs) contiennent un IMU. Contrairement à la plupart des autres matériels, il est recommandé de faire plus que hardwareMap.get() pour commencer à l’utiliser. Notez que ceci est configuré lors de la création d’une nouvelle configuration par défaut en tant que imu. Voir la page de la doc FTC couvrant l’interface IMU et ses paramètres <https://ftc-docs.firstinspires.org/programming_resources/imu/imu.html>`_ pour plus d’informations. La façon dont l’IMU sera initialisé ici est la suivante :
// Retrieve the IMU from the hardware map
imu = hardwareMap.get(IMU.class, "imu");
// Adjust the orientation parameters to match your robot
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
// Without this, the REV Hub's orientation is assumed to be logo up / USB forward
imu.initialize(parameters);
L’angle doit être lu à chaque boucle. En outre, bien que l’IMU conserve une position zéro cohérente entre les modes opératoires (notamment entre les modes autonome et téléopération), il est important d’ajouter une contrainte pour réinitialiser l’angle afin de contrecarrer la dérive et parce que le zéro peut changer en raison de certains types de déconnexions.
Note
Les objets BNO055 remettent à zéro l’IMU lorsque initialize est appelé. La classe BNO055 n’est pas recommandée pour les nouveaux développements. La classe IMU n’a pas ce comportement, et est le remplacement approprié à partir du SDK v8.1.
// This button choice was made so that it is hard to hit on accident,
// it can be freely changed based on preference.
// The equivalent button is start on Xbox-style controllers.
if (gamepad1.options) {
imu.resetYaw();
}
double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
Ensuite, les valeurs de translation du manche doivent être contrariées par le cap du robot. L’IMU renvoie le cap, mais nous devons faire tourner le mouvement dans le sens inverse de la rotation du robot, donc sa valeur négative est prise. Les valeurs du joystick sont un vecteur, et la rotation d’un vecteur en 2D nécessite cette formule (proved here), où \(x_1\) et \(y_1\) sont les composantes du vecteur original, \(\beta\) est l’angle de rotation, et \(x_2\) et \(y_2\) sont les composantes du vecteur résultant.
// Rotate the movement direction counter to the bot's rotation
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
Ensuite, ces valeurs tournées peuvent être introduites dans la cinématique du mécanisme présentée plus haut.
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
Exemple de Code Final Centré sur le Terrain¶
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
@TeleOp
public class FieldCentricMecanumTeleOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
// Declare our motors
// Make sure your ID's match your configuration
DcMotor frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor");
DcMotor backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor");
DcMotor frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor");
DcMotor backRightMotor = hardwareMap.dcMotor.get("backRightMotor");
// Reverse the right side motors. This may be wrong for your setup.
// If your robot moves backwards when commanded to go forwards,
// reverse the left side instead.
// See the note about this earlier on this page.
frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
// Retrieve the IMU from the hardware map
IMU imu = hardwareMap.get(IMU.class, "imu");
// Adjust the orientation parameters to match your robot
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
// Without this, the REV Hub's orientation is assumed to be logo up / USB forward
imu.initialize(parameters);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;
// This button choice was made so that it is hard to hit on accident,
// it can be freely changed based on preference.
// The equivalent button is start on Xbox-style controllers.
if (gamepad1.options) {
imu.resetYaw();
}
double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
// Rotate the movement direction counter to the bot's rotation
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
rotX = rotX * 1.1; // Counteract imperfect strafing
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
frontLeftMotor.setPower(frontLeftPower);
backLeftMotor.setPower(backLeftPower);
frontRightMotor.setPower(frontRightPower);
backRightMotor.setPower(backRightPower);
}
}
}