public void DriveOnHeadingReverse(
navXPIDController.PIDResult yawPIDResult, float heading, float distanceInches) {
// calculate encoder counts for distance
float wheelDiameter = 4; // inches
float wheelCirc = wheelDiameter * (float) 3.14159;
float encoderTicksPerRotation = 1120;
float ticksPerInch = encoderTicksPerRotation / wheelCirc;
int ticksToTravel = (int) (distanceInches * ticksPerInch);
// check motor position
int startEncCount = robot.leftfrontMotor.getCurrentPosition();
int DEVICE_TIMEOUT_MS = 500;
/* Drive straight forward at 1/2 of full drive speed */
double drive_speed = 0.5;
yawPIDController.setPID(0.05, YAW_PID_I, YAW_PID_D);
yawPIDController.setSetpoint(heading);
DecimalFormat df = new DecimalFormat("#.##");
try {
while ((robot.leftfrontMotor.getCurrentPosition() - startEncCount) > -ticksToTravel
&& !Thread.currentThread().isInterrupted()) {
if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
if (yawPIDResult.isOnTarget()) {
robot.Drive(-drive_speed, -drive_speed);
telemetry.addData("PIDOutput", df.format(drive_speed) + ", " + df.format(drive_speed));
} else {
double output = yawPIDResult.getOutput();
if (output < -0.5) {
output = -0.5;
}
if (output > 0.5) {
output = 0.5;
}
robot.Drive(-drive_speed + output, -drive_speed - output);
telemetry.addData(
"PIDOutput",
df.format(limit(-drive_speed - output))
+ ", "
+ df.format(limit(-drive_speed + output)));
}
telemetry.addData("Yaw", df.format(navx_device.getYaw()));
telemetry.addData("Output", df.format(yawPIDResult.getOutput()));
telemetry.addData("RevEnc:", robot.leftfrontMotor.getCurrentPosition());
telemetry.addData("EncStart:", startEncCount);
} else {
/* A timeout occurred */
Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
}
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
@Override
public void loop() {
/* reset the navX-Model device yaw angle so that whatever direction */
/* it is currently pointing will be zero degrees. */
if (first_iteration) {
first_iteration = false;
navx_device.zeroYaw();
yawPIDResult = new navXPIDController.PIDResult();
}
/* Wait for new Yaw PID output values, then update the motors
with the new PID value with each new output value.
*/
/* Drive straight forward at 1/2 of full drive speed */
double drive_speed = 0.5;
if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
if (yawPIDResult.isOnTarget()) {
leftMotor.setPower(drive_speed);
rightMotor.setPower(drive_speed);
} else {
double output = yawPIDResult.getOutput();
leftMotor.setPower(drive_speed + output);
rightMotor.setPower(drive_speed - output);
}
} else {
/* No sensor update has been received since the last time */
/* the loop() function was invoked. Therefore, there's no */
/* need to update the motors at this time. */
}
}
public void TurnToHeading(
navXPIDController.PIDResult yawPIDResult, double heading, double maxTimeSeconds) {
try {
long starttime = System.currentTimeMillis();
TARGET_ANGLE_DEGREES = heading;
int DEVICE_TIMEOUT_MS = 500;
yawPIDController.setPID(0.02, YAW_PID_I, YAW_PID_D);
yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
DecimalFormat df = new DecimalFormat("#.##");
while (!Thread.currentThread().isInterrupted()
&& (System.currentTimeMillis() - starttime) < (long) (maxTimeSeconds * 1000)) {
if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
if (yawPIDResult.isOnTarget()) {
float zero = 0;
robot.FloatMotors();
telemetry.addData("PIDOutput", df.format(0.00));
telemetry.update();
} else {
double output = yawPIDResult.getOutput();
if (output > 0 && output < 0.25) {
output = 0.25;
}
if (output < 0 && output > -0.25) {
output = -0.25;
}
robot.Drive(output, -output);
telemetry.addData("PIDOutput", df.format(output) + ", " + df.format(-output));
}
} else {
/* A timeout occurred */
Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
telemetry.addData("timeout", "timeout occured");
}
telemetry.addData("Yaw aw", df.format(navx_device.getYaw()));
telemetry.update();
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
@Override
public void init() {
leftMotor = hardwareMap.dcMotor.get("left_drive");
rightMotor = hardwareMap.dcMotor.get("right_drive");
navx_device =
AHRS.getInstance(
hardwareMap.deviceInterfaceModule.get("dim"),
NAVX_DIM_I2C_PORT,
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);
yawPIDController.enable(true);
first_iteration = true;
}
@Override
public void loop() {
if (!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) {
navx_device.zeroYaw();
} else {
telemetry.addData("navX-Micro", "Startup Calibration in Progress");
}
} else {
/* Wait for new Yaw PID output values, then update the motors
with the new PID value with each new output value.
*/
if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
if (yawPIDResult.isOnTarget()) {
leftMotor.setPowerFloat();
rightMotor.setPowerFloat();
telemetry.addData("Motor Output", df.format(0.00));
} else {
double output = yawPIDResult.getOutput();
leftMotor.setPower(output);
rightMotor.setPower(-output);
telemetry.addData("Motor Output", df.format(output) + ", " + df.format(-output));
}
} else {
/* No sensor update has been received since the last time */
/* the loop() function was invoked. Therefore, there's no */
/* need to update the motors at this time. */
}
telemetry.addData("Yaw", df.format(navx_device.getYaw()));
}
}
@Override
public void runOpMode() throws InterruptedException {
// use the Hardware4962 stuff that decribes the robot.
robot.init(hardwareMap);
// hardware and initialize the sensors
// odsSensor = hardwareMap.opticalDistanceSensor.get("ods");
colorC = hardwareMap.i2cDevice.get("color sensor");
colorCreader = new I2cDeviceSynchImpl(colorC, new I2cAddr(0x1e), false);
colorCreader.engage();
// navX setup
navx_device =
AHRS.getInstance(
hardwareMap.deviceInterfaceModule.get("dim"),
NAVX_DIM_I2C_PORT,
AHRS.DeviceDataType.kProcessedData,
NAVX_DEVICE_UPDATE_RATE_HZ);
/* 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);
yawPIDController.enable(true);
// color sensor initilization
colorCreader.write8(3, 1); // Set the mode of the color sensor to passive (0=active)
// passive = turn off the LED
// intake not running.
double intakePower = 0.0;
// wait for the start button to be pressed.
waitForStart();
// Make sure the navX is ready to be used.
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");
telemetry.update();
}
}
// start at a heading of zero.
navx_device.zeroYaw();
int DEVICE_TIMEOUT_MS = 500;
navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult();
DecimalFormat df = new DecimalFormat("#.##");
/** ********************************************************************************** */
/** ********************************************************************************** */
/*
/* start of code that does stuff
*/
// DriveOnHeadingReverse(yawPIDResult,0,36);
// robot.StopDriving();
// sleep(10000);
// Drive parallel to the ramp and then turn parallel to the wall at the first beacon.
DriveOnHeading(yawPIDResult, 0, 65);
robot.StopDriving();
}
public void gyroTurn(double degrees, boolean buttFirst) {
// degrees=degrees*-1;
yawPIDController =
new navXPIDController(navx_device, navXPIDController.navXTimestampedDataSource.YAW);
yawPIDController.setSetpoint(degrees);
yawPIDController.setContinuous(true);
yawPIDController.setOutputRange(Keys.MAX_SPEED * -1, Keys.MAX_SPEED);
yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, Keys.TOLERANCE_LEVEL_2);
yawPIDController.enable(true);
int DEVICE_TIMEOUT_MS = 500;
navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult();
telemetry.addData("Yaw", navx_device.getYaw());
double degreesNow = navx_device.getYaw();
double degreesToGo = degreesNow + degrees;
// telemetry.addData("if state",navx_device.getYaw());
// telemetry.addData("other if",degreesToGo);
telemetry.addData("boolean", navx_device.getYaw() > degreesToGo);
// telemetry.addData("boolean",navx_device.getYaw()<degreesToGo);
if (navx_device.getYaw() > degreesToGo) {
telemetry.addData("if", "getYaw>degrees");
telemetry.addData(
"more boolean",
!(degreesToGo - Keys.TOLERANCE_LEVEL_1 < navx_device.getYaw()
&& navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_1));
while (!(degreesToGo - Keys.TOLERANCE_LEVEL_1 < navx_device.getYaw()
&& navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_1)) {
collector.setPower(Keys.COLLECTOR_POWER * -1);
telemetry.addData("while", "turningLeft1");
double turnPower = .8;
if (buttFirst) {
turnPower = -.8;
turnRight(turnPower);
} else {
turnLeft(turnPower);
}
telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
}
telemetry.addData(
"more boolean2", navx_device.getYaw() > degreesToGo + Keys.TOLERANCE_LEVEL_2);
while (navx_device.getYaw() > degreesToGo + Keys.TOLERANCE_LEVEL_2) {
collector.setPower(Keys.COLLECTOR_POWER * -1);
telemetry.addData("while", "turningLeft2");
double turnPower = .75;
if (buttFirst) {
turnPower = -1 * turnPower;
turnRight(turnPower);
} else {
turnLeft(turnPower);
}
telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
}
while (!(degreesToGo - Keys.TOLERANCE_LEVEL_3 < navx_device.getYaw()
&& navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_3)) {
collector.setPower(Keys.COLLECTOR_POWER * -1);
telemetry.addData("while", "turningLeft3");
double turnPower = .7;
if (buttFirst) {
turnPower = -1 * turnPower;
turnRight(turnPower);
} else {
turnLeft(turnPower);
}
telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
}
telemetry.addData("while", "done");
} else if (navx_device.getYaw() < degreesToGo) {
telemetry.addData("if", "getYaw<degrees");
while (!(degreesToGo - Keys.TOLERANCE_LEVEL_1 < navx_device.getYaw()
&& navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_1)) {
collector.setPower(Keys.COLLECTOR_POWER * -1);
double turnPower = .8;
if (buttFirst) {
turnPower = -1 * turnPower;
turnLeft(turnPower);
} else {
turnRight(turnPower);
}
telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
telemetry.addData("while", "turningRight");
}
while (!(degreesToGo - Keys.TOLERANCE_LEVEL_2 < navx_device.getYaw()
&& navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_2)) {
collector.setPower(Keys.COLLECTOR_POWER * -1);
double turnPower = .75;
if (buttFirst) {
turnPower = -1 * turnPower;
turnLeft(turnPower);
} else {
turnRight(turnPower);
}
telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
telemetry.addData("while", "turningRight");
}
while (!(degreesToGo - Keys.TOLERANCE_LEVEL_3 < navx_device.getYaw()
&& navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_3)) {
collector.setPower(Keys.COLLECTOR_POWER * -1);
double turnPower = .7;
if (buttFirst) {
turnPower = -1 * turnPower;
turnLeft(turnPower);
} else {
turnRight(turnPower);
}
telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
telemetry.addData("while", "turningRight");
}
telemetry.addData("whileD", "done");
}
telemetry.addData("ifD", "done");
rest();
}
