Here's the full code with minor improvements:
```
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name = "CHSA TeleOp Drive", group = "TeleOp")
public class Op extends OpMode {
// Motor and servo objects
private DcMotor leftDrive, rightDrive, armMotor;
private Servo gripper, armExtension, wrist;
// Constants for servo positions
private static final double GRIPPER_OPEN = 0.6;
private static final double GRIPPER_CLOSED = 0.0;
private static final double ARM_EXTENSION_STOP = 0.5;
private static final double ARM_EXTENSION_FORWARD = 1.0;
private static final double ARM_EXTENSION_REVERSE = 0.0;
private static final double WRIST_P1 = 0.5;
private static final double WRIST_P2 = 0.6;
private static final double WRIST_P3 = 0.7;
private static final double WRIST_P4 = 0.8;
// Gamepad input thresholds
private static final double GAMEPAD_THRESHOLD_HIGH = 0.9;
private static final double GAMEPAD_THRESHOLD_LOW = 0.05;
// Arm motor variables
private int armPosition;
private double armPower;
@Override
public void init() {
// Initialize motor and servo objects
leftDrive = hardwareMap.get(DcMotor.class, "leftDrive");
rightDrive = hardwareMap.get(DcMotor.class, "rightDrive");
armMotor = hardwareMap.get(DcMotor.class, "armMotor");
gripper = hardwareMap.get(Servo.class, "grip");
armExtension = hardwareMap.get(Servo.class, "armExtension");
wrist = hardwareMap.get(Servo.class, "wrist");
// Set motor directions and modes
leftDrive.setDirection(DcMotor.Direction.FORWARD);
rightDrive.setDirection(DcMotor.Direction.REVERSE);
armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Set initial servo positions
gripper.setPosition(GRIPPER_OPEN);
armExtension.setPosition(ARM_EXTENSION_STOP);
wrist.setPosition(WRIST_P1);
// Update telemetry
telemetry.addData("Status", "Initialized");
}
@Override
public void loop() {
// Get gamepad inputs
double drive = -gamepad1.left_stick_y * 0.5;
double turn = gamepad1.left_stick_x * 0.4;
double leftPower = drive + turn;
double rightPower = drive - turn;
// Control drivetrain
leftDrive.setPower(leftPower);
rightDrive.setPower(rightPower);
// Control arm motor
if (Math.abs(gamepad1.right_stick_y) > GAMEPAD_THRESHOLD_HIGH) {
armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armPower = -gamepad1.right_stick_y * 0.6;
armMotor.setPower(armPower);
armPosition = armMotor.getCurrentPosition();
} else if (Math.abs(gamepad1.right_stick_y) > GAMEPAD_THRESHOLD_LOW) {
armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armPower = -gamepad1.right_stick_y * 0.4;
armMotor.setPower(armPower);
armPosition = armMotor.getCurrentPosition();
} else {
armMotor.setTargetPosition(armPosition);
armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
armMotor.setPower(0.2);
}
// Control gripper
if (gamepad1.a) {
gripper.setPosition(GRIPPER_CLOSED);
} else if (gamepad1.b) {
gripper.setPosition(GRIPPER_OPEN);
}
// Control arm extension
if (gamepad1.right_trigger > 0.1) {
armExtension.setPosition(ARM_EXTENSION_FORWARD);
} else if (gamepad1.left_trigger > 0.1) {
armExtension.setPosition(ARM_EXTENSION_REVERSE);
} else if (gamepad1.y) {
armExtension.setPosition(ARM_EXTENSION_STOP);
}
// Control wrist
if (gamepad1.dpad_down) {
wrist.setPosition(WRIST_P1);
} else if (gamepad1.dpad_left) {
wrist.setPosition(WRIST_P2);
} else if (gamepad1.dpad_right) {
wrist.setPosition(WRIST_P3);
} else if (gamepad1.dpad_up)
