Hey, i would use the following code:
PHP Code:
public class CCAutonomous extends LinearOpMode {
OpticalDistanceSensor opticalDistanceSensor;
//Drive motors
DcMotor motorFrontRight,motorFrontLeft, motorBackRight, motorBackLeft;
int leftFrontPos, leftBackPos, rightFrontPos, rightBackPos;
@Override
public void runOpMode() throws InterruptedException {
// opticalDistanceSensor = hardwareMap.opticalDistanceSensor.get("sensor_ods");
motorFrontRight = hardwareMap.dcMotor.get("front_right");
motorFrontLeft = hardwareMap.dcMotor.get("front_left");
motorBackRight = hardwareMap.dcMotor.get("back_right");
motorBackLeft = hardwareMap.dcMotor.get("back_left");
motorFrontLeft.setDirection(DcMotor.Direction.REVERSE);
motorBackLeft.setDirection(DcMotor.Direction.REVERSE);
motorFrontRight.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorFrontLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorBackRight.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorBackLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
waitForStart();
// Reset enoders to zero
motorFrontLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorFrontRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorBackLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorBackRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
waitOneFullHardwareCycle(); // We use these in attempt to gain stability.
motorFrontLeft.setTargetPosition(2440);
motorFrontRight.setTargetPosition(2440);
motorBackLeft.setTargetPosition(2440);
motorBackRight.setTargetPosition(2440);
leftFrontPos = motorLeftFront.getCurrentPosition();
leftBackPos = motorLeftBack.getCurrentPosition();
rightFrontPos = motorFrontRight.getCurrentPosition();
rightBackPos = motorBackRight.getCurrentPosition();
motorFrontRight.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorFrontLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorBackRight.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorBackLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
while(leftBackPos < 2440 && leftFrontPos < 2440 && rightBackPos < 2440 && rightFrontPos < 2440){
motorFrontLeft.setPower(-0.5);
motorFrontRight.setPower(-0.5);
motorBackLeft.setPower(-0.5);
motorBackRight.setPower(-0.5);
leftFrontPos = motorLeftFront.getCurrentPosition();
leftBackPos = motorLeftBack.getCurrentPosition();
rightFrontPos = motorFrontRight.getCurrentPosition();
rightBackPos = motorBackRight.getCurrentPosition();
}
telemetry.addData("2 ", "motorFrontLeft: " + String.format("%d", motorFrontLeft.getTargetPosition()));
telemetry.addData("3 ", "motorFrontRight: " + String.format("%d", motorFrontRight.getTargetPosition()));
telemetry.addData("4 ", "motorBackLeft: " + String.format("%d", motorBackLeft.getTargetPosition()));
telemetry.addData("5 ", "motorBackRight: " + String.format("%d", motorBackRight.getTargetPosition()));
}
}
Use this code instead and tell me if it worked, I will use this code too if you confirm that it worked