-
Notifications
You must be signed in to change notification settings - Fork 0
/
ModernRoboticsTeleop.java
154 lines (117 loc) · 5.3 KB
/
ModernRoboticsTeleop.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
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.TouchSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
@TeleOp(name = "ModernRoboticsTeleop", group = "Iterative Opmode")
@Disabled
public class ModernRoboticsTeleop extends OpMode {
// Declare OpMode members.
private ElapsedTime runtime = new ElapsedTime();
private DcMotor frontLeftMotor = null;
private DcMotor frontRightMotor = null;
private DcMotor backLeftMotor = null;
private DcMotor backRightMotor = null;
/*
* Code to run ONCE when the driver hits INIT
*/
@Override
public void init() {
final double MIDDLEPOSITION180 = 0.0;
//final double MIDDLE_POSITION_CONTINOUS = 0.0;
telemetry.addData("Status", "Initialized");
// Initialize the hardware variables. Note that the strings used here as parameters
// to 'get' must correspond to the names assigned during the robot configuration
// step (using the FTC Robot Controller app on the phone).
frontLeftMotor = hardwareMap.get(DcMotor.class, "frontLeft");
frontRightMotor = hardwareMap.get(DcMotor.class, "frontRight");
backRightMotor = hardwareMap.get(DcMotor.class, "backRight");
backLeftMotor = hardwareMap.get(DcMotor.class, "backLeft");
// Most robots need the motor on one side to be reversed to drive forward
// Reverse the motor that runs backwards when connected directly to the battery
frontLeftMotor.setDirection(DcMotor.Direction.FORWARD);
frontRightMotor.setDirection(DcMotor.Direction.REVERSE);
backLeftMotor.setDirection(DcMotor.Direction.FORWARD);
backRightMotor.setDirection(DcMotor.Direction.REVERSE);
// Tell the driver that initialization is complete.
telemetry.addData("Status", "Initialized");
}
/*
* Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
*/
@Override
public void init_loop() {
}
/*
* Code to run ONCE when the driver hits PLAY
*/
@Override
public void start() {
runtime.reset();
}
boolean bumperPressedHistory = false;
boolean bumperClicked = false;
boolean leftBumperPressedHistory = false;
boolean leftBumperClicked = false;
/*
* Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
*/
@Override
public void loop() {
double collectionPower = 0.0;
double sideways = 0.0;
double rotationPower = 0.0;
double extend = 0.0;
double deliveryPower = 0.0;
boolean barDownPosition;
boolean elbowBent;
double position = 0.0;
double clawPosition = 0.0;
//we set what to do when the motor is not given power, which is to brake completely, instead of coasting
frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
double drive = -gamepad1.left_stick_y;
//drive is what direction we want to move, either forwards, backwards, or neither
double holonomic = -gamepad1.left_stick_x;
//holonomic is what direction we want to move sideways
double turnLeft = gamepad1.right_trigger;
//turnRight is how much we want to turn right
double turnRight = gamepad1.left_trigger;
//turnLeft is how much we want to turn left
boolean halfSpeed = leftBumperClicked == true;
//we are calculating the power to send to each different wheel, which each need their own power since it is calculated in different ways
double frontLeftPower = Range.clip(drive - holonomic + turnRight - turnLeft, -1.0, 1.0);
double frontRightPower = Range.clip(drive + holonomic - turnRight + turnLeft, -1.0, 1.0);
double backRightPower = Range.clip(drive - holonomic - turnRight + turnLeft, -1.0, 1.0);
double backLeftPower = Range.clip(drive + holonomic + turnRight - turnLeft, -1.0, 1.0);
if (halfSpeed) {
frontLeftPower = 0.35 * (frontLeftPower);
frontRightPower = 0.35 * (frontRightPower);
backRightPower = 0.35 * (backRightPower);
backLeftPower = 0.35 * (backLeftPower);
}
// Send calculated power to wheels and motors
frontLeftMotor.setPower(frontLeftPower);
frontRightMotor.setPower(frontRightPower);
backLeftMotor.setPower(backLeftPower);
backRightMotor.setPower(backRightPower);
// Show the elapsed game time
// telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Slow Mode", halfSpeed);
turnLeft = 0;
turnRight = 0;
}
/*
* Code to run ONCE after the driver hits STOP
*/
@Override
public void stop() {
}
}