-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathAutonomousDriveBlue.java
More file actions
100 lines (85 loc) · 3.48 KB
/
AutonomousDriveBlue.java
File metadata and controls
100 lines (85 loc) · 3.48 KB
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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
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;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
@Autonomous(name="AutonomousDriveBlue DON'T USE", group="Pushbot")
public class AutonomousDriveBlue extends LinearOpMode {
private DcMotor leftBack = null;
private DcMotor rightBack = null;
private DcMotor leftFront = null;
private DcMotor rightFront = null;
private DcMotor wheel = null;
private ElapsedTime runtime = new ElapsedTime();
public void setPowers(double direction, double speed, double rotation){
double leftBackPower;
double rightBackPower;
double leftFrontPower;
double rightFrontPower;
double forward = Math.cos(direction)*speed;
double side = Math.sin(direction)*speed;
rightBackPower = Range.clip((forward + side + rotation), -1.0, 1.0);
leftBackPower = Range.clip((forward - side - rotation), -1.0, 1.0);
rightFrontPower = Range.clip((forward - side + rotation), -1.0, 1.0);
leftFrontPower = Range.clip((forward + side - rotation), -1.0, 1.0);
leftBack.setPower(leftBackPower);
rightBack.setPower(rightBackPower);
leftFront.setPower(leftFrontPower);
rightFront.setPower(rightFrontPower);
}
@Override
public void runOpMode() {
// ArrayList<double[]> Instructions = new ArrayList<double[]>();
// double instruction1[] = {1, 2, 3};
// Instructions.add(instruction1);
leftBack = hardwareMap.get(DcMotor.class, "left_back");
rightBack = hardwareMap.get(DcMotor.class, "right_back");
leftFront = hardwareMap.get(DcMotor.class, "left_front");
rightFront = hardwareMap.get(DcMotor.class, "right_front");
wheel = hardwareMap.get(DcMotor.class, "wheel");
leftBack.setDirection(DcMotor.Direction.REVERSE);
rightBack.setDirection(DcMotor.Direction.REVERSE);
leftFront.setDirection(DcMotor.Direction.REVERSE);
rightFront.setDirection(DcMotor.Direction.FORWARD);
wheel.setDirection(DcMotor.Direction.FORWARD);
runtime.reset();
waitForStart();
double startTime = getRuntime();
//Backward
leftFront.setPower(-0.5);
rightFront.setPower(-0.5);
leftBack.setPower(-0.5);
rightBack.setPower(-0.5);
while(opModeIsActive() && getRuntime() < startTime + 2){
telemetry.addData("I am", runtime);
}
//Stop
leftFront.setPower(0);
rightFront.setPower(0);
leftBack.setPower(0);
rightBack.setPower(0);
//Wheel movement
wheel.setPower(-0.25);
while(opModeIsActive() && getRuntime() < startTime + 9){
telemetry.addData("I am", runtime);
}
//Right
leftFront.setPower(0.5);
rightFront.setPower(-0.5);
leftBack.setPower(-0.5);
rightBack.setPower(0.5);
while(opModeIsActive() && getRuntime() < startTime + 10.2){
telemetry.addData("I am", runtime);
}
//Forwards
leftFront.setPower(-0.25);
rightFront.setPower(-0.25);
leftBack.setPower(-0.25);
rightBack.setPower(-0.25);
while(opModeIsActive() && getRuntime() < startTime + 10.55){
telemetry.addData("I am", runtime);
}
stop();
}
}