@Override
public void runOpMode() throws InterruptedException {
GyroSensor sensorGyro;
int xVal, yVal, zVal = 0;
int heading = 0;
// write some device information (connection info, name and type)
// to the log file.
hardwareMap.logDevices();
// get a reference to our GyroSensor object.
sensorGyro = hardwareMap.gyroSensor.get("gyro");
// calibrate the gyro.
sensorGyro.calibrate();
// wait for the start button to be pressed.
waitForStart();
// make sure the gyro is calibrated.
while (sensorGyro.isCalibrating()) {
Thread.sleep(50);
}
while (opModeIsActive()) {
// if the A and B buttons are pressed, reset Z heading.
if (gamepad1.a && gamepad1.b) {
// reset heading.
sensorGyro.resetZAxisIntegrator();
}
// get the x, y, and z values (rate of change of angle).
xVal = sensorGyro.rawX();
yVal = sensorGyro.rawY();
zVal = sensorGyro.rawZ();
// get the heading info.
// the Modern Robotics' gyro sensor keeps
// track of the current heading for the Z axis only.
heading = sensorGyro.getHeading();
telemetry.addData("1. x", String.format("%03d", xVal));
telemetry.addData("2. y", String.format("%03d", yVal));
telemetry.addData("3. z", String.format("%03d", zVal));
telemetry.addData("4. h", String.format("%03d", heading));
Thread.sleep(100);
}
}
@Override
public void runOpMode() throws InterruptedException {
motorLeft = hardwareMap.dcMotor.get("motor_1");
motorRight = hardwareMap.dcMotor.get("motor_2");
sensorGyro = hardwareMap.gyroSensor.get("gyro");
// sensorRGB = hardwareMap.colorSensor.get("mr");
motorLeft.setDirection(DcMotor.Direction.REVERSE);
// button = hardwareMap.servo.get("servo_");
climber = hardwareMap.servo.get("servo_2");
climber.setPosition(positionclimber);
odsleft = hardwareMap.opticalDistanceSensor.get("odsleft");
odsright = hardwareMap.opticalDistanceSensor.get("odsright");
sensorUltra = hardwareMap.ultrasonicSensor.get("ultra");
sensorGyro.calibrate();
telemetry.addData("Ultrasonic sensor: ", sensorUltra.getUltrasonicLevel());
waitForStart();
while (sensorGyro.isCalibrating()) {
Thread.sleep(50);
}
sensorGyro.resetZAxisIntegrator();
while (sensorGyro.isCalibrating()) ;
thing = sensorUltra.getUltrasonicLevel();
// while ((thing > high_threshold || thing < 10) && opModeIsActive()) {
// thing = sensorUltra.getUltrasonicLevel();
// motorRight.setPower(-.7);
// motorLeft.setPower(-.7);
// telemetry.addData("Ultrasonic sensor: ", sensorUltra.getUltrasonicLevel());
// telemetry.addData("Right","");
//
// }
thing = sensorUltra.getUltrasonicLevel();
while (thing > threshold || thing <= 1) {
thing = sensorUltra.getUltrasonicLevel();
telemetry.addData("Ultrasonic sensor: ", sensorUltra.getUltrasonicLevel());
motorLeft.setPower(-.7);
motorRight.setPower(-.7);
telemetry.addData("Straight", "");
}
int rightval = 0;
int leftval = 0;
motorRight.setPower(0);
motorLeft.setPower(0);
while (thing < threshold2) {
thing = sensorUltra.getUltrasonicLevel();
motorRight.setPower(.4);
motorLeft.setPower(-.4);
telemetry.addData("Ultrasonic sensor: ", sensorUltra.getUltrasonicLevel());
telemetry.addData("Left", "");
}
rightval = sensorGyro.getHeading();
while (sensorGyro.getHeading() < 358 && sensorGyro.getHeading() > 2) {
motorRight.setPower(-.5);
motorLeft.setPower(.5);
telemetry.addData("Center", "");
}
motorRight.setPower(0);
motorLeft.setPower(0);
thing = sensorUltra.getUltrasonicLevel();
while (thing < threshold3) {
thing = sensorUltra.getUltrasonicLevel();
motorRight.setPower(-.4);
motorLeft.setPower(.4);
telemetry.addData("Ultrasonic sensor: ", thing);
telemetry.addData("Right", "");
}
motorLeft.setPower(0);
motorRight.setPower(0);
Thread.sleep(1000);
leftval = sensorGyro.getHeading();
int dif = (leftval + (360 - rightval)) / 2;
telemetry.addData("rightval", rightval);
telemetry.addData("leftval", leftval);
telemetry.addData("right", Math.abs((rightval + dif) % 360));
telemetry.addData("left", Math.abs((leftval - dif) % 360));
waitOneFullHardwareCycle();
while (sensorGyro.getHeading() < Math.abs((rightval + dif) % 360)
&& sensorGyro.getHeading() > Math.abs((leftval - dif) % 360)) {
motorRight.setPower(.4);
motorLeft.setPower(-.4);
telemetry.addData("Align", "");
}
waitOneFullHardwareCycle();
while ((thing > threshold4 || thing <= 1) && opModeIsActive()) {
thing = sensorUltra.getUltrasonicLevel();
telemetry.addData("Ultrasonic sensor: ", sensorUltra.getUltrasonicLevel());
motorLeft.setPower(-.7);
motorRight.setPower(-.7);
telemetry.addData("Straight", "");
}
waitOneFullHardwareCycle();
motorLeft.setPower(0);
motorRight.setPower(0);
waitOneFullHardwareCycle();
climber.setPosition(.1);
Thread.sleep(1000);
}
