Mecanum TeleOp

Física del Mecanum

Mecanum drive es un tipo de chasis muy popular en FTC®. La transmisión Mecanum permite el movimiento holonómico. Esto significa que la transmisión es capaz de moverse en cualquier dirección mientras gira: hacia delante, hacia atrás, de lado a lado, trasladándose mientras gira, etc. Aquí hay un video <https://www.youtube.com/watch?v=pP8ajNMx84k>`_ que demuestra este movimiento.

Nota

Algunos kits de chasis mecanumm COTS comunes son el goBILDA Strafer Chassis Kit y el REV Mecanum Drivetrain Kit.

Ruedas Mecanum tienen rodillos con un ángulo de 45 grados respecto al resto de la rueda. Al estar en contacto con el suelo y no con algo sólido como en una rueda de tracción <Traction wheel>, en lugar de crear una fuerza paralela a la orientación de la rueda, crea una a 45 grados de la paralela. Dependiendo de cómo se accionen las ruedas, las componentes X o Y de los vectores de fuerza pueden anularse, lo que permite el movimiento en cualquier dirección.

Vectorización para crear movimientos omnidireccionales

Una configuración de accionamiento mecanum estándar posee 4 ruedas mecanum orientadas en forma de «X». Esto significa que los rodillos están inclinados hacia el centro cuando se mira desde arriba. Esta configuración permite sumar los vectores de fuerza generados por los rodillos desplazados y derivar el movimiento en cualquier dirección. Es importante tener en cuenta que, debido a la fricción, el movimiento perfecto no es posible en todas las direcciones, por lo que un chasis mecanum será capaz de conducir ligeramente más rápido hacia delante/atrás que en cualquier otra dirección. La combinación de traslación y rotación también dará como resultado un movimiento más lento.

En la imagen anterior, los vectores 1, 2, 3 y 4 son los vectores de fuerza creados por las ruedas mecanum cuando se le ordena al chasis que se dirija hacia la parte superior de la imagen. Todos los motores van hacia delante. Las líneas azul y roja son sus componentes X e Y, respectivamente. Aquí hay algunos ejemplos de cómo las ruedas deben ser impulsadas para lograr diferentes movimientos:

Atención

Se recomienda encarecidamente no codificar estos movimientos; hay una forma mucho mejor descrita a continuación que permite un verdadero movimiento holonómico y es mucho más elegante.

Derivación de las ecuaciones de control de Mecanum

Antes de pensar en el mecanum, imagina un escenario en el que tienes una chasis de tanque de 2 motores que quieres controlar utilizando el eje Y del stick izquierdo para el movimiento hacia delante/atrás, y el eje X del stick derecho para el giro pivotante. Los motores están configurados para que el positivo sea en el sentido de las agujas del reloj para el motor derecho cuando el cuerpo está de espaldas a usted, y el motor izquierdo es lo contrario. Para controlar sólo el movimiento hacia delante/atrás, sólo tienes que ajustar las potencias del motor al valor del stick Y (invierte el signo ya que Y está invertido):

Java

double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!

leftMotor.setPower(y);
rightMotor.setPower(y);

Blocks

Aunque al principio añadir la rotación pueda parecer una tarea difícil, en realidad es super sencillo. Todo lo que tienes que hacer es restar el valor del stick X derecho de las ruedas derechas, y sumarlo al izquierdo:

Java

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);

Blocks

Aquí, si el stick izquierdo se pulsa hacia arriba, ambos motores se alimentarán con un valor positivo, haciendo que el robot se mueva hacia delante. Si se pulsa hacia abajo, ambos motores recibirán un valor negativo, lo que hará que el robot retroceda. Un principio similar se aplica a la rotación: si se empuja el stick derecho hacia la derecha, las ruedas izquierdas girarán hacia delante y las derechas hacia atrás, provocando la rotación. Lo contrario ocurre al empujar el stick hacia la izquierda. Si ambos sticks son empujados al mismo tiempo, digamos que el stick Y izquierdo está en 1 y el stick X derecho también está en 1, el valor de las ruedas izquierdas será \(1+1=2\) (que se recorta a 1 en el SDK) y las ruedas derechas serán \(1-1=0\), lo que provoca una curva hacia la derecha.

La aplicación del movimiento omnidireccional con ruedas mecanum funciona según el mismo principio que la adición del giro en el ejemplo del tanque. Los valores X del stick izquierdo se sumarán o restarán a cada rueda dependiendo de cómo tenga que girar esa rueda para obtener el movimiento deseado. La única diferencia con el giro es que en lugar de que las ruedas del mismo lado tengan el mismo signo, las ruedas diagonales entre sí tendrán el mismo signo.

Queremos un valor positivo de X en el stick izquierdo para correlacionarlo con un giro hacia la derecha. Si volvemos a la imagen de vectorización, esto significa que la parte delantera izquierda y trasera derecha necesitan rotar hacia delante, mientras que la parte trasera izquierda y delantera derecha necesitan rotar hacia atrás. Por lo tanto, debemos añadir el valor x a la parte delantera izquierda y trasera derecha y restarlo de la parte trasera derecha y delantera izquierda:

Java

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);

Blocks

Importante

La mayoría de los motores FTC giran en sentido contrario a las agujas del reloj cuando se les da potencia positiva por defecto, con la excepción de los NeveRest. Si su chasis utiliza un número par de engranajes, esto invertirá la dirección de giro de los motores.

En la mayoría de las chasises, tendrá que invertir el lado izquierdo para que la potencia positiva se mueva hacia adelante con la mayoría de los motores, e invertir el lado derecho con NeveRests. La presencia de engranajes entre la caja de engranajes del motor y la rueda puede cambiar esto, que es el caso para el goBILDA Strafer y el REV Mecanum Drivetrain Kit.

Esto es lo mismo que el ejemplo del tanque, excepto que ahora con 4 motores y el componente de ametrallamiento añadido. De forma similar al ejemplo del tanque, el componente Y se añade a todas las ruedas, y el X derecho (rx) se añade a las ruedas izquierdas y se resta de las derechas. Ahora, hemos añadido un componente X izquierdo (x) que nos permite ametrallar hacia la derecha. Al hacer esto, sin embargo, en realidad hemos permitido ametrallar en cualquier dirección. Si lo piensas, pulsar el joystick izquierdo hacia la izquierda hará lo mismo a la inversa, que es lo que se necesita para ametrallar hacia la izquierda. Si se pulsa a 45 grados, los componentes x e y del joystick serán iguales. Esto hará que dos motores diagonales se cancelen, permitiendo el movimiento diagonal. Este mismo efecto se aplica a todos los ángulos del joystick.

Ahora que tenemos un programa de conducción mecanum que funciona, hay algunas cosas que se pueden hacer para limpiarlo. La primera de ellas sería multiplicar el valor X izquierdo por algo para contrarrestar el ametrallamiento imperfecto. Hacer esto hará que la conducción se sienta más precisa en direcciones no alineadas con el eje, y hará que la conducción centrada en el campo sea más precisa. En este tutorial, usaremos 1.1, pero depende de las preferencias del conductor.

Java

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;

Blocks

La otra mejora que podemos hacer es escalar los valores en el rango de -1 a 1.

Dado que el SDK simplemente recorta (limita) las potencias a ese rango, podemos perder la relación que estamos buscando a menos que proactivamente pongamos todos los números de nuevo en ese rango mientras seguimos manteniendo nuestra relación calculada. Por ejemplo, si calculamos valores de 0,4, 0,1, 1,1 y 1,4, se recortarán a 0,4, 0,1, 1,0 y 1,0, que no es la misma relación. En lugar de eso, tenemos que dividirlos todos por el valor absoluto de la potencia mayor cuando supere 1:

Java

// 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;

Blocks

¡Asegúrate de configurar las potencias en tu motor y actualizar esto en cada bucle en un opmode!

Código de muestra final centrado en el robot

Java

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);
        }
    }
}

Blocks

Blocks archivo de descarga

Centrado en el Campo

Con el accionamiento mecanum centrado en el campo, el joystick de traslación controla la dirección del robot en relación con el campo, en contraposición a la estructura del robot. Esto es lo que prefieren algunos conductores, y facilita algunas acciones evasivas, ya que se puede girar mientras se traslada en una dirección determinada más fácilmente. Para ello, los componentes x/y de los joysticks se giran en sentido contrario al ángulo del robot, que viene dado por la IMU.

Hay una IMU dentro de los Control Hubs (y modelos antiguos de los Expansion Hubs). A diferencia de la mayoría del hardware, se recomienda hacer algo más que hardwareMap.get() para empezar a usarlo. Nota, esto se configura al crear una nueva configuración por defecto como imu. Consulta la página FTC doc page covering the IMU interface and its parameters para más información. La forma en que la IMU se inicializará aquí es:

// 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);

El ángulo necesita ser leído en cada bucle. Además de esto, mientras que la IMU mantiene una posición cero consistente entre OpModes (relevantemente, incluyendo entre autónomo y teleop), añadir un bind para reiniciar el ángulo es importante para contrarrestar la deriva y porque el cero puede cambiar debido a algunos tipos de desconexiones.

Nota

Los objetos BNO055 pondrán a cero la IMU cuando se llame a initialize. La clase BNO055 no se recomienda para nuevos desarrollos. La clase IMU no tiene este comportamiento, y es el reemplazo apropiado a partir de 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);

Entonces, los valores del joystick de traslación necesitan ser contrarrotados por el rumbo del robot. La IMU devuelve el rumbo, sin embargo necesitamos rotar el movimiento en sentido contrario a la rotación del robot, por lo que se toma su negativo. Los valores del joystick son un vector, y rotar un vector en 2D requiere esta fórmula (proved here), donde \(x_1\) y \(y_1\) son los componentes del vector original, \(\beta\) es el ángulo de rotación, y \(x_2\) y \(y_2\) son los componentes del vector resultante.

\[\begin{split}x_2=x_1cos \beta - y_1sin \beta \\ y_2=x_1sin \beta + y_1cos \beta\end{split}\]

// 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);

A continuación, estos valores rotados pueden introducirse en la cinemática mecanum mostrada anteriormente.

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;

Código de Muestra Final Centrado en el Campo

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);
        }
    }
}