Drive By Encoders

Autonomous Navigation with Motor Encoders

Part 1: Plugging in Your Encoders:

REV Expansion Hub with AndyMark Motors

REV Expansion Hub with CORE HEX Motors

Modern Robotics

Part 2: Autonomous OpMode with Encoders

- Create a new file by selecting the “+” button:

- Use the settings shown below:

- Since this is an autonomous program, you will want to REMOVE the main loop:

- Next, you will want to add an “encoderDrive” method to the area OUTSIDE the runOpmode method, but inside the main java class.

- Copy and paste the following method into the highlighted space in your encoder program above:

public void encoderDrive(double speed, double leftInches, double rightInches, double timeoutS)  {
int newLeftTarget;
int newRightTarget;
 // Ensure that the opmode is still active
 if (opModeIsActive()) {
 // Determine new target position, and pass to motor controller
 newLeftTarget = leftDrive.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);  newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
 leftDrive.setTargetPosition(newLeftTarget);
 rightDrive.setTargetPosition(newRightTarget);
 // Turn On RUN_TO_POSITION
 leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 // reset the timeout time and start motion.
 runtime.reset();
 leftDrive.setPower(Math.abs(speed));
 rightDrive.setPower(Math.abs(speed));
 /* keep looping while we are still active, and there is time left, and both motors are running.  *Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor  * hits target position, the motion will stop. This is "safer" in the event that the robot will  * always end the motion as soon as possible.
*However, if you require that BOTH motors have finished their moves before the robot  *continues onto the next step, use (isBusy() || isBusy()) in the loop test.
*/
 while (opModeIsActive() && (runtime.seconds() < timeoutS) &&
(leftDrive.isBusy() && rightDrive.isBusy()))  
{
 // Display it for the driver.
 telemetry.addData("Path1", "Running to %7d :%7d", newLeftTarget, newRightTarget);  telemetry.addData("Path2", "Running at %7d :%7d",
 leftDrive.getCurrentPosition(),
 rightDrive.getCurrentPosition());
 telemetry.update();
 }
 // Stop all motion;
 leftDrive.setPower(0);
 rightDrive.setPower(0);
 // Turn off RUN_TO_POSITION
 leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 // sleep(250); // optional pause after each move
 }
 }
- Finally, you will want to add a bunch of constants to the section of code just below the space where  you create your motor objects.  
private ElapsedTime runtime = new ElapsedTime();
static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder
static final double DRIVE_GEAR_REDUCTION = 2.0 ; // This is < 1.0 if geared UP
static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /  (WHEEL_DIAMETER_INCHES * 3.1415);

Part 3: Setting Your Constants Right

- Measure the diameter of your wheel and change its value in the list of variables below… - You will also need to change the COUNTS_PER_MOTOR_REV value to match your motors.

Motor	NevRest 20	NevRest 40	NevRest 60	Tetrix	REV COREHEX
Counts Per Rev	560	1120	1680	1440	288

- Finally, you will need to calculate your DRIVE_GEAR_REDUCTION. To do this, you will need to take the number of gear/sprocket teeth on your wheel and divide them by the number of gear/sprocket teeth on your motor shaft.

For example, you might have 30 teeth on your wheel and 20 on your motor, so your DRIVE_GEAR_REDUCTION would be: 30 ��������ℎ

20 ��������ℎ= 1.5

- At this point, you should build your OpMode again and measure your robot’s path. If your constants are correct, then the number of inches you put in the code should match the inches your robot travelled.

Part 4: Making your Robot Go Where YOU Want

- As you can see above, the encoderDrive() method accepts a few parameters.

o Speed

o Left Inches

o Right Inches

o Timeout*

So if I wanted to drive forward for 48 inches, at half speed I would call:

encoderDrive(.5,48,48,5);

*The Timeout variable just makes sure the motors don’t stall out for a super long amount of time if they can’t spin.

You can call this method over and over again with new distances and speeds. A turn might look like this: encoderDrive(.5,9,-9,2);

Backing up:

encoderDrive(.5,-24,-24,6);

It is worth noting that the inches values will only be accurate if you have the correct constant variables entered for your motors, wheels, and gear drive train.

Using some trial and error (and some calculations) do your best to make your robot navigate the courses set up by your instructor using your motor encoders.