To test out the mechanical and software concepts of the Mecanum drive, we wrote a simple java program to allow the robot to be controlled from a gamepad. The control method was to allow the right joystick on the gamepad to control translation in x and y, while the left joystick x-axis controls rotation.
This is the sketch of the algorithm to be used:
Motor control in FTC OpModes using the Modern Robotics Motor Controller can be done three ways:
- Direct control of the motor by applying a variable duty cycle voltage (RUN_WITHOUT_ENCODER mode, named for the programming constant used to select it.)
- Encoder controlled, with the motor controller using a PID control system to achieve a specified encoder pulse rate (RUN_WITH_ENCODER mode.)
- Run-to-position, with the motor controller using its PID control system to achieve a specified encoder count (RUN_TO_POSITION mode.)
For this program, we wanted to try the encoder controlled method in order to get more accurate relative speed from each motor to more accurately follow commands. It is also easier under encoder control to get more speed control at lower speeds. This graph, courtesy of Phil Malone, shows why:
The blue graph is RUN_WITHOUT_ENCODER mode, the red graph is RUN_USING_ENCODER mode, and the green graph is RUN_TO_POSITION. The x-axis is the speed variable sent to the motor, while the y-axis shows the actual speed in rpm (for the Neverest 40).
When we first tried to run using encoders, the motor behavior was not what we expected. The motor would not run at all until a certain input threshold was reached, and then the motor would run at full speed. After much research, we discovered that the encoder signal channels A and B were switched. (The signal indication on the back of the motor and the signal documentation for the Modern Robotics motor controller don’t match correctly. When we switched the channel green and yellow wires, the motors behaved properly. Note that the photos showing the Motor Controller connections in the post about the robot hardware are not correct.
Here is the listing of the program. Comments in the program explain the various pieces.
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
/**
* Team 9960 Revision 161027.0
* This program provides driver station control of the Team 9960 Mecanum Drive Prototype.
*
* This robot uses four VEX Mecanum wheels, each direct driven by Neverest 20 motors.
* It is designed as a linear op mode, and uses RUN_WITH_ENCODER motor operation.
*
* The gamepad1 right joystick is used for translation movement, while the left joystick x-axis controls rotation.
*
*/
@TeleOp(name="Mecanum Proto Manual", group="Linear Opmode") // @Autonomous(...) is the other common choice
// @Disabled
public class MecanumProtoManual9960 extends LinearOpMode {
/* Declare OpMode members. */
private ElapsedTime runtime = new ElapsedTime();
DcMotor leftFrontMotor = null;
DcMotor rightFrontMotor = null;
DcMotor leftRearMotor = null;
DcMotor rightRearMotor = null;
// declare motor speed variables
double RF; double LF; double RR; double LR;
// declare joystick position variables
double X1; double Y1; double X2; double Y2;
// operational constants
double joyScale = 0.5;
double motorMax = 0.6; // Limit motor power to this value for Andymark RUN_USING_ENCODER mode
@Override
public void runOpMode() {
telemetry.addData("Status", "Initialized");
telemetry.update();
/* Initialize the hardware variables. Note that the strings used here as parameters
* to 'get' must correspond to the names assigned during the robot configuration
* step (using the FTC Robot Controller app on the phone).
*/
leftFrontMotor = hardwareMap.dcMotor.get("leftFront");
rightFrontMotor = hardwareMap.dcMotor.get("rightFront");
leftRearMotor = hardwareMap.dcMotor.get("leftRear");
rightRearMotor = hardwareMap.dcMotor.get("rightRear");
// Set the drive motor direction:
// "Reverse" the motor that runs backwards when connected directly to the battery
// These polarities are for the Neverest 20 motors
leftFrontMotor.setDirection(DcMotor.Direction.REVERSE);
rightFrontMotor.setDirection(DcMotor.Direction.FORWARD);
leftRearMotor.setDirection(DcMotor.Direction.REVERSE);
rightRearMotor.setDirection(DcMotor.Direction.FORWARD);
// Set the drive motor run modes:
// "RUN_USING_ENCODER" causes the motor to try to run at the specified fraction of full velocity
// Note: We were not able to make this run mode work until we switched Channel A and B encoder wiring into
// the motor controllers. (Neverest Channel A connects to MR Channel B input, and vice versa.)
leftFrontMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightFrontMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
leftRearMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightRearMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Wait for the game to start (driver presses PLAY)
waitForStart();
runtime.reset();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.update();
// Reset speed variables
LF = 0; RF = 0; LR = 0; RR = 0;
// Get joystick values
Y1 = -gamepad1.right_stick_y * joyScale; // invert so up is positive
X1 = gamepad1.right_stick_x * joyScale;
Y2 = -gamepad1.left_stick_y * joyScale; // Y2 is not used at present
X2 = gamepad1.left_stick_x * joyScale;
// Forward/back movement
LF += Y1; RF += Y1; LR += Y1; RR += Y1;
// Side to side movement
LF += X1; RF -= X1; LR -= X1; RR += X1;
// Rotation movement
LF += X2; RF -= X2; LR += X2; RR -= X2;
// Clip motor power values to +-motorMax
LF = Math.max(-motorMax, Math.min(LF, motorMax));
RF = Math.max(-motorMax, Math.min(RF, motorMax));
LR = Math.max(-motorMax, Math.min(LR, motorMax));
RR = Math.max(-motorMax, Math.min(RR, motorMax));
// Send values to the motors
leftFrontMotor.setPower(LF);
rightFrontMotor.setPower(RF);
leftRearMotor.setPower(LR);
rightRearMotor.setPower(RR);
// Send some useful parameters to the driver station
telemetry.addData("LF", "%.3f", LF);
telemetry.addData("RF", "%.3f", RF);
telemetry.addData("LR", "%.3f", LR);
telemetry.addData("RR", "%.3f", RR);
}
}
}