Search…
Robot Navigation - OnBot Java

Introduction to Robot Navigation

As alluded to in the Hello Robot - Robot Control section, robot control comes in many different forms. One of the control types to consider for robots with drivetrains, is robot navigation.
Robot navigation as a concept is dependent on the type of drivetrain and the type of operation mode. For instance, the code to control a mecanum drivetrain differs from the code used to control a differential drivetrain. There is also a difference between coding for teleoperated driving, with a gamepad, or coding for autonomous, where each movement of the robot must be defined within code.
The following section goes through some of the basics of programming for a differential drivetrain, as well as how to set up a teleoperated arcade style drivetrain code. The concepts and logic highlighted in this section will be applicable the autonomous control section Elapsed Time.
Sections
Goals of Section

Basics of Programming Drivetrains

Programming Drivetrain Motors

Start by creating a basic op mode called DualDrive.
Visit the Test Bed - OnBot Java section for more information on creating an op mode. The op mode below focuses on hardware mapping only the relevant drivetrain motors.
1
package org.firstinspires.ftc.teamcode;
2
3
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
5
import com.qualcomm.robotcore.hardware.DcMotor;
6
import com.qualcomm.robotcore.hardware.DcMotorSimple;
7
8
@TeleOp
9
public class DualDrive extends LinearOpMode {
10
private DcMotor rightmotor;
11
private DcMotor leftmotor;
12
13
@Override
14
public void runOpMode() {
15
16
rightmotor = hardwareMap.get(DcMotor.class, "rightmotor");
17
leftmotor = hardwareMap.get(DcMotor.class, "leftmotor");
18
19
waitForStart();
20
21
while (opModeIsActive()) {
22
23
}
24
}
25
}
Copied!
Since the focus of this section is creating a functional drivetrain in code, lets started by adding rightmotor.setPower(1); and leftmotor.setPower(1); to the op more while loop.
1
while (opModeIsActive()) {
2
rightmotor.setPower(1);
3
leftmotor.setPower(1);
4
}
Copied!
Before moving on try running the code as is and consider the following questions:
  • What behavior is the robot exhibiting?
  • What direction is the robot spinning in?
When motors run at different speeds they spin along their center pivot point. But the motors are both set to a power (or duty cycle) of 1?
DC Motors are capable of spinning in two different directions depending on the current flow: clockwise and counter clockwise. When using a positive power value the Control Hub sends current to the motor for it to spin in a clockwise direction.
With the Class Bot and current code, both motors are currently set to run in the clockwise direction. If you set the robot on blocks and run the code again though, you can see that the motors run in opposing directions. With the mirrored way the motors mount to the drivetrain, one motor is naturally the inverse of the other.
Why would the inverse motor cause the robot to spin in a circle? Both speed and direction of rotation of the wheels impact the overall direction the robot moves in. In this case, both motors were assigned to have the same power and direction but how the motors transfer motion to the wheels, causes the robot to spin instead of moving forward.
Check the Introduction to Motion section for more information on the mechanics of transferring motion and power.
In the info block above you were asked to determine which direction the robot spun in. The robot pivots in the direction of the inversed motor. For instance, when the right motor is the inversed motor the robot will pivot to the right. If the left motor is the inversed motor the robot will pivot to the left.

The Affect of Drivetrain Motors on Drivetrain Movement

For the Class Bot, the robot pivots to the right, so the right motor will be reversed. Add the line rightmotor.setDirection(DcMotorSimple.Direction.REVERSE); to the op mode under the variable declarations.
1
public void runOpMode() {
2
float x;
3
double y;
4
5
rightmotor = hardwareMap.get(DcMotor.class, "rightmotor");
6
leftmotor = hardwareMap.get(DcMotor.class, "leftmotor");
7
8
rightmotor.setDirection(DcMotorSimple.Direction.REVERSE);
9
10
waitForStart();
11
12
while (opModeIsActive()) {
13
rightmotor.setPower(1);
14
leftmotor.setPower(1);
15
}
16
}
Copied!
Adding the rightmotor.setDirection(DcMotorSimple.Direction.REVERSE); code line reverses (or inverses) the direction of the right motor. Both motors now consider the same direction forward an

Teleoperated Driving - Arcade Style

Recall that when the motors were running in opposing directions the robot spun in circles. This same logic will be used to control the robot using the arcade style of control mentioned in the Hello Robot - Autonomous Robot section.

Programming with gamepads

To start, create two variables
xx
and
yy
. Both variables will be doubles.
1
public void runOpMode() {
2
double x;
3
double y;
4
5
rightmotor = hardwareMap.get(DcMotor.class, "rightmotor");
6
leftmotor = hardwareMap.get(DcMotor.class, "leftmotor");
7
8
rightmotor.setDirection(DcMotorSimple.Direction.REVERSE);
9
10
waitForStart();
Copied!
Assign
yy
as y = -gamepad1.right_stick_y; , which is the y-axis of the right joystick.
Remember positive/negative values inputted by the gamepad's y-axis are inverse of the positive/negative values of the motor.
Assign the
xx
as the x = gamepad1.right_stick_x;, which is the x axis of the right gamepad joystick. The x-axis of the joystick does not need to be inverted.
1
while (opModeIsActive()) {
2
x = gamepad1.right_stick_x;
3
y = -gamepad1.right_stick_y;
4
5
rightmotor.setPower(1);
6
leftmotor.setPower(1);
7
}
Copied!
Setting x = gamepad1.right_stick_x; and y = -gamepad1.right_stick_y; assigns values from the gamepad joystick to
xx
and
yy
. As previously mentioned, the joystick gives values along a two dimension coordinate system.
yy
receives the value from the y- axis and
xx
receives the value from the x-axis. Both axis output values between -1 and 1.
To better understand consider the following table. The table shows the expected value generated from moving the joystick all the way in one direction, along the axis. For instance, when the joystick is pushed all the way in the upwards direction the coordinate values are (0,1).
The table below assumes that the the
yy
values from the gamepad have been inverted in code when assigned to the variable
yy
.
Joystick Direction
xx
yy
0
1
0
-1
-1
0
1
0
Now that you have a better understanding of how the physical movement of the gamepad affects the numerical inputs given to your Control System; its time to consider how to control the drivetrain using the joystick.
Recall, from the Programming Drivetrain Motors section, that the speed and direction of a motor plays a large part in how the drivetrain moves.
The numerical outputs for setPowerdetermine the speed and direction of the motors. For instance, when both motors are set to 1 they move in the forward direction at full speed (or 100% of duty cycle). Much like the gamepads, the numerical value for setPoweris in a range of -1 to 1. The absolute value of the assigned number determines percentage of duty cycle. As an example, 0.3 and -0.3 both indicate that the motor is operating at a duty cycle of 30%. The sign of the number indicates the direction the motor is rotating in. To better understand, consider the following graphic.
When a motor is assigned a setPower value between -1 and 0, the motor will rotate in the direction it considers to be reverse. When a motor is assigned a value between 0 and 1, it will rotate forward.
In the Programming Drivetrain Motors section, it was discussed that a robot rotates when the motors are moving in opposing directions. However, this has more to do with both speed and direction. To think of it numerically, a differential drivetrain will turn to the right when the setPowervalue for the right motor is less than that of the left motor. This is exhibited in the following example.
When both motors are rightmotor.setPower(1); leftmotor.setPower(1);the robot will run at full speed in a mostly straight line. However, when the rightmotor is running in the same direction but at a lower speed, such as rightmotor.setPower(0.3); leftmotor.setPower(1); the robot will turn or rotate to the right. This is likely to be an arching movement that is not as sharp as a full pivot. In contrast when the rightmotor is set to full speed but in the opposite direction of the leftmotor, the robot pivots to the right. So, mathematically the following is considered to be true:
rightmotor.setPower = leftmotor.setPower
Forward or Reverse
rightmotor.setPower > leftmotor.setPower
Left Turn
rightmotor.setPower < leftmotor.setPower
Right Turn
As previously implied, gamepad1.right_stick_y and gamepad1.right_stick_xsend values to the Control System from the game pad joystick. In contrast, the setPower function interprets numerical information set in the code and sends the appropriate current to the motors to dictate how the motors behave.
In an arcade drive, the following joy stick inputs (directions) need to correspond with the following outputs (motor power values).
Joystick Direction
(X,Y)
rightmotor
leftmotor
(0,1)
1
1
(0,-1)
-1
-1
(-1,0)
1
-1
(1,0)
-1
1
To get the outputs expressed in the table above, the gamepad values must be assigned to each motor in a meaningful way, where. Algebraic principles can be used to determine the two formulas needed to get the values. However, the formulas are provided below.
rightmotor=yxleftmotor=y+xrightmotor = y-x \\ leftmotor = y+x
Rather than setPower(1); both the motors can be set to the above formulas. For instance, the right motor can be set as rightmotor.setPower(y-x);.
1
while (opModeIsActive()) {
2
x = gamepad1.right_stick_x;
3
y = -gamepad1.right_stick_y;
4
5
rightmotor.setPower(y-x);
6
leftmotor.setPower(y+x);
7
}
Copied!
With this you now have a functional teleoperated arcade drive. From here you can start adding hardware mapping for the other pieces of robot hardware. Below is an outline of the expected code for the Class Bot with full hardware mapping.
1
package org.firstinspires.ftc.teamcode;
2
3
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4
import com.qualcomm.robotcore.hardware.Blinker;
5
import com.qualcomm.robotcore.hardware.Servo;
6
import com.qualcomm.robotcore.hardware.Gyroscope;
7
import com.qualcomm.robotcore.hardware.DigitalChannel;
8
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
9
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
10
import com.qualcomm.robotcore.hardware.DcMotor;
11
import com.qualcomm.robotcore.hardware.DcMotorSimple;
12
import com.qualcomm.robotcore.util.ElapsedTime;
13
14
@TeleOp
15
16
public class DualDrive extends LinearOpMode {
17
private Blinker control_Hub;
18
private DcMotor arm;
19
private Servo claw;
20
private Gyroscope imu;
21
private DcMotor leftmotor;
22
private DcMotor rightmotor;
23
private DigitalChannel touch;
24
25
26
@Override
27
public void runOpMode() {
28
double x;
29
double y;
30
31
control_Hub = hardwareMap.get(Blinker.class, "Control Hub");
32
arm = hardwareMap.get(DcMotor.class, "arm");
33
claw = hardwareMap.get(Servo.class, "claw");
34
imu = hardwareMap.get(Gyroscope.class, "imu");
35
leftmotor = hardwareMap.get(DcMotor.class, "leftmotor");
36
rightmotor = hardwareMap.get(DcMotor.class, "rightmotor");
37
touch = hardwareMap.get(DigitalChannel.class, "touch");
38
39
rightmotor.setDirection(DcMotorSimple.Direction.REVERSE);
40
41
telemetry.addData("Status", "Initialized");
42
telemetry.update();
43
44
// Wait for the game to start (driver presses PLAY)
45
waitForStart();
46
47
// run until the end of the match (driver presses STOP)
48
while (opModeIsActive()) {
49
x = gamepad1.right_stick_x;
50
y = -gamepad1.right_stick_y;
51
52
rightmotor.setPower(y-x);
53
leftmotor.setPower(y+x);
54
55
telemetry.addData("Status", "Running");
56
telemetry.update();
57
58
}
59
}
Copied!