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