Rotate to Angle (FTC)

navX2-Micro can be used to rotate a robot quickly and accurately to a specified angle. This can occur not only with holonomic drive systems that provide independent Z-axis rotation (the capability to “spin on a dime” while also moving in a linear direction), but also on robots with only two drive motors  This same technique can be used to help a robot drive in a straight line.

This example code below will automatically rotate the robot to a specified angle. This rotation can occur not only when the robot is still as in this example, but also when the robot is driving. When using field-oriented control on a holonomic drive system, this will cause the robot to drive in a straight line, in whatever direction is selected.

The PID Controller coefficients defined in the example code will need to be tuned for your drive system.

For more details on this approach, please visit Chief Delphi, including this helpful post.

FTC Android Example

	ppackage org.firstinspires.ftc.teamcode;
	import android.util.Log;
	import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
	import com.kauailabs.navx.ftc.AHRS;
	import com.kauailabs.navx.ftc.navXPIDController;
	import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
	import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
	import com.qualcomm.robotcore.hardware.DcMotor;
	import com.qualcomm.robotcore.hardware.DcMotorController;
	import com.qualcomm.robotcore.util.ElapsedTime;
	import java.text.DecimalFormat;
	/*
	* An example linear op mode where the robot will rotate
	* to a specified angle an then stop.
	*
	* This example uses a simple PID controller configuration
	* with a P coefficient, and will likely need tuning in order
	* to achieve optimal performance.
	*
	* Note that for the best accuracy, a reasonably high update rate
	* for the navX-Model sensor should be used.
	*/
	@TeleOp(name = "Concept: navX Rotate to Angle PID - Linear", group = "Concept")
	// @Disabled Comment this in to remove this from the Driver Station OpMode List
	public class ConceptNavXRotateToAnglePIDLinearOp extends LinearOpMode {
	DcMotor leftMotor;
	DcMotor rightMotor;
	private AHRS navx_device;
	private navXPIDController yawPIDController;
	private ElapsedTime runtime = new ElapsedTime();
	private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50;
	private final double TARGET_ANGLE_DEGREES = 90.0;
	private final double TOLERANCE_DEGREES = 2.0;
	private final double MIN_MOTOR_OUTPUT_VALUE = -1.0;
	private final double MAX_MOTOR_OUTPUT_VALUE = 1.0;
	private final double YAW_PID_P = 0.005;
	private final double YAW_PID_I = 0.0;
	private final double YAW_PID_D = 0.0;
	private boolean calibration_complete = false;
	@Override
	public void runOpMode() throws InterruptedException {
	leftMotor = hardwareMap.dcMotor.get("left motor");
	rightMotor = hardwareMap.dcMotor.get("right motor");
	navx_device = AHRS.getInstance(hardwareMap.get(NavxMicroNavigationSensor.class, "navx"),
	AHRS.DeviceDataType.kProcessedData,
	NAVX_DEVICE_UPDATE_RATE_HZ);
	rightMotor.setDirection(DcMotor.Direction.REVERSE);
	/* If possible, use encoders when driving, as it results in more */
	/* predictable drive system response. */
	//leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
	//rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
	/* Create a PID Controller which uses the Yaw Angle as input. */
	yawPIDController = new navXPIDController( navx_device,
	navXPIDController.navXTimestampedDataSource.YAW);
	/* Configure the PID controller */
	yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
	yawPIDController.setContinuous(true);
	yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
	yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
	yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
	waitForStart();
	while ( !calibration_complete ) {
	/* navX-Micro Calibration completes automatically ~15 seconds after it is
	powered on, as long as the device is still. To handle the case where the
	navX-Micro has not been able to calibrate successfully, hold off using
	the navX-Micro Yaw value until calibration is complete.
	*/
	calibration_complete = !navx_device.isCalibrating();
	if (!calibration_complete) {
	telemetry.addData("navX-Micro", "Startup Calibration in Progress");
	}
	}
	navx_device.zeroYaw();
	try {
	yawPIDController.enable(true);
	/* Wait for new Yaw PID output values, then update the motors
	with the new PID value with each new output value.
	*/
	final double TOTAL_RUN_TIME_SECONDS = 30.0;
	int DEVICE_TIMEOUT_MS = 500;
	navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult();
	DecimalFormat df = new DecimalFormat("#.##");
	while ( (runtime.time() < TOTAL_RUN_TIME_SECONDS) &&
	!Thread.currentThread().isInterrupted()) {
	if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
	if (yawPIDResult.isOnTarget()) {
	leftMotor.setPowerFloat();
	rightMotor.setPowerFloat();
	telemetry.addData("PIDOutput", df.format(0.00));
	} else {
	double output = yawPIDResult.getOutput();
	leftMotor.setPower(output);
	rightMotor.setPower(-output);
	telemetry.addData("PIDOutput", df.format(output) + ", " +
	df.format(-output));
	}
	} else {
	/* A timeout occurred */
	Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
	}
	telemetry.addData("Yaw", df.format(navx_device.getYaw()));
	}
	}
	catch(InterruptedException ex) {
	Thread.currentThread().interrupt();
	}
	finally {
	navx_device.close();
	telemetry.addData("LinearOp", "Complete");
	}
	}
	}

view raw navXRotateToAnglePIDOp hosted with by GitHub