Turning the Drivetrain Using RUN_TO_POSITION

In the Programming Drivetrain Motors - OnBot Java section, the mechanism of setPower(); was discussed. setPower();dictates what direction and speed a motor moves in. On a drivetrain this dictates whether the robot moves in forward, reverse, or turns.

In RUN_TO_POSITIONmode the encoder counts ( or setTargetPosition();) are used instead of setPower(); to dictate directionality of the motor. If a target position value is greater than the current position of the encoder, the motor moves forward. If the target position value is less than the current position of the encoder, the motor moves backwards

Since speed an directionality impacts how a robot turns, setTargetPostion(); and setVelocity();need to be edited to get the robot to turn. Consider the following code:

int leftTarget = (int)(610 * COUNTS_PER_MM);
int rightTarget = (int)(-610 * COUNTS_PER_MM);
double TPS = (100/ 60) * COUNTS_PER_WHEEL_REV;
       
        
waitForStart();
        
        
leftmotor.setTargetPosition(leftTarget);
rightmotor.setTargetPosition(rightTarget);
        
leftmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rightmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        
leftmotor.setVelocity(TPS);
rightmotor.setVelocity(TPS);

The rightTarget has been changed to be a negative target position. Assuming that the encoder starts at zero due to STOP_AND_RESET_ENCODER this causes the robot to turn to the right. Velocity remains the same for both motors. If you try running this code, you may notice that the robot pivots along its center of rotation. To get a wider turn changing the velocity so that the right motor is running at a lower velocity than the left motor. Adjust the velocity and target position as needed to get the turn you need.

For more information on how direction and speed impact the movement of a robot please refer to the explanation of setPower(); in the Programming Drivetrain Motors section.

The following code walks through adding a turn to the program, after the robot moves forward for 2 feet. After the robot reaches the 2 foot goal, there is a call to STOP_AND_RESET_ENCODERS this will reduce the need to calculate what position to go to after a position has been reached.

@Autonomous

public class HelloRobot_EncoderAuton extends LinearOpMode {
    private DcMotorEx leftmotor;
    private DcMotorEx rightmotor;
    private DcMotor arm;
    private Servo claw;
    private TouchSensor touch;
    
    static final double     COUNTS_PER_MOTOR_REV    = 28.0; 
    static final double     DRIVE_GEAR_REDUCTION    = 30.24;   
    static final double     WHEEL_CIRCUMFERENCE_MM  = 90.0 * 3.14;
    
    static final double     COUNTS_PER_WHEEL_REV    = COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION;
    static final double     COUNTS_PER_MM           = COUNTS_PER_WHEEL_REV / WHEEL_CIRCUMFERENCE_MM;
    
    @Override
    public void runOpMode() {
        leftmotor = hardwareMap.get(DcMotorEx.class, "leftmotor");
        rightmotor = hardwareMap.get(DcMotorEx.class, "rightmotor");
        arm = hardwareMap.get(DcMotor.class, "arm");
        claw = hardwareMap.get(Servo.class, "claw");
        touch = hardwareMap.get(TouchSensor.class, "touch");

        rightmotor.setDirection(DcMotor.Direction.REVERSE);

        leftmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rightmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        
        // TPS variable split to change velocity for each motor when necessary
        
        int leftTarget = (int)(610 * COUNTS_PER_MM);
        int rightTarget = (int)(610 * COUNTS_PER_MM);
        double LTPS = (175/ 60) * COUNTS_PER_WHEEL_REV;         
        double RTPS = (175/ 60) * COUNTS_PER_WHEEL_REV;
        
        waitForStart();
        
        
        leftmotor.setTargetPosition(leftTarget);
        rightmotor.setTargetPosition(rightTarget);
        
        leftmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        rightmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        
        leftmotor.setVelocity(LTPS);
        rightmotor.setVelocity(RTPS);
        
        //wait for motor to reach position before moving on 
        while (opModeIsActive() && (leftmotor.isBusy() && rightmotor.isBusy())) {
            telemetry.addData("left", leftmotor.getCurrentPosition());
            telemetry.addData("right", rightmotor.getCurrentPosition());
            telemetry.update();
        }
        // Reset encoders to zero 
        leftmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rightmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        
        // changing variables to match new needs 
        
        leftTarget = (int)(300 * COUNTS_PER_MM);
        rightTarget = (int)( -300 * COUNTS_PER_MM);
        LTPS = (100/ 60) * COUNTS_PER_WHEEL_REV;         
        RTPS = (70/ 60) * COUNTS_PER_WHEEL_REV;
        
        leftmotor.setTargetPosition(leftTarget);
        rightmotor.setTargetPosition(rightTarget);
        
        leftmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        rightmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        
        leftmotor.setVelocity(LTPS);
        rightmotor.setVelocity(RTPS);
        
        //wait for motor to reach position before moving on 
        while (opModeIsActive() && (leftmotor.isBusy() && rightmotor.isBusy())) {
            telemetry.addData("left", leftmotor.getCurrentPosition());
            telemetry.addData("right", rightmotor.getCurrentPosition());
            telemetry.update();
        }
    }
}