forked from acmerobotics/road-runner-quickstart
-
Notifications
You must be signed in to change notification settings - Fork 0
/
RedFrontstage.java
104 lines (93 loc) · 4.12 KB
/
RedFrontstage.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
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
101
102
103
104
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.Vision.OpenCVRed;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
import org.openftc.easyopencv.OpenCvWebcam;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
@Disabled
@Autonomous (name = "Red Frontstage")
public class RedFrontstage extends LinearOpMode {
OpenCvWebcam webcam;
OpenCVRed pipeline = new OpenCVRed(telemetry);
private DcMotor Arm, Extend = null;
@Override
public void runOpMode()
{
Arm = hardwareMap.get(DcMotor.class, "AE");
Arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
Arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
Arm.setDirection(DcMotor.Direction.REVERSE);
Extend = hardwareMap.get(DcMotor.class, "SE");
Extend.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
Extend.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
Extend.setDirection(DcMotor.Direction.REVERSE);
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
webcam.setPipeline(pipeline);
webcam.setMillisecondsPermissionTimeout(5000); // Timeout for obtaining permission is configurable. Set before opening.
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
{
@Override
public void onOpened()
{webcam.startStreaming(1920, 1080, OpenCvCameraRotation.UPRIGHT);}
@Override
public void onError(int errorCode) {telemetry.clearAll(); telemetry.addLine("Camera failed to open");}
});
telemetry.addLine("Waiting for start");
telemetry.update();
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
//Left
TrajectorySequence Left = drive.trajectorySequenceBuilder(new Pose2d())
.lineToLinearHeading(new Pose2d(25, 9, Math.toRadians(0)))
.back(7)
.strafeRight(10)
.forward(33)
.turn(Math.toRadians(-92))
.forward(84)
.build();
//Middle
TrajectorySequence Middle = drive.trajectorySequenceBuilder(new Pose2d())
.lineToLinearHeading(new Pose2d(31, 0, Math.toRadians(0)))
.back(7)
.strafeLeft(20)
.forward(27)
.turn(Math.toRadians(-92))
.forward(109)
.build();
//Right
TrajectorySequence Right = drive.trajectorySequenceBuilder(new Pose2d())
.lineToLinearHeading(new Pose2d(22, 0, Math.toRadians(0)))
.back(4)
.strafeLeft(14)
.turn(Math.toRadians(-92))
.strafeLeft(8)
.forward(20)
.back(6)
.strafeLeft(26)
.forward(84)
.build();
/*
* Wait for the user to press start on the Driver Station
*/
waitForStart();
String result = pipeline.getResult();
webcam.stopStreaming();
webcam.closeCameraDevice();
if(result=="LEFT") {drive.followTrajectorySequence(Left);};
if(result=="MIDDLE") {drive.followTrajectorySequence(Middle);};
if(result=="RIGHT") {drive.followTrajectorySequence(Right);};
while (opModeIsActive())
{
telemetry.update();
sleep(100);
}
}
}