-
Notifications
You must be signed in to change notification settings - Fork 0
/
FORWARD_LEFT_Parking.java
72 lines (55 loc) · 2.19 KB
/
FORWARD_LEFT_Parking.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
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import java.util.List;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Hardware;
import com.qualcomm.robotcore.hardware.HardwareMap;
@Autonomous(name="FORWARD LEFT Parking", group="HorusHardware")
@Disabled
public class FORWARD_LEFT_Parking extends LinearOpMode {
//This Autonomous Will make Horus move forward a bit, then Strafe Left
//and Stay in the Parking Zone of the Bridge
HardwareHorus robot = new HardwareHorus(); // Using Horus' hardware
@Override
public void runOpMode(){
telemetry.addData("Status", "Initialize Hardware");
telemetry.update();
robot.init(hardwareMap);
double max = robot.maxpow; // 1.00
double mid = robot.midpow; // 0.75
double min = robot.minpow; // 0.50
// Send telemetry message to signify robot waiting;
telemetry.addData("Status", "Hello! Waiting For Start");
telemetry.update();
// Wait for the game to start (driver presses PLAY)
waitForStart();
/*
List of Autonomous Commands located in HardwareHorus.java
DriveForward(power) StrafeLeft(power)
DriveBackwards(power) StrafeRight(power)
TurnLeft(power) AutoHookDown()
TurnRight(power) AutoHookUp()
GrabPlate() ReleasePlate()
StopAllWheelMotors()
*/
robot.Arm.setPower(mid);
sleep(1000);
robot.Arm.setPower(0.0);
sleep(50);
//Horus Will Wait 15 Seconds to allow other teams to perform their Code
robot.StopAllWheelMotors();
sleep(15000);
//Horus will move Forward towards the center of the field
robot.DriveForward(mid);
sleep(900);
robot.StopAllWheelMotors();
sleep(50);
//Horus will Strafe Left towards under the Alliance Bridge
robot.StrafeLeft(mid);
sleep(1700);
robot.StopAllWheelMotors();
sleep(50);
}
}