Skip to content

Commit ad21025

Browse files
committed
update to most recent version 9.0.1
1 parent f326c0d commit ad21025

File tree

5 files changed

+337
-131
lines changed

5 files changed

+337
-131
lines changed
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
package org.firstinspires.ftc.teamcode;
2+
3+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4+
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
5+
import com.qualcomm.robotcore.hardware.CRServo;
6+
import com.qualcomm.robotcore.hardware.Servo;
7+
8+
@TeleOp
9+
public class DozerOneStick extends LinearOpMode {
10+
11+
private CRServo backLeft;
12+
private CRServo backRight;
13+
private Servo eyes;
14+
private Servo plowLeft;
15+
private Servo plowRight;
16+
17+
@Override
18+
public void runOpMode() {
19+
double y;
20+
double clockwise;
21+
double bl;
22+
double br;
23+
int speed = 1;
24+
boolean shouldSlowmode = false;
25+
boolean slowmodeChanged = false;
26+
double plowPos = 0.1;
27+
boolean eyesOnTurn = true;
28+
boolean eyesChanged = false;
29+
30+
backLeft = hardwareMap.get(CRServo.class, "backLeft");
31+
backRight = hardwareMap.get(CRServo.class, "backRight");
32+
eyes = hardwareMap.get(Servo.class, "eyes");
33+
plowLeft = hardwareMap.get(Servo.class, "plowLeft");
34+
plowRight = hardwareMap.get(Servo.class, "plowRight");
35+
36+
backLeft.setDirection(CRServo.Direction.REVERSE);
37+
plowLeft.setDirection(Servo.Direction.REVERSE);
38+
39+
eyes.setPosition(0.5);
40+
plowLeft.setPosition(plowPos);
41+
plowRight.setPosition(plowPos);
42+
43+
waitForStart();
44+
if (opModeIsActive()) {
45+
while (opModeIsActive()) {
46+
clockwise = (double) gamepad1.left_stick_x;
47+
y = (double) -gamepad1.left_stick_y;
48+
49+
if (gamepad1.dpad_up) {
50+
y = 1.0;
51+
} else if (gamepad1.dpad_down) {
52+
y = -1.0;
53+
}
54+
55+
if (gamepad1.dpad_right) {
56+
clockwise = 1.0;
57+
} else if (gamepad1.dpad_left) {
58+
clockwise = -1.0;
59+
}
60+
61+
bl = y + clockwise;
62+
br = y - clockwise;
63+
64+
if (gamepad1.right_bumper) {
65+
speed = 2;
66+
} else if (gamepad1.left_bumper) {
67+
speed = 4;
68+
} else if (gamepad1.start) {
69+
if (!slowmodeChanged) {
70+
shouldSlowmode = !shouldSlowmode;
71+
slowmodeChanged = true;
72+
}
73+
} else if (slowmodeChanged) {
74+
slowmodeChanged = false;
75+
} else {
76+
speed = shouldSlowmode ? 2 : 1;
77+
}
78+
79+
bl /= speed;
80+
br /= speed;
81+
82+
backLeft.setPower(bl);
83+
backRight.setPower(br);
84+
85+
if (gamepad1.back) {
86+
if (!eyesChanged) {
87+
eyesOnTurn = !eyesOnTurn;
88+
eyesChanged = true;
89+
}
90+
} else if (eyesChanged){
91+
eyesChanged = false;
92+
}
93+
94+
if (eyesOnTurn) {
95+
eyes.setPosition(0.3 * gamepad1.left_stick_x + 0.5);
96+
} else if (gamepad1.right_stick_x != 0) {
97+
if (eyes.getPosition() < 0.2) {
98+
eyes.setPosition(0.2);
99+
} else if (eyes.getPosition() > 0.8) {
100+
eyes.setPosition(0.8);
101+
} else {
102+
eyes.setPosition(eyes.getPosition() + 0.01 * gamepad1.right_stick_x);
103+
sleep(8);
104+
}
105+
}
106+
107+
if (gamepad1.right_stick_y != 0) {
108+
if (plowPos < 0.075) {
109+
plowPos = 0.075;
110+
} else if (plowPos > 0.6) {
111+
plowPos = 0.6;
112+
} else {
113+
plowPos += 0.01 * -gamepad1.right_stick_y;
114+
sleep(8);
115+
}
116+
}
117+
plowLeft.setPosition(plowPos);
118+
plowRight.setPosition(plowPos);
119+
120+
telemetry.update();
121+
}
122+
}
123+
}
124+
}
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
package org.firstinspires.ftc.teamcode;
2+
3+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4+
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
5+
import com.qualcomm.robotcore.hardware.CRServo;
6+
import com.qualcomm.robotcore.hardware.Servo;
7+
8+
@TeleOp
9+
public class DozerOneStickReversed extends LinearOpMode {
10+
11+
private CRServo backLeft;
12+
private CRServo backRight;
13+
private Servo eyes;
14+
private Servo plowLeft;
15+
private Servo plowRight;
16+
17+
@Override
18+
public void runOpMode() {
19+
double y;
20+
double clockwise;
21+
double bl;
22+
double br;
23+
int speed = 1;
24+
boolean shouldSlowmode = false;
25+
boolean slowmodeChanged = false;
26+
double plowPos = 0.1;
27+
boolean eyesOnTurn = true;
28+
boolean eyesChanged = false;
29+
30+
backLeft = hardwareMap.get(CRServo.class, "backLeft");
31+
backRight = hardwareMap.get(CRServo.class, "backRight");
32+
eyes = hardwareMap.get(Servo.class, "eyes");
33+
plowLeft = hardwareMap.get(Servo.class, "plowLeft");
34+
plowRight = hardwareMap.get(Servo.class, "plowRight");
35+
36+
backLeft.setDirection(CRServo.Direction.REVERSE);
37+
plowLeft.setDirection(Servo.Direction.REVERSE);
38+
39+
eyes.setPosition(0.5);
40+
plowLeft.setPosition(plowPos);
41+
plowRight.setPosition(plowPos);
42+
43+
waitForStart();
44+
if (opModeIsActive()) {
45+
while (opModeIsActive()) {
46+
clockwise = (double) gamepad1.right_stick_x;
47+
y = (double) -gamepad1.right_stick_y;
48+
49+
if (gamepad1.dpad_up) {
50+
y = 1.0;
51+
} else if (gamepad1.dpad_down) {
52+
y = -1.0;
53+
}
54+
55+
if (gamepad1.dpad_right) {
56+
clockwise = 1.0;
57+
} else if (gamepad1.dpad_left) {
58+
clockwise = -1.0;
59+
}
60+
61+
bl = y + clockwise;
62+
br = y - clockwise;
63+
64+
if (gamepad1.right_bumper) {
65+
speed = 2;
66+
} else if (gamepad1.left_bumper) {
67+
speed = 4;
68+
} else if (gamepad1.start) {
69+
if (!slowmodeChanged) {
70+
shouldSlowmode = !shouldSlowmode;
71+
slowmodeChanged = true;
72+
}
73+
} else if (slowmodeChanged) {
74+
slowmodeChanged = false;
75+
} else {
76+
speed = shouldSlowmode ? 2 : 1;
77+
}
78+
79+
bl /= speed;
80+
br /= speed;
81+
82+
backLeft.setPower(bl);
83+
backRight.setPower(br);
84+
85+
if (gamepad1.back) {
86+
if (!eyesChanged) {
87+
eyesOnTurn = !eyesOnTurn;
88+
eyesChanged = true;
89+
}
90+
} else if (eyesChanged){
91+
eyesChanged = false;
92+
}
93+
94+
if (eyesOnTurn) {
95+
eyes.setPosition(0.3 * gamepad1.right_stick_x + 0.5);
96+
} else if (gamepad1.left_stick_x != 0) {
97+
if (eyes.getPosition() < 0.2) {
98+
eyes.setPosition(0.2);
99+
} else if (eyes.getPosition() > 0.8) {
100+
eyes.setPosition(0.8);
101+
} else {
102+
eyes.setPosition(eyes.getPosition() + 0.01 * gamepad1.left_stick_x);
103+
sleep(8);
104+
}
105+
}
106+
107+
if (gamepad1.left_stick_y != 0) {
108+
if (plowPos < 0.075) {
109+
plowPos = 0.075;
110+
} else if (plowPos > 0.6) {
111+
plowPos = 0.6;
112+
} else {
113+
plowPos += 0.01 * -gamepad1.left_stick_y;
114+
sleep(8);
115+
}
116+
}
117+
plowLeft.setPosition(plowPos);
118+
plowRight.setPosition(plowPos);
119+
120+
telemetry.update();
121+
}
122+
}
123+
}
124+
}
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
package org.firstinspires.ftc.teamcode;
2+
3+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
4+
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
5+
import com.qualcomm.robotcore.hardware.CRServo;
6+
import com.qualcomm.robotcore.hardware.DcMotor;
7+
import com.qualcomm.robotcore.hardware.Servo;
8+
9+
@TeleOp
10+
public class DozerTwoStick extends LinearOpMode {
11+
12+
private CRServo backLeft;
13+
private CRServo backRight;
14+
private Servo eyes;
15+
private Servo plowLeft;
16+
private Servo plowRight;
17+
18+
@Override
19+
public void runOpMode() {
20+
double y;
21+
double clockwise;
22+
double bl;
23+
double br;
24+
int speed = 1;
25+
boolean shouldSlowmode = false;
26+
boolean slowmodeChanged = false;
27+
double plowPos = 0.1;
28+
boolean eyesOnTurn = true;
29+
boolean eyesChanged = false;
30+
31+
backLeft = hardwareMap.get(CRServo.class, "backLeft");
32+
backRight = hardwareMap.get(CRServo.class, "backRight");
33+
eyes = hardwareMap.get(Servo.class, "eyes");
34+
plowLeft = hardwareMap.get(Servo.class, "plowLeft");
35+
plowRight = hardwareMap.get(Servo.class, "plowRight");
36+
37+
plowLeft.setDirection(Servo.Direction.REVERSE);
38+
39+
eyes.setPosition(0.5);
40+
plowLeft.setPosition(plowPos);
41+
plowRight.setPosition(plowPos);
42+
43+
waitForStart();
44+
if (opModeIsActive()) {
45+
while (opModeIsActive()) {
46+
backLeft.setPower(gamepad1.left_stick_y);
47+
backRight.setPower(-gamepad1.right_stick_y);
48+
49+
if (gamepad1.back) {
50+
if (!eyesChanged) {
51+
eyesOnTurn = !eyesOnTurn;
52+
eyesChanged = true;
53+
}
54+
} else if (eyesChanged){
55+
eyesChanged = false;
56+
}
57+
58+
if (eyesOnTurn) {
59+
eyes.setPosition(0.3 * -(gamepad1.left_stick_y - gamepad1.right_stick_y) + 0.5);
60+
} else if ((gamepad1.right_trigger - gamepad1.left_trigger) != 0) {
61+
if (eyes.getPosition() < 0.2) {
62+
eyes.setPosition(0.2);
63+
} else if (eyes.getPosition() > 0.8) {
64+
eyes.setPosition(0.8);
65+
} else {
66+
eyes.setPosition(eyes.getPosition() + 0.01 * (gamepad1.right_trigger - gamepad1.left_trigger));
67+
sleep(8);
68+
}
69+
}
70+
71+
if (plowPos < 0.075) {
72+
plowPos = 0.075;
73+
} else if (plowPos > 0.6) {
74+
plowPos = 0.6;
75+
} else {
76+
plowPos += 0.01 * ((gamepad1.dpad_up ? 1 : 0) - (gamepad1.dpad_down ? 1 : 0));
77+
sleep(8);
78+
}
79+
80+
plowLeft.setPosition(plowPos);
81+
plowRight.setPosition(plowPos);
82+
83+
telemetry.update();
84+
}
85+
}
86+
}
87+
}
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
#!/usr/bin/bash
2+
git rebase upstream/master

0 commit comments

Comments
 (0)