The necessary imports and class variables are declared, including the motors (frontLeftMotor, frontRightMotor, rearLeftMotor, rearRightMotor), servos (servo1, servo2), touch sensor (touchSensor), speed multiplier (speedMultiplier), and gamepads (gamepad1, gamepad2).
The runOpMode() method is the main method that executes when the OpMode is run. It initializes the hardware, waits for the start command, and enters a loop to read gamepad inputs, calculate motor powers, control servos, check the touch sensor state, and update telemetry.
The initializeHardware() method initializes the hardware components by getting references to the motors, servos, and touch sensor from the hardware map. It also sets the motor directions and assigns the gamepads.
The readGamepadInputs() method reads the gamepad inputs, including the drive, strafe, and rotate values from the left stick for controlling the robot's movement. It also checks for the left trigger input to determine the speed multiplier. Additionally, it handles the servo controls based on button presses on gamepad2. Pressing the X and Y buttons increments and decrements the position of servo1, respectively. Pressing the B and DPAD_UP buttons does the same for servo2.
The performAutomatedTurn() method is called when the A button is pressed on gamepad1. It sets the motor powers to perform the desired automated turn action, such as rotating left at full power. Adjust this method to define the specific turn action you want.
The calculateMotorPowers() method calculates the motor powers based on the gamepad inputs and speed multiplier. It uses the mecanum drive equations to determine the individual motor powers for the desired movement.
The setMotorPowers() method sets the motor powers for all four motors.
The applyMotorPowers() method applies the motor powers by setting the power values for each motor.
The controlServos() method handles the servo controls. Since the servo positions are updated directly in the readGamepadInputs() method based on the button presses on gamepad2, no additional code is needed here.
The checkTouchSensor() method checks the state of the touch sensor. If the touch sensor is pressed, it performs a specific action, such as stopping the robot by setting all motor powers to zero.
The telemetry is updated to display the speed multiplier and touch sensor state during runtime.
---code---
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.TouchSensor;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.hardware.Gamepad;
public class MecanumDriveOpMode extends LinearOpMode {
private DcMotor frontLeftMotor;
private DcMotor frontRightMotor;
private DcMotor rearLeftMotor;
private DcMotor rearRightMotor;
private Servo servo1;
private Servo servo2;
private TouchSensor touchSensor;
private double speedMultiplier = 0.5; // Initial speed multiplier
private Gamepad gamepad1;
private Gamepad gamepad2;
private boolean servo1ButtonState = false;
private boolean servo2ButtonState = false;
@Override
public void runOpMode() {
initializeHardware();
waitForStart();
while (opModeIsActive()) {
readGamepadInputs();
calculateMotorPowers();
applyMotorPowers();
controlServos();
checkTouchSensor();
telemetry.addData("Speed Multiplier", speedMultiplier);
telemetry.addData("Touch Sensor State", touchSensor.isPressed());
telemetry.update();
}
}
private void initializeHardware() {
frontLeftMotor = hardwareMap.get(DcMotor.class, "front_left_motor");
frontRightMotor = hardwareMap.get(DcMotor.class, "front_right_motor");
rearLeftMotor = hardwareMap.get(DcMotor.class, "rear_left_motor");
rearRightMotor = hardwareMap.get(DcMotor.class, "rear_right_motor");
servo1 = hardwareMap.get(Servo.class, "servo1");
servo2 = hardwareMap.get(Servo.class, "servo2");
touchSensor = hardwareMap.get(TouchSensor.class, "touch_sensor");
frontLeftMotor.setDirection(DcMotor.Direction.REVERSE);
frontRightMotor.setDirection(DcMotor.Direction.FORWARD);
rearLeftMotor.setDirection(DcMotor.Direction.REVERSE);
rearRightMotor.setDirection(DcMotor.Direction.FORWARD);
gamepad1 = gamepad1;
gamepad2 = gamepad2;
}
private void readGamepadInputs() {
double drive = -gamepad1.left_stick_y;
double strafe = gamepad1.left_stick_x;
double rotate = gamepad1.right_stick_x;
if (gamepad1.left_trigger > 0) {
speedMultiplier = 0.5;
} else {
speedMultiplier = 1.0;
}
if (gamepad2.a) {
performAutomatedTurn();
}
double servo1Position = servo1.getPosition();
if (gamepad2.x && !servo1ButtonState) {
// Increment servo1 position when X button is pressed
servo1Position += 0.1;
servo1Position = Range.clip(servo1Position, 0.0, 1.0);
}
if (gamepad2.y && !servo1ButtonState) {
// Decrement servo1 position when Y button is pressed
servo1Position -= 0.1;
servo1Position = Range.clip(servo1Position, 0.0, 1.0);
}
servo1.setPosition(servo1Position);
servo1ButtonState = gamepad2.x || gamepad2.y;
double servo2Position = servo2.getPosition();
if (gamepad2.b && !servo2ButtonState) {
// Increment servo2 position when B button is pressed
servo2Position += 0.1;
servo2Position = Range.clip(servo2Position, 0.0, 1.0);
}
if (gamepad2.dpad_up && !servo2ButtonState) {
// Decrement servo2 position when DPAD_UP button is pressed
servo2Position -= 0.1;
servo2Position = Range.clip(servo2Position, 0.0, 1.0);
}
servo2.setPosition(servo2Position);
servo2ButtonState = gamepad2.b || gamepad2.dpad_up;
}
private void performAutomatedTurn() {
// Perform the desired automated turn action
// (e.g., rotate left at full power)
setMotorPowers(1.0, -1.0, 1.0, -1.0);
}
private void calculateMotorPowers() {
double drive = -gamepad1.left_stick_y;
double strafe = gamepad1.left_stick_x;
double rotate = gamepad1.right_stick_x;
double frontLeftPower = drive - strafe + rotate;
double frontRightPower = drive + strafe - rotate;
double rearLeftPower = drive + strafe + rotate;
double rearRightPower = drive - strafe - rotate;
double maxPower = Math.max(
Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)),
Math.max(Math.abs(rearLeftPower), Math.abs(rearRightPower))
);
if (maxPower > 1.0) {
frontLeftPower /= maxPower;
frontRightPower /= maxPower;
rearLeftPower /= maxPower;
rearRightPower /= maxPower;
}
frontLeftPower *= speedMultiplier;
frontRightPower *= speedMultiplier;
rearLeftPower *= speedMultiplier;
rearRightPower *= speedMultiplier;
setMotorPowers(frontLeftPower, frontRightPower, rearLeftPower, rearRightPower);
}
private void setMotorPowers(double frontLeftPower, double frontRightPower, double rearLeftPower, double rearRightPower) {
frontLeftMotor.setPower(frontLeftPower);
frontRightMotor.setPower(frontRightPower);
rearLeftMotor.setPower(rearLeftPower);
rearRightMotor.setPower(rearRightPower);
}
private void applyMotorPowers() {
frontLeftMotor.setPower(frontLeftPower);
frontRightMotor.setPower(frontRightPower);
rearLeftMotor.setPower(rearLeftPower);
rearRightMotor.setPower(rearRightPower);
}
private void controlServos() {
// No additional code needed for servo control using gamepad2
}
private void checkTouchSensor() {
if (touchSensor.isPressed()) {
// Action to perform when the touch sensor is pressed
// (e.g., stop the robot)
setMotorPowers(0.0, 0.0, 0.0, 0.0);
}
}
}
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class DistanceSensorArmExample extends LinearOpMode {
private DistanceSensor distanceSensor1;
private DistanceSensor distanceSensor2;
private Servo armServo;
private static final double ARM_LEFT_POSITION = 0.2;
private static final double ARM_RIGHT_POSITION = 0.8;
private static final double MAX_DISTANCE_THRESHOLD = 10.0; // Maximum allowed distance for arm movement
@Override
public void runOpMode() throws InterruptedException {
initializeHardware();
waitForStart();
while (opModeIsActive()) {
double distance1 = getFilteredDistance(distanceSensor1);
double distance2 = getFilteredDistance(distanceSensor2);
// Move the arm left or right based on the closest distance
if (distance1 < distance2 && distance1 < MAX_DISTANCE_THRESHOLD) {
moveArmLeft();
} else if (distance2 < MAX_DISTANCE_THRESHOLD) {
moveArmRight();
} else {
stopArm();
}
telemetry.addData("Distance 1 (inches)", distance1);
telemetry.addData("Distance 2 (inches)", distance2);
telemetry.update();
idle();
}
}
private void initializeHardware() {
HardwareMap hardwareMap = hardwareMap;
distanceSensor1 = hardwareMap.get(Rev2mDistanceSensor.class, "distance_sensor1");
distanceSensor2 = hardwareMap.get(Rev2mDistanceSensor.class, "distance_sensor2");
armServo = hardwareMap.get(Servo.class, "arm_servo");
}
private double getFilteredDistance(DistanceSensor distanceSensor) {
final int FILTER_COUNT = 5;
double totalDistance = 0;
// Collect multiple distance readings
for (int i = 0; i < FILTER_COUNT; i++) {
totalDistance += distanceSensor.getDistance(DistanceUnit.INCH);
sleep(10);
}
// Calculate average distance
double averageDistance = totalDistance / FILTER_COUNT;
return averageDistance;
}
private void moveArmLeft() {
armServo.setPosition(ARM_LEFT_POSITION);
}
private void moveArmRight() {
armServo.setPosition(ARM_RIGHT_POSITION);
}
private void stopArm() {
armServo.setPosition(0.5); // Set the servo to a neutral position
}
}
