@Override
public void stop() {
stopLift();
liftLeft.close();
liftRight.close();
}
@Override
public void runOpMode() {
elapsedTime = new ElapsedTime();
dim = hardwareMap.deviceInterfaceModule.get("dim");
dim.setDigitalChannelMode(GYROSCOPE_SPOT, DigitalChannelController.Mode.OUTPUT);
gyro = hardwareMap.gyroSensor.get("gyro");
dim.setDigitalChannelState(GYROSCOPE_SPOT, true);
motorBL = hardwareMap.dcMotor.get("motorBL");
motorBR = hardwareMap.dcMotor.get("motorBR");
motorFL = hardwareMap.dcMotor.get("motorFL");
elapsedTime.startTime();
motorFR = hardwareMap.dcMotor.get("motorFR");
// color = hardwareMap.colorSensor.get("color");
int distance1 = 5550;
int distance3 = 9700;
double currentAngle = 0.0;
while (motorBL.getCurrentPosition() < distance1) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
}
while (currentAngle < 90.0) {
startMotors(-1, -1, -1, -1);
getEncoderValues();
currentAngle = gyro.getRotation();
}
while (motorBL.getCurrentPosition() < distance3) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
}
getTime();
elapsedTime.reset();
double currentTime = 0.0;
while (currentTime < 5.0) {
getEncoderValues();
getTime();
currentTime = elapsedTime.time();
}
while (motorBL.getCurrentPosition() > 9000) { // 9034
startMotors(1, -1, -1, 1);
getEncoderValues();
}
while (currentAngle > 45.0) {
startMotors(-1, -1, -1, -1);
getEncoderValues();
currentAngle = gyro.getRotation();
}
elapsedTime.reset();
currentTime = 0.0;
while (currentTime < 5.0) {
startMotors(-1, 1, 1, -1);
getEncoderValues();
getTime();
currentTime = elapsedTime.time();
}
stopMotors();
motorBL.close();
motorFL.close();
motorBR.close();
motorFR.close();
}
@Override
public void close() {
dcMotor.close();
}