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