public void init() {
motorRight = hardwareMap.dcMotor.get("motor_2");
motorLeft = hardwareMap.dcMotor.get("motor_1");
motorLeft.setDirection(DcMotor.Direction.REVERSE);
gyro = hardwareMap.gyroSensor.get("gyro");
gyro.calibrate();
while (gyro.isCalibrating()) {
try {
wait(20);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
drive = new ArcadeDrive(motorRight, motorLeft, gyro);
components.add(drive);
servoController = hardwareMap.servoController.get("Servo Controller 1");
servoController.pwmEnable();
servo = new NormalServo(servoController, 1);
components.add(servo);
}
public void turnRightgyro(double degreesToTurn, GyroSensor gyro, float power) {
long timeNow;
long timeLast = System.currentTimeMillis();
double gyroOffset = gyro.getRotation();
double degreesLeft = 0.0;
while (degreesLeft < degreesToTurn) {
timeNow = System.currentTimeMillis();
degreesLeft = (gyro.getRotation() - gyroOffset) * (timeNow - timeLast) / 1000;
timeLast = timeNow;
a.setPower(-power);
b.setPower(-power);
c.setPower(power);
c.setPower(power);
}
}
@Override
public void start() {
run_using_encoders();
yeero = hardwareMap.gyroSensor.get("gyro_sensor");
set_point = yeero.getRotation();
}
@Override
public void loop() {
//
// Manage the drive wheel motors.
//
float l_left_drive_power = (float) scale_motor_power(-gamepad1.left_stick_y);
float l_right_drive_power = (float) scale_motor_power(-gamepad1.right_stick_y);
set_drive_power(l_left_drive_power, l_right_drive_power);
//
// Manage the arm motor.
//
float l_gp2_left_stick_y = -gamepad2.left_stick_y;
float l_left_arm_power = (float) scale_motor_power(l_gp2_left_stick_y);
v_motor_left_arm.setPower(l_left_arm_power);
// ----------------------------------------------------------------------
//
// Servo Motors
//
// Obtain the current values of the gamepad 'x' and 'b' buttons.
//
// Note that x and b buttons have boolean values of true and false.
//
// The clip method guarantees the value never exceeds the allowable range of
// [0,1].
//
// The setPosition methods write the motor power values to the Servo
// class, but the positions aren't applied until this method ends.
//
if (gamepad2.x) {
m_hand_position(a_hand_position() + 0.05);
} else if (gamepad2.b) {
m_hand_position(a_hand_position() - 0.05);
}
if (gamepad1.right_bumper) {
set_point = yeero.getRotation();
gyro_degrees = 0.0;
}
t1.scheduleAtFixedRate(new UpdateDegrees(), GYRO_UPDATE_INTERVAL, GYRO_UPDATE_INTERVAL);
//
// Send telemetry data to the driver station.
//
update_telemetry(); // Update common telemetry
telemetry.addData("10", "GP1 Left: " + -gamepad1.left_stick_y);
telemetry.addData("11", "GP1 Right: " + -gamepad1.right_stick_y);
telemetry.addData("12", "GP2 Left: " + l_gp2_left_stick_y);
telemetry.addData("13", "GP2 X: " + gamepad2.x);
telemetry.addData("14", "GP2 Y: " + gamepad2.b);
telemetry.addData("Gyro", "Degrees = " + gyro_degrees);
telemetry.addData("LE", "LEncoder = " + a_left_encoder_count());
telemetry.addData("RE", "REncoder = " + a_right_encoder_count());
}
public void run(GyroSensor gyro, LinearOpMode callingOpMode, Boolean init)
throws InterruptedException {
int initial = gyro.getHeading();
double timeElapsed = 0;
while (init) {
sleep(500);
int current = gyro.getHeading();
int driftChange = current - initial;
timeElapsed = System.nanoTime() / 1000000000; // convert to seconds by dividing by 1 billion
double driftRate = timeElapsed / driftChange; // seconds per degree
callingOpMode.telemetry.addData("Drift Rate", driftRate + " seconds per degree");
callingOpMode.telemetry.addData("Time", timeElapsed);
if (driftRate < 7d) {
callingOpMode.telemetry.addData("Gyro", "Please reinitialize");
break;
}
}
}
@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() {
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 runOpMode() throws InterruptedException {
sweeper = hardwareMap.dcMotor.get("motor_4");
motorLeft = hardwareMap.dcMotor.get("motor_1");
motorRight = hardwareMap.dcMotor.get("motor_2");
climbers = hardwareMap.servo.get("servo_1");
sensorGyro = hardwareMap.gyroSensor.get("gyro");
// sensorRGB = hardwareMap.colorSensor.get("mr");
motorLeft.setDirection(DcMotor.Direction.REVERSE);
wrist = hardwareMap.servo.get("servo_3");
score = hardwareMap.servo.get("servo_1");
climbers = hardwareMap.servo.get("servo_2");
climberknockleft = hardwareMap.servo.get("servo_5");
climberknockright = hardwareMap.servo.get("servo_6");
extend = hardwareMap.servo.get("servo_4");
// button = hardwareMap.servo.get("servo_");
odsleft = hardwareMap.opticalDistanceSensor.get("odsleft");
odsright = hardwareMap.opticalDistanceSensor.get("odsright");
climberknockleft.setPosition(positionclimberknockleftclosed);
climberknockright.setPosition(positionclimberknockrightclosed);
arm = hardwareMap.dcMotor.get("motor_3");
motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
int Rotation = 1440;
int Amount = 6;
int[] Distance = {
5000, 0, 0, 12000, 8000, 0, 0, 0
}; // int[] Distance = {2000,2050,5000, 12000, 8000, 0, 0, 0};//int[] Distance =
// {2000,2050,3000, 12000, 8000, 4000, 0};
// 4250//11000//10500//11500
// {2000,2050,5000, 12000, 8000, 0, 0, 0}
int i = 0;
int xVal, yVal, zVal = 0;
int heading = 0;
// set the starting 2q S 3qA3eASZzxa saxz of the wrist and neck
positionwrist = Range.clip(positionwrist, ARM_MIN_RANGE, ARM_MAX_RANGE);
positionclimber = Range.clip(positionclimber, ARM_MIN_RANGE, ARM_MAX_RANGE);
positionscore = Range.clip(positionscore, ARM_MIN_RANGE, ARM_MAX_RANGE);
wrist.setPosition(0.5);
score.setPosition(0.9);
climbers.setPosition(0.85);
Boolean stop = false;
sensorGyro.calibrate();
waitForStart();
while (sensorGyro.isCalibrating()) {
Thread.sleep(50);
}
// write some device information (connection info, name and type)
// to the log file.
// get a reference to our GyroSensor object.
resetStartTime();
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
// uncomment this to enable sweeper
sweeper.setPower(0.85);
motorRight.setPower(1);
motorLeft.setPower(0.78);
boolean reset = false;
while (opModeIsActive() && this.time < 30) {
if (i < 7) {
telemetry.addData("odsleft", odsleft.getLightDetected());
telemetry.addData("odsright", odsright.getLightDetected());
telemetry.addData("time: ", this.time);
telemetry.addData("i=", i);
if (reset) {
motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
reset = false;
}
if (i == 0) {
// do nothing because already going right way
}
if (i == 1) {
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
// change direction to knock down sweeper
}
/*if(i==2)
{
sweeper.setPower(-0.85);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
}
if(i==3)
{sweeper.setPower(-0.85);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
//go parallel to ramp
}*/
if (i == 4) {
sweeper.setPower(-0.85);
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
// go to bucket
}
if (i == 4) {
telemetry.addData("here", "here");
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
sweeper.setPower(0);
motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
telemetry.addData("odsleft", odsleft.getLightDetected());
telemetry.addData("odsright", odsright.getLightDetected());
if (first) {
starttime = this.time;
first = false;
telemetry.addData("first", "first");
}
drop = true;
condition =
(odsleft.getLightDetected() < distanceleft
|| odsright.getLightDetected() < distanceright)
&& (this.time - starttime < 4.0)
&& opModeIsActive();
if (condition) { // only get 3 seconds to line up so motors don't keep going
/*
* if (odsleft.getLightDetected() < .5)
* {
*
* */
telemetry.addData("odsleft: ", odsleft.getLightDetected());
telemetry.addData("odsright: ", odsright.getLightDetected());
telemetry.addData("time: ", this.time);
telemetry.addData("time taken: ", (this.time - starttime));
if (odsleft.getLightDetected() < distanceleft && opModeIsActive()) {
left = 0;
motorLeft.setPower(0.2);
} else {
left = 1;
motorLeft.setPower(0);
}
if (odsright.getLightDetected() < distanceright && opModeIsActive()) {
right = 0;
motorRight.setPower(0.2);
} else {
right = 1;
motorRight.setPower(0);
}
} else {
motorLeft.setPower(0);
motorRight.setPower(0);
i++;
}
}
if (i != 4) {
motorRight.setTargetPosition(Distance[i]);
motorLeft.setTargetPosition(Distance[i]);
motorRight.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
motorLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
}
if (motorRight.getCurrentPosition() >= Distance[i]
&& motorLeft.getCurrentPosition() >= Distance[i]) {
motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
if (i == 2) {
sweeper.setPower(0);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
while ((heading < 30 || heading > 40) && opModeIsActive()) { // 323,// 327
// turn parallel to ramp
motorLeft.setPower(0.8);
motorRight.setPower(-1);
heading = sensorGyro.getHeading();
telemetry.addData("gyro val: ", heading);
}
}
if (i == 3) {
sweeper.setPower(0);
motorLeft.setDirection(DcMotor.Direction.REVERSE);
motorRight.setDirection(DcMotor.Direction.FORWARD);
while ((heading < 277 || heading > 279)
&& opModeIsActive()) { // 270,272//329,331//323,// 327
// turn to dump climbers
motorLeft.setPower(-0.8);
motorRight.setPower(1.0);
heading = sensorGyro.getHeading();
telemetry.addData("gyro val: ", heading);
}
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.REVERSE);
}
// don't know if this will work
if (i == 6) {
/*
if((servocount>0.02) && opModeIsActive());
{
telemetry.addData("there", "there");
telemetry.addData("climber", servocount);
climbers.setPosition(servocount);
servocount-=0.01;
}
*/
if (true || odsright.getLightDetected() > 0.2) {
climbers.setPosition(0.01);
}
i++;
}
if (i != 4 && i != 6) {
drop = false;
} else {
drop = true;
}
if (!drop) {
motorRight.setPower(0);
motorLeft.setPower(0);
motorRight.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
sleep(50);
reset = true;
motorLeft.setPower(1);
motorRight.setPower(1);
sleep(50);
i++;
}
}
} else {
if (!end) {
programtimetaken = this.time;
end = true;
}
motorLeft.setPower(0);
motorRight.setPower(0);
telemetry.addData("done", programtimetaken);
telemetry.addData("odsleft: ", odsleft.getLightDetected());
telemetry.addData("odsright: ", odsright.getLightDetected());
}
}
motorLeft.setDirection(DcMotor.Direction.FORWARD);
motorRight.setDirection(DcMotor.Direction.FORWARD);
}
@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);
}
@Override
public void runOpMode() throws InterruptedException {
lwa = hardwareMap.dcMotor.get("leftwheelA");
lwb = hardwareMap.dcMotor.get("leftwheelB");
rwa = hardwareMap.dcMotor.get("rightwheelA");
rwb = hardwareMap.dcMotor.get("rightwheelB");
rwa.setDirection(DcMotor.Direction.REVERSE);
rwb.setDirection(DcMotor.Direction.REVERSE);
liftL = hardwareMap.dcMotor.get("liftL");
liftR = hardwareMap.dcMotor.get("liftR");
// liftR.setDirection(DcMotor.Direction.REVERSE);
// liftL.setDirection(DcMotor.Direction.REVERSE);
collector = hardwareMap.dcMotor.get("collector");
// rightCR = hardwareMap.servo.get("rightCR");
// leftCR = hardwareMap.servo.get("leftCR");
swivel = hardwareMap.servo.get("swivel");
dump = hardwareMap.servo.get("dump");
trigL = hardwareMap.servo.get("trigL");
trigR = hardwareMap.servo.get("trigR");
dds = hardwareMap.servo.get("dds");
holdL = hardwareMap.servo.get("holdL");
holdR = hardwareMap.servo.get("holdR");
holdC = hardwareMap.servo.get("holdC");
// leftPivot = hardwareMap.servo.get("leftPivot");
// rightPivot = hardwareMap.servo.get("rightPivot");
lineSensor = hardwareMap.opticalDistanceSensor.get("dist1");
touch = hardwareMap.touchSensor.get("touch");
Gyro = hardwareMap.gyroSensor.get("Gyro");
// leftPivot.setPosition(1);
// rightPivot.setPosition(0);
dump.setPosition(0);
trigL.setPosition(0.8);
trigR.setPosition(0.05);
// leftCR.setPosition(0.5);
// rightCR.setPosition(0.5);
dds.setPosition(1);
holdL.setPosition(0.75);
holdR.setPosition(0.05);
holdC.setPosition(1);
double lineSensorValue;
double heading = 0;
// double integratedHeading = Gyro.getIntegratedZValue();
double headingError;
double targetHeading;
double drivesteering;
double driveGain = 0.005;
double leftPower;
double rightPower;
double midPower = 0;
double minPower = 0.2;
Gyro.calibrate();
// wait for the start button to be pressed
waitForStart();
timer = new ElapsedTime();
collector.setPower(0);
while (rwa.getCurrentPosition() > -7700 && timer.time() < 15) {
rwa.setPower(-0.75);
rwb.setPower(-0.75);
lwa.setPower(-0.75);
lwb.setPower(-0.75);
}
rwa.setPower(0);
rwb.setPower(0);
lwa.setPower(0);
lwb.setPower(0);
sleep(100);
heading = Gyro.getHeading();
telemetry.addData("heading", heading);
heading = 359;
sleep(100);
while (heading > 225) {
heading = Gyro.getHeading();
// integratedHeading = Gyro.getIntegratedZValue();
telemetry.addData("heading", heading);
// telemetry.addData("Integrated", integratedHeading);
targetHeading = 225;
headingError = targetHeading - heading;
drivesteering = driveGain * headingError;
if (drivesteering > 1) {
drivesteering = 1;
telemetry.addData("Caught illegal value", "reset drivesteering to 1");
}
leftPower = midPower + drivesteering;
rightPower = midPower + drivesteering;
if (leftPower > minPower) {
leftPower = minPower;
}
if (rightPower < minPower) {
rightPower = minPower;
}
lwa.setPower(leftPower);
lwb.setPower(leftPower);
rwa.setPower(rightPower);
rwb.setPower(rightPower);
}
while (heading < 225) {
heading = Gyro.getHeading();
// integratedHeading = Gyro.getIntegratedZValue();
telemetry.addData("heading", heading);
// telemetry.addData("Integrated", integratedHeading);
targetHeading = 225;
headingError = targetHeading - heading;
drivesteering = driveGain * headingError;
if (drivesteering > 1) {
drivesteering = 1;
telemetry.addData("Caught illegal value", "reset drivesteering to 1");
}
rightPower = midPower - drivesteering;
leftPower = midPower + drivesteering;
if (rightPower > minPower) {
rightPower = minPower;
}
if (leftPower < minPower) {
leftPower = minPower;
}
lwa.setPower(leftPower);
lwb.setPower(leftPower);
rwa.setPower(rightPower);
rwb.setPower(rightPower);
}
lwa.setPower(1);
lwb.setPower(1);
rwa.setPower(1);
rwb.setPower(1);
sleep(400);
lwa.setPower(0);
lwb.setPower(0);
rwa.setPower(0);
rwb.setPower(0);
telemetry.addData("Event", "Done");
sleep(100);
while (!touch.isPressed() && timer.time() < 20) {
lineSensorValue = lineSensor.getLightDetectedRaw();
if (lineSensorValue < 10) {
lwa.setPower(0.5);
lwb.setPower(0.5);
rwa.setPower(0.0);
rwb.setPower(0.0);
} else {
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.5);
rwb.setPower(0.5);
}
}
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
sleep(100);
collector.setPower(0.0);
sleep(100);
if (timer.time() < 20) {
lwa.setPower(-0.35);
lwb.setPower(-0.35);
rwa.setPower(-0.35);
rwb.setPower(-0.35);
sleep(225);
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
sleep(100);
dds.setPosition(0);
sleep(900);
}
lwa.setPower(-1);
lwb.setPower(-1);
rwa.setPower(-1);
rwb.setPower(-1);
sleep(500);
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
while (heading > 130) {
heading = Gyro.getHeading();
telemetry.addData("heading", heading);
rwa.setPower(0.5);
rwb.setPower(0.5);
lwa.setPower(-0.5);
lwb.setPower(-0.5);
}
lwa.setPower(-0.5);
lwb.setPower(-0.5);
rwa.setPower(-0.5);
rwb.setPower(-0.5);
sleep(1000);
dds.setPosition(1);
lwa.setPower(0.0);
lwb.setPower(0.0);
rwa.setPower(0.0);
rwb.setPower(0.0);
sleep(500);
}