Автономный дрифт FTC ⇐ JAVA

[Anonymous]

Наш робот автономно использует энкодеры для управления автомобилем. Мы проверили в ручном режиме — работает. В автономном нет. Например, в первой последовательности назад он движется назад на 20 дюймов, но левый двигатель продолжает работать, заставляя робота поворачиваться на 90 градусов. Аппаратное обеспечение правильное. Пожалуйста, помогите.
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.JavaUtil;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;

@Autonomous(name = "Auto_Goal_Blue3", preselectTeleOp = "MainMove Plus Pro Max Ultra Super Optimized CE Ultra Long Range 3000")
public class Auto_Goal_Blue3 extends LinearOpMode {

private DcMotor left_drive;
private DcMotor right_drive;
private DcMotorEx flywheel;
private DcMotor intake;
private DistanceSensor sensor_distance;

double COUNTS_PER_INCH;
ElapsedTime runtime;

/**
* This OpMode illustrates the concept of driving a path based on encoder counts.
* This OpMode requires that you have encoders on the wheels, otherwise you would use RobotAutoDriveByTime.
* This OpMode also requires that the drive Motors have been configured such that a
* positive power command moves them forward, and causes the encoders to count up.
*
* The desired path in this example is:
* - Drive forward for 48 inches
* - Spin right for 12 Inches
* - Drive backward for 24 inches
*
* This OpMode has a function called encoderDrive that performs the actual movement.
* This function assumes that each movement is relative to the last stopping place.
* There are other ways to perform encoder based moves, but this method is probably the simplest.
* This OpMode uses the RUN_TO_POSITION mode.
*/
@Override
public void runOpMode() {
int DRIVE_SPEED;
int TURN_SPEED;
int COUNTS_PER_MOTOR_REV;
int DRIVE_GEAR_REDUCTION;
double WHEEL_DIAMETER_INCHES;

left_drive = hardwareMap.get(DcMotor.class, "left_drive");
right_drive = hardwareMap.get(DcMotor.class, "right_drive");
flywheel = hardwareMap.get(DcMotorEx.class, "flywheel");
intake = hardwareMap.get(DcMotor.class, "intake");
sensor_distance = hardwareMap.get(DistanceSensor.class, "sensor_distance");

double ticksPerRev = flywheel.getMotorType().getTicksPerRev();

// DO NOT CHANGE ANYTHING EXCEPT DRIVE/TURN SPEED
COUNTS_PER_MOTOR_REV = 28;
DRIVE_GEAR_REDUCTION = 20;
WHEEL_DIAMETER_INCHES = 3.8;
COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * Math.PI);
DRIVE_SPEED = 1;
TURN_SPEED = 1;
runtime = new ElapsedTime();
// To drive forward, most robots need the motor on one side to be reversed,
// because the axles point in opposite directions.
// When run, this OpMode should start both motors driving forward.
// So adjust the motor directions based on your first test drive.
// Note: The settings here assume direct drive on left and right wheels.
// Gear Reduction or 90 Deg drives may require direction flips.
left_drive.setDirection(DcMotor.Direction.REVERSE);
right_drive.setDirection(DcMotor.Direction.FORWARD);
flywheel.setDirection(DcMotor.Direction.FORWARD);
intake.setDirection(DcMotor.Direction.FORWARD);
left_drive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
right_drive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
left_drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
right_drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
left_drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
right_drive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

// Send telemetry message to indicate successful Encoder reset.
telemetry.addData("Starting at", JavaUtil.formatNumber(left_drive.getCurrentPosition(), 7, 0) + " : " + JavaUtil.formatNumber(right_drive.getCurrentPosition(), 7, 0));
telemetry.update();
// Wait for the game to start (driver presses START).
waitForStart();
// Note: Reverse movement is obtained by setting a negative distance (not speed).
encoderDrive(DRIVE_SPEED, -20, -20, 1);
flywheel.setPower(-1);
intake.setPower(-1);
//sleep(250);
// 0.67
flywheel.setPower(0.6);
intake.setPower(0);
while (opModeIsActive() && (flywheel.getVelocity() * 60 / ticksPerRev) < 165) {
telemetry.addData("Flywheel RPM", flywheel.getVelocity() * 60 / ticksPerRev);
telemetry.addLine("Waiting for flywheel to spin up...");
telemetry.update();
}
intake.setPower(1);
sleep(4500);
flywheel.setPower(0);
intake.setPower(0);
// 5
encoderDrive(TURN_SPEED, 12, -12, 1);
encoderDrive(DRIVE_SPEED, -18, -18, 0.5);
intake.setPower(1);
encoderDrive(TURN_SPEED, 12, -12, 0.5);
// collect ball (-26)
//
// 0.2
encoderDrive(DRIVE_SPEED, -10, -10, 0.5);
// collect ball (-26)
//
// 0.2
// sp 0.1
// -18
encoderDrive(0.1, -19, -19, 2.4);
encoderDrive(DRIVE_SPEED, 30, 30, 0.5);
intake.setPower(0);
encoderDrive(TURN_SPEED, -20, 20, 1);
// 5
encoderDrive(DRIVE_SPEED, 15, 15, 1);
// L-2
// L2
encoderDrive(TURN_SPEED, -2.5, 2.5, 1);
flywheel.setPower(-1);
intake.setPower(-1);
sleep(250);
flywheel.setPower(0.73);
intake.setPower(0);
sleep(2000);
intake.setPower(1);
sleep(4500);
intake.setPower(0);
flywheel.setPower(0);
left_drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
right_drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
left_drive.setPower(0.2);
right_drive.setPower(1);
sleep(10000);
}

/**
* Function to perform a relative move, based on encoder counts.
* Encoders are not reset as the move is based on the current position.
* Move will stop if any of three conditions occur:
* 1) Move gets to the desired position
* 2) Move runs out of time
* 3) Driver stops the OpMode running.
*/
private void encoderDrive(double speed, double leftInches, double rightInches, double timeoutS) {
double newLeftTarget;
double newRightTarget;

// Ensure that the OpMode is still active.
if (opModeIsActive()) {
// Determine new target position, and pass to motor controller.
newLeftTarget = left_drive.getCurrentPosition() + Math.floor(leftInches * COUNTS_PER_INCH);
newRightTarget = right_drive.getCurrentPosition() + Math.floor(rightInches * COUNTS_PER_INCH);
left_drive.setTargetPosition((int) newLeftTarget);
right_drive.setTargetPosition((int) newRightTarget);
// Turn On RUN_TO_POSITION.
left_drive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
right_drive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// Reset the timeout time and start motion.
runtime.reset();
left_drive.setPower(Math.abs(speed));
right_drive.setPower(Math.abs(speed));
// Keep looping while we are still active, and there is time left, and both motors are running.
// Note: We use (isBusy() and isBusy()) in the loop test, which means that when EITHER motor hits
// its target position, the motion will stop. This is "safer" in the event that the robot will
// always end the motion as soon as possible.
// However, if you require that BOTH motors have finished their moves before the robot continues
// onto the next step, use (isBusy() or isBusy()) in the loop test.
while (opModeIsActive() && runtime.seconds() < timeoutS && left_drive.isBusy() && right_drive.isBusy()) {
// Display it for the driver.
telemetry.addData("Running to", JavaUtil.formatNumber(newLeftTarget, 7, 0) + " :" + JavaUtil.formatNumber(newRightTarget, 7, 0));
telemetry.addData("Currently at", JavaUtil.formatNumber(left_drive.getCurrentPosition(), 7, 0) + " :" + JavaUtil.formatNumber(right_drive.getCurrentPosition(), 7, 0));
//telemetry.addData("range", JavaUtil.formatNumber(sensor_distance.getDistance(DistanceUnit.INCH), 2) + " in");
telemetry.update();
}
// Stop all motion.
left_drive.setPower(0);
right_drive.setPower(0);
// Turn off RUN_TO_POSITION.
left_drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
right_drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Optional pause after each move.
sleep(250);
}
}
}


Подробнее здесь: https://stackoverflow.com/questions/798 ... mous-drift