@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 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();
}
