-
Notifications
You must be signed in to change notification settings - Fork 0
/
DriveSquare.java
74 lines (51 loc) · 2.15 KB
/
DriveSquare.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
// simple autonomous program that drives bot in a square pattern then ends.
// this code assumes it will end before the period is over but if the period ended while
// still driving, this code would just stop.
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
@Autonomous(name="Drive Square", group="Exercises")
//@Disabled
public class DriveSquare extends LinearOpMode
{
DcMotor leftMotor;
DcMotor rightMotor;
// called when init button is pressed.
@Override
public void runOpMode() throws InterruptedException
{
leftMotor = hardwareMap.dcMotor.get("left_motor");
rightMotor = hardwareMap.dcMotor.get("right_motor");
leftMotor.setDirection(DcMotor.Direction.REVERSE);
telemetry.addData("Mode", "waiting");
telemetry.update();
// wait for start button.
waitForStart();
telemetry.addData("Mode", "running");
telemetry.update();
sleep(500); // wait so that above telemetry is visible.
// each iteration of this for loop will drive one side of the square.
for(int i = 0; i < 4; i++)
{
telemetry.addData("Mode", "driving side " + (i + 1));
telemetry.update();
leftMotor.setPower(0.25);
rightMotor.setPower(0.25);
sleep(1000); // drive straight for 1 second.
leftMotor.setPower(0.0);
rightMotor.setPower(0.0);
sleep(500); // wait half second for bot to stop moving.
// now set motors, one forward one reverse. Should cause the bot to rotate.
leftMotor.setPower(0.25);
rightMotor.setPower(-0.25);
sleep(1700); // adjust this delay to get the bot to rotate 90 degrees.
leftMotor.setPower(0.0);
rightMotor.setPower(0.0);
sleep(500); // wait for bot to stop moving.
}
// make sure the motors are off.
rightMotor.setPower(0);
leftMotor.setPower(0);
}
}