Skip to content

Commit 96b448a

Browse files
samfreundsamfreund
committed
11.27.21 11:09
1 parent 531076a commit 96b448a

1 file changed

Lines changed: 154 additions & 0 deletions

File tree

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/fieldOriented.java‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
package org.firstinspires.ftc.teamcode.teleop;
2+
3+
import com.qualcomm.hardware.bosch.BNO055IMU;
4+
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
5+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
6+
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
7+
import com.qualcomm.robotcore.hardware.DcMotor;
8+
import com.qualcomm.robotcore.hardware.HardwareMap;
9+
import com.qualcomm.robotcore.hardware.Servo;
10+
11+
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
12+
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
13+
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
14+
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
15+
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
16+
17+
import java.util.Locale;
18+
19+
@TeleOp(name = "mecanumDriveRed", group = "Competition")
20+
public class fieldOriented extends LinearOpMode {
21+
@Override
22+
public void runOpMode() {
23+
// Declare our motors
24+
// Make sure your ID's match your configuration
25+
DcMotor lF = hardwareMap.dcMotor.get("front_left");
26+
DcMotor lB = hardwareMap.dcMotor.get("back_left");
27+
DcMotor rF = hardwareMap.dcMotor.get("front_right");
28+
DcMotor rB = hardwareMap.dcMotor.get("back_right");
29+
DcMotor arm1 = hardwareMap.dcMotor.get("arm1");
30+
DcMotor duckies = hardwareMap.dcMotor.get("duckies");
31+
Servo wrist1 = hardwareMap.servo.get("wrist");
32+
Servo grabber = hardwareMap.servo.get("grabber");
33+
34+
35+
// Reverse the right side motors
36+
// Reverse left motors if you are using NeveRests
37+
38+
rF.setDirection(DcMotor.Direction.REVERSE);
39+
rB.setDirection(DcMotor.Direction.REVERSE);
40+
41+
// Set motors to brake when not in use
42+
lF.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
43+
rF.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
44+
lB.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
45+
rB.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
46+
arm1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
47+
48+
49+
waitForStart();
50+
51+
if (isStopRequested()) return;
52+
53+
while (opModeIsActive()) {
54+
55+
56+
double rx = -gamepad1.right_stick_x; // Remember, this is reversed!
57+
double x = -gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
58+
double y = gamepad1.left_stick_y;
59+
60+
61+
double frontLeftPower = y + x + rx;
62+
double backLeftPower = y - x + rx;
63+
double frontRightPower = y - x - rx;
64+
double backRightPower = y + x - rx;
65+
66+
// Put powers in the range of -1 to 1 only if they aren't already
67+
// Not checking would cause us to always drive at full speed
68+
if (Math.abs(frontLeftPower) > 1 || Math.abs(backLeftPower) > 1 ||
69+
Math.abs(frontRightPower) > 1 || Math.abs(backRightPower) > 1) {
70+
// Find the largest power
71+
double max = 0;
72+
max = Math.max(Math.abs(frontLeftPower), Math.abs(backLeftPower));
73+
max = Math.max(Math.abs(frontRightPower), max);
74+
max = Math.max(Math.abs(backRightPower), max);
75+
76+
// Divide everything by max (it's positive so we don't need to worry
77+
// about signs)
78+
frontLeftPower /= max;
79+
backLeftPower /= max;
80+
frontRightPower /= max;
81+
backRightPower /= max;
82+
}
83+
if (gamepad1.right_bumper) {
84+
lF.setPower(frontLeftPower * 0.25);
85+
lB.setPower(backLeftPower * 0.25);
86+
rF.setPower(frontRightPower * 0.25);
87+
rB.setPower(backRightPower * 0.25);
88+
telemetry.addLine("Speed one quarter");
89+
telemetry.update();
90+
} else if (gamepad1.left_bumper) {
91+
lF.setPower(frontLeftPower * 0.75);
92+
lB.setPower(backLeftPower * 0.75);
93+
rF.setPower(frontRightPower * 0.75);
94+
rB.setPower(backRightPower * 0.75);
95+
telemetry.addLine("Speed 3/4");
96+
telemetry.update();
97+
} else {
98+
lF.setPower(frontLeftPower);
99+
lB.setPower(backLeftPower);
100+
rF.setPower(frontRightPower);
101+
rB.setPower(backRightPower);
102+
telemetry.addLine("Speed full");
103+
telemetry.update();
104+
}
105+
106+
107+
if (gamepad2.a) {
108+
grabber.setPosition(0.7);
109+
telemetry.addLine("Open");
110+
telemetry.update();
111+
} else if (gamepad2.b) {
112+
grabber.setPosition(0.45);
113+
telemetry.addLine("Ball");
114+
telemetry.update();
115+
} else if (gamepad2.y) {
116+
grabber.setPosition(0.1);
117+
telemetry.addLine("Box");
118+
telemetry.update();
119+
}
120+
121+
//set wrist position
122+
wrist1.setPosition(gamepad2.right_trigger);
123+
124+
//TODO
125+
//set wrist position option 2
126+
/*
127+
if(gamepad2.dpad_up){
128+
wrist1.setPosition(1);
129+
} else if(gamepad2.dpad_right){
130+
wrist1.setPosition(0.7);
131+
} else if(gamepad2.dpad_down){
132+
wrist1.setPosition(0.5);
133+
} else if(gamepad2.dpad_left){
134+
wrist1.setPosition(0.3);
135+
}
136+
*/
137+
138+
//TODO
139+
140+
int ticks = 0;
141+
ticks += -(int) gamepad2.left_stick_y * 2;
142+
arm1.setPower(1);
143+
arm1.setTargetPosition(ticks);
144+
arm1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
145+
146+
147+
if (gamepad1.x || gamepad2.x) {
148+
duckies.setPower(-0.5);
149+
} else {
150+
duckies.setPower(0);
151+
}
152+
}
153+
}
154+
}

0 commit comments

Comments
 (0)