API tools faq
paste
Advertisement
Guest User

depot

a guest
Mar 20th, 2019
115
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 29.93 KB | None | 0 0
raw download clone embed print report
  1. /* Copyright (c) 2017 FIRST. All rights reserved.
  2. *
  3. * Redistribution and use in source and binary forms, with or without modification,
  4. * are permitted (subject to the limitations in the disclaimer below) provided that
  5. * the following conditions are met:
  6. *
  7. * Redistributions of source code must retain the above copyright notice, this list
  8. * of conditions and the following disclaimer.
  9. *
  10. * Redistributions in binary form must reproduce the above copyright notice, this
  11. * list of conditions and the following disclaimer in the documentation and/or
  12. * other materials provided with the distribution.
  13. *
  14. * Neither the name of FIRST nor the names of its contributors may be used to endorse or
  15. * promote products derived from this software without specific prior written permission.
  16. *
  17. * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
  18. * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  19. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  20. * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21. * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
  22. * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  23. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  24. * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  25. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  26. * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  27. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  28. */
  29.  
  30. package org.firstinspires.ftc.teamcode;
  31.  
  32. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  33. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  34. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  35. import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  36. import com.qualcomm.robotcore.hardware.DcMotor;
  37. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  38. import com.qualcomm.robotcore.util.ElapsedTime;
  39. import com.qualcomm.robotcore.util.Range;
  40.  
  41. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  42. import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
  43. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  44. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  45. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  46.  
  47. import java.util.List;
  48.  
  49.  
  50. /**
  51. * This file contains an minimal example of a Linear "OpMode". An OpMode is a 'program' that runs in either
  52. * the autonomous or the teleop period of an FTC match. The names of OpModes appear on the menu
  53. * of the FTC Driver Station. When an selection is made from the menu, the corresponding OpMode
  54. * class is instantiated on the Robot Controller and executed.
  55. *
  56. * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot
  57. * It includes all the skeletal structure that all linear OpModes contain.
  58. *
  59. * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
  60. * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
  61. */
  62.  
  63. @Autonomous(name="Autonomie Depot", group="Linear Opmode")
  64. //@Disabled
  65. public class depot extends LinearOpMode {
  66.  
  67. // Declare OpMode members.
  68. private ElapsedTime runtime = new ElapsedTime();
  69. RobotBNG robot = new RobotBNG();
  70. private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
  71. private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
  72. private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
  73. private static final String VUFORIA_KEY = "AfOR5K3/////AAABmdlBtn+igEYQhEK8iiFVxKojIHnUdNvFj3IWgHLrGZcG17s9QYJOCPRRPf5NYeizXyxUsAmGyo8vErJwbq8xXsvs2Mpx9W8+Ndlq2oRNuGYctM4FLfjoa9hFj4YFZSOLNdtO5Rlb7/QLI8ydOjc7n2pIYPIY7hHLz+RuelzhvujdlvrvCU3XGilWESXpEymhEGfatbiRoSQAt67NTyw2XXx0hP8wt43OlAqrvIh2PIf3pInJgJS44aLtVsuZX2ErL+nuWzO9I8KTriPibgdM6PD2z6YJrW9dcRACjjhRAuqS3VY/eqEJ/BMqIVpXn8QvXd5Wr39UeATNP0an5HF76/9xx6ds0VHEXUJBWrovn7VY";
  74. static final double COUNTS_PER_MOTOR_REV = 1120 ;
  75. static final double DRIVE_GEAR_REDUCTION = 0.75 ; // This is < 1.0 if geared UP
  76. static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
  77. static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
  78. (WHEEL_DIAMETER_INCHES * 3.1415);
  79. static final double DRIVE_SPEED = 0.8; // Nominal speed for better accuracy.
  80. static final double TURN_SPEED = 0.5; // Nominal half speed for better accuracy.
  81.  
  82. static final double HEADING_THRESHOLD = 1 ; // As tight as we can make it with an integer gyro
  83. static final double P_TURN_COEFF = 0.05; // Larger is more responsive, but also less stable
  84. static final double P_DRIVE_COEFF = 0.1; // Larger is more responsive, but also less stable
  85. private VuforiaLocalizer vuforia;
  86. public String position;
  87. private TFObjectDetector tfod;
  88. boolean start = false;
  89.  
  90.  
  91. @Override
  92. public void runOpMode() {
  93.  
  94.  
  95. robot.init(hardwareMap);
  96. robot.sliderSucker.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  97. robot.mineralRiser.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  98. robot.suckerFlip.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  99. robot.sliderSucker.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  100. robot.mineralRiser.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  101. robot.suckerFlip.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  102. robot.gyro.calibrate();
  103.  
  104. // make sure the gyro is calibrated before continuing
  105. while (!isStopRequested() && robot.gyro.isCalibrating()) {
  106. sleep(50);
  107. idle();
  108. }
  109. telemetry.addData(">", "Robot Ready."); //
  110. telemetry.addData("Status", "Initialized");
  111. telemetry.update();
  112. initTF();
  113. ActivateTF();
  114. RunTF();
  115.  
  116.  
  117. waitForStart();
  118.  
  119. StopTF();
  120. landing();
  121.  
  122. EncoderStrafe(-DRIVE_SPEED, -6, -6, 3);
  123.  
  124. gyroDrive(DRIVE_SPEED, 12.0, 5.0);
  125. gyroHold(TURN_SPEED, 5.0, 0.5);
  126.  
  127. sleep(2500);
  128. //ExtendSucker(745);
  129. robot.Sucker.setPosition(0f);
  130. sleep(500);
  131. robot.Sucker.setPosition(0.5f);
  132.  
  133. if (position == "LEFT")
  134. {
  135. //ExtendSucker(-705);
  136. sleep(2500);
  137. gyroDrive(DRIVE_SPEED, -4.0, 0.0);
  138. gyroTurn(TURN_SPEED, 45.0);
  139. gyroHold(TURN_SPEED, 45.0, 0.5);
  140. gyroDrive(DRIVE_SPEED, 36.0, 50.0);
  141. gyroDrive(DRIVE_SPEED, -2.0, 50.0);
  142.  
  143. gyroTurn(TURN_SPEED, 130.0);
  144. gyroHold(TURN_SPEED, 130.0, 0.5);
  145. //gyroTurn(TURN_SPEED, 150.0);
  146. //gyroHold(TURN_SPEED, 150.0, 0.5);
  147. gyroDrive(DRIVE_SPEED, 15.0, 130.0);
  148. EncoderStrafe(-DRIVE_SPEED, -6, -6, 3);
  149. EncoderStrafe(DRIVE_SPEED, 2, 2, 3);
  150. gyroDrive(DRIVE_SPEED, 16.0, 130.0);
  151.  
  152. //ExtendSucker(800);
  153. FlipSuckerDown();
  154. }
  155. else if (position == "CENTER")
  156. {
  157. //robot.ExtendSucker(-100);
  158. //FlipSuckerDown();
  159. //ExtendSucker(-605);
  160. //FlipSuckerUp();
  161. sleep(2500);
  162. gyroTurn(TURN_SPEED, 85.0);
  163. gyroHold(TURN_SPEED, 85.0, 0.5);
  164. gyroDrive(DRIVE_SPEED, 47.0, 90.0);
  165. gyroTurn(TURN_SPEED, 115.0);
  166. gyroHold(TURN_SPEED, 115.0, 0.5);
  167. gyroDrive(DRIVE_SPEED, 20.0, 130.0);
  168.  
  169. //ExtendSucker(800);
  170. FlipSuckerDown();
  171. }
  172. else
  173. {
  174. //ExtendSucker(-705);
  175. // gyroTurn(TURN_SPEED, -40.0);
  176. gyroDrive(DRIVE_SPEED, -4.0, 0.0);
  177. gyroTurn(TURN_SPEED, -45.0);
  178. gyroDrive(DRIVE_SPEED, 15.0, -45.0);
  179. gyroDrive(DRIVE_SPEED, -20.0, -45.0);
  180. gyroTurn(TURN_SPEED, 80.0);
  181. gyroDrive(DRIVE_SPEED, 50.0, 80.0);
  182. gyroTurn(TURN_SPEED, 130.0);
  183. gyroDrive(DRIVE_SPEED, 7.0, 130.0);
  184.  
  185. /*gyroHold(TURN_SPEED, 0.0, 0.5);
  186. gyroDrive(DRIVE_SPEED, 30.0, -50.0);
  187. gyroDrive(DRIVE_SPEED, -19.0, 0.0);
  188. gyroTurn(TURN_SPEED, 93.0);
  189. gyroHold(TURN_SPEED, 93.0, 0.5);
  190. gyroDrive(DRIVE_SPEED, 63, 93.0);
  191. gyroTurn(TURN_SPEED, 130.0);
  192. gyroHold(TURN_SPEED, 130.0, 0.5);*/
  193. //ExtendSucker(800);
  194. FlipSuckerDown();
  195. }
  196.  
  197. telemetry.addData("Mineral ", position);
  198. telemetry.update();
  199. runtime.reset();
  200.  
  201. }
  202. public void ExtendSucker(int mmToExtend) {
  203. int newTargetPos = robot.sliderSucker.getCurrentPosition() + (mmToExtend * robot.extendMotorCountsPerRev60 / robot.mmPerSuckerExtention);
  204. robot.sliderSucker.setTargetPosition(newTargetPos);
  205. robot.sliderSucker.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  206. if(mmToExtend > 0)
  207. robot.sliderSucker.setPower(1f * Math.signum(mmToExtend));
  208. else robot.sliderSucker.setPower(-0.5f);
  209.  
  210. while (robot.sliderSucker.isBusy() && !isStopRequested()) {
  211.  
  212. }
  213.  
  214. robot.sliderSucker.setPower(0f);
  215. robot.sliderSucker.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  216.  
  217. }
  218.  
  219.  
  220. private void initVuforia() throws RuntimeException {
  221. /*
  222. * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  223. */
  224. VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  225.  
  226. parameters.vuforiaLicenseKey = VUFORIA_KEY;
  227. parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
  228. // Instantiate the Vuforia engine
  229. vuforia = ClassFactory.getInstance().createVuforia(parameters);
  230. // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
  231. }
  232.  
  233. /**
  234. * Initialize the Tensor Flow Object Detection engine.
  235. */
  236. private void initTfod() {
  237. int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  238. "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  239. TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  240. tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  241. tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  242. }
  243.  
  244.  
  245. public void initTF() throws RuntimeException
  246. {
  247. initVuforia();
  248.  
  249. if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  250. initTfod();
  251. } else {
  252. telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  253. }
  254. }
  255.  
  256. public void ActivateTF() throws RuntimeException
  257. {
  258. if (tfod != null) {
  259. tfod.activate();
  260. }
  261. }
  262.  
  263. public void RunTF() throws RuntimeException
  264. {
  265. if (tfod != null) {
  266. // getUpdatedRecognitions() will return null if no new information is available since
  267. // the last time that call was made.
  268. while(!opModeIsActive() && !isStopRequested()) {
  269. List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  270. if (updatedRecognitions != null) {
  271. telemetry.addData("# Object Detected", updatedRecognitions.size());
  272. if (updatedRecognitions.size() == 1)
  273. {
  274. for (Recognition recognition : updatedRecognitions)
  275. {
  276. if (recognition.getLabel().equals(LABEL_SILVER_MINERAL))
  277. {
  278. if (recognition.getLeft() <= recognition.getImageWidth() / 2)
  279. position = "RIGHT";
  280. else
  281. position = "CENTER";
  282. }
  283.  
  284. else if (recognition.getLabel().equals(LABEL_GOLD_MINERAL))
  285. {
  286. if (recognition.getLeft() <= recognition.getImageWidth() / 2)
  287. position = "CENTER";
  288. else
  289. position = "RIGHT";
  290. }
  291. }
  292. telemetry.addData("Gold Mineral Position", position);
  293. telemetry.update();
  294. }
  295.  
  296. if (updatedRecognitions.size() >= 2) {
  297. int goldMineralX = -1;
  298. int silverMineralX = -1;
  299. int goldMineralMax = -1;
  300. int silverMineralMax = -1;
  301.  
  302. for (Recognition recognition : updatedRecognitions) {
  303. if (recognition.getLabel().equals(LABEL_GOLD_MINERAL) /*&& recognition.getHeight() * recognition.getWidth() > goldMineralMax*/) {
  304. goldMineralX = (int) recognition.getLeft();
  305. goldMineralMax = (int) recognition.getHeight();
  306. } else if (recognition.getLabel().equals(LABEL_SILVER_MINERAL) /*&& recognition.getHeight() * recognition.getWidth() > silverMineralMax*/) {
  307. silverMineralX = (int) recognition.getLeft();
  308. silverMineralMax = (int) recognition.getHeight();
  309. }
  310. }
  311.  
  312. if (goldMineralX != -1 && silverMineralX != -1) {
  313. if (goldMineralX < silverMineralX) {
  314. position = "CENTER";
  315. telemetry.addData("Gold Mineral Position", "CENTER");
  316. } else {
  317. position = "RIGHT";
  318. telemetry.addData("Gold Mineral Position", "RIGHT");
  319. }
  320. }
  321. else {
  322. position = "LEFT";
  323. telemetry.addData("Gold Mineral Position", "LEFT");
  324.  
  325. }
  326. telemetry.update();
  327.  
  328.  
  329. }
  330.  
  331.  
  332. }
  333. }
  334. }
  335. }
  336.  
  337.  
  338.  
  339. public void StopTF() {
  340. if (tfod != null) {
  341. tfod.shutdown();
  342. }
  343. }
  344. public void gyroDrive ( double speed,
  345. double distance,
  346. double angle) {
  347.  
  348. int newLeftTarget;
  349. int newRightTarget;
  350. int moveCounts;
  351. double max;
  352. double error;
  353. double steer;
  354. double leftSpeed;
  355. double rightSpeed;
  356.  
  357. // Ensure that the opmode is still active
  358. if (opModeIsActive() && !isStopRequested()) {
  359.  
  360. robot.motorLeft1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  361. robot.motorLeft2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  362. robot.motorRight2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  363. robot.motorRight2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  364.  
  365. double semn = 1;
  366. if (distance < 0) {
  367. robot.motorLeft1.setDirection(DcMotor.Direction.FORWARD);
  368. robot.motorLeft2.setDirection(DcMotor.Direction.FORWARD);
  369. robot.motorRight1.setDirection(DcMotor.Direction.REVERSE);
  370. robot.motorRight2.setDirection(DcMotor.Direction.REVERSE);
  371. semn = -1;
  372. distance = -distance;
  373.  
  374. }
  375. else {
  376. robot.motorLeft1.setDirection(DcMotor.Direction.REVERSE);
  377. robot.motorLeft2.setDirection(DcMotor.Direction.REVERSE);
  378. robot.motorRight1.setDirection(DcMotor.Direction.FORWARD);
  379. robot.motorRight2.setDirection(DcMotor.Direction.FORWARD);
  380. }
  381. // Determine new target position, and pass to motor controller
  382. moveCounts = (int)(distance * COUNTS_PER_INCH);
  383. newLeftTarget = robot.motorLeft1.getCurrentPosition() + moveCounts;
  384. newRightTarget = robot.motorRight1.getCurrentPosition() + moveCounts;
  385.  
  386. // Set Target and Turn On RUN_TO_POSITION
  387. robot.motorLeft1.setTargetPosition(newLeftTarget);
  388. robot.motorRight1.setTargetPosition(newRightTarget);
  389.  
  390. robot.motorLeft2.setTargetPosition( robot.motorLeft2.getCurrentPosition() + moveCounts);
  391. robot.motorRight2.setTargetPosition( robot.motorRight2.getCurrentPosition() + moveCounts);
  392.  
  393. robot.motorLeft1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  394. robot.motorRight1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  395.  
  396. robot.motorLeft2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  397. robot.motorRight2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  398.  
  399. // start motion.
  400.  
  401. speed = Range.clip(Math.abs(speed), 0.0, 1.0);
  402. robot.motorLeft1.setPower(speed);
  403. robot.motorRight1.setPower(speed);
  404. robot.motorLeft2.setPower(speed);
  405. robot.motorRight2.setPower(speed);
  406.  
  407. // keep looping while we are still active, and BOTH motors are running.
  408. while (!isStopRequested() && opModeIsActive() &&
  409. (robot.motorLeft1.isBusy() && robot.motorRight1.isBusy() && robot.motorLeft2.isBusy() && robot.motorRight2.isBusy())) {
  410.  
  411. // adjust relative speed based on heading error.
  412. error = getError(angle);
  413. steer = getSteer(error, P_DRIVE_COEFF);
  414.  
  415. // if driving in reverse, the motor correction also needs to be reversed
  416. if (semn < 0)
  417. steer *= -1.0;
  418.  
  419. leftSpeed = speed - steer;
  420. rightSpeed = speed + steer;
  421.  
  422. // Normalize speeds if either one exceeds +/- 1.0;
  423. max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
  424. if (max > 1.0)
  425. {
  426. leftSpeed /= max;
  427. rightSpeed /= max;
  428. }
  429.  
  430. robot.motorLeft1.setPower(leftSpeed);
  431. robot.motorRight1.setPower(rightSpeed);
  432. robot.motorLeft2.setPower(leftSpeed);
  433. robot.motorRight2.setPower(rightSpeed);
  434.  
  435. // Display drive status for the driver.
  436. telemetry.addData("Err/St", "%5.1f/%5.1f", error, steer);
  437. telemetry.addData("Target", "%7d:%7d", newLeftTarget, newRightTarget);
  438. telemetry.addData("Actual", "%7d:%7d", robot.motorLeft1.getCurrentPosition(),
  439. robot.motorRight1.getCurrentPosition());
  440. telemetry.addData("Speed", "%5.2f:%5.2f", leftSpeed, rightSpeed);
  441. telemetry.addData("heading ", robot.gyro.getHeading());
  442.  
  443. telemetry.update();
  444.  
  445. }
  446.  
  447. // Stop all motion;
  448. robot.motorRight1.setPower(0);
  449. robot.motorLeft1.setPower(0);
  450.  
  451. robot.motorRight2.setPower(0);
  452. robot.motorLeft2.setPower(0);
  453.  
  454. // Turn off RUN_TO_POSITION
  455. robot.motorRight1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  456. robot.motorLeft1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  457. robot.motorRight2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  458. robot.motorLeft2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  459.  
  460. }
  461. }
  462. public void gyroTurn ( double speed, double angle) {
  463.  
  464. robot.motorLeft1.setDirection(DcMotor.Direction.REVERSE);
  465. robot.motorLeft2.setDirection(DcMotor.Direction.REVERSE);
  466. robot.motorRight1.setDirection(DcMotor.Direction.FORWARD);
  467. robot.motorRight2.setDirection(DcMotor.Direction.FORWARD);
  468.  
  469. // keep looping while we are still active, and not on heading.
  470. while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF) && !isStopRequested()) {
  471. // Update telemetry & Allow time for other processes to run.
  472. telemetry.update();
  473. }
  474. }
  475.  
  476. public void gyroHold( double speed, double angle, double holdTime) {
  477.  
  478. ElapsedTime holdTimer = new ElapsedTime();
  479.  
  480. // keep looping while we have time remaining.
  481. holdTimer.reset();
  482. while (opModeIsActive() && (holdTimer.time() < holdTime) && !isStopRequested()) {
  483. // Update telemetry & Allow time for other processes to run.
  484. onHeading(speed, angle, P_TURN_COEFF);
  485. telemetry.update();
  486. }
  487.  
  488. // Stop all motion;
  489. robot.motorLeft1.setPower(0);
  490. robot.motorRight1.setPower(0);
  491. robot.motorLeft2.setPower(0);
  492. robot.motorRight2.setPower(0);
  493.  
  494. }
  495.  
  496. boolean onHeading(double speed, double angle, double PCoeff) {
  497. double error ;
  498. double steer ;
  499. boolean onTarget = false ;
  500. double leftSpeed;
  501. double rightSpeed;
  502.  
  503. // determine turn power based on +/- error
  504. error = getError(angle);
  505.  
  506. if (Math.abs(error) <= HEADING_THRESHOLD) {
  507. steer = 0.0;
  508. leftSpeed = 0.0;
  509. rightSpeed = 0.0;
  510. onTarget = true;
  511. }
  512. else {
  513. steer = getSteer(error, PCoeff);
  514. rightSpeed = speed * steer;
  515. leftSpeed = -rightSpeed;
  516. }
  517.  
  518. // Send desired speeds to motors.
  519. robot.motorLeft1.setPower(leftSpeed);
  520. robot.motorRight1.setPower(rightSpeed);
  521.  
  522. robot.motorLeft2.setPower(leftSpeed);
  523. robot.motorRight2.setPower(rightSpeed);
  524.  
  525. // Display it for the driver.
  526. telemetry.addData("Target", "%5.2f", angle);
  527. telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
  528. telemetry.addData("Speed.", "%5.2f:%5.2f", leftSpeed, rightSpeed);
  529.  
  530. return onTarget;
  531. }
  532. public double getError(double targetAngle) {
  533.  
  534. double robotError;
  535.  
  536. // calculate error in -179 to +180 range (
  537. robotError = targetAngle - robot.gyro.getIntegratedZValue();
  538. while (robotError > 180) robotError -= 360;
  539. while (robotError <= -180) robotError += 360;
  540. return robotError;
  541. }
  542.  
  543. /**
  544. * returns desired steering force. +/- 1 range. +ve = steer left
  545. * @param error Error angle in robot relative degrees
  546. * @param PCoeff Proportional Gain Coefficient
  547. * @return
  548. */
  549. public double getSteer(double error, double PCoeff) {
  550. return Range.clip(error * PCoeff, -1, 1);
  551. }
  552. public void EncoderStrafe(double speed,
  553. double leftInches, double rightInches,
  554. double timeoutS) {
  555. int newLeftTarget;
  556. int newRightTarget;
  557. robot.motorLeft1.setDirection(DcMotor.Direction.FORWARD);
  558. robot.motorLeft2.setDirection(DcMotor.Direction.REVERSE);
  559.  
  560. robot.motorRight1.setDirection(DcMotor.Direction.FORWARD);
  561. robot.motorRight2.setDirection(DcMotor.Direction.REVERSE);
  562.  
  563. robot.motorLeft1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  564. robot.motorLeft2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  565. robot.motorRight1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  566. robot.motorRight2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  567.  
  568. robot.motorLeft1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  569. robot.motorRight1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  570. robot.motorLeft2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  571. robot.motorRight2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  572.  
  573. // Ensure that the opmode is still active
  574. if (opModeIsActive()) {
  575.  
  576. // Determine new target position, and pass to motor controller
  577. newLeftTarget = robot.motorLeft1.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH);
  578. newRightTarget = robot.motorRight1.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
  579. robot.motorLeft1.setTargetPosition(newLeftTarget);
  580. robot.motorRight1.setTargetPosition(newRightTarget);
  581. robot.motorLeft2.setTargetPosition(robot.motorLeft2.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH));
  582. robot.motorRight2.setTargetPosition(robot.motorRight2.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH));
  583.  
  584.  
  585. // Turn On RUN_TO_POSITION
  586. robot.motorLeft1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  587. robot.motorRight1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  588. robot.motorLeft2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  589. robot.motorRight2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  590.  
  591. // reset the timeout time and start motion.
  592. runtime.reset();
  593. robot.motorLeft2.setPower(speed);
  594. robot.motorRight1.setPower(speed);
  595. robot.motorLeft1.setPower(speed);
  596. robot.motorRight2.setPower(speed);
  597.  
  598.  
  599. // keep looping while we are still active, and there is time left, and both motors are running.
  600. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
  601. // its target position, the motion will stop. This is "safer" in the event that the robot will
  602. // always end the motion as soon as possible.
  603. // However, if you require that BOTH motors have finished their moves before the robot continues
  604. // onto the next step, use (isBusy() || isBusy()) in the loop test.
  605. while (!isStopRequested() && opModeIsActive() &&
  606. (runtime.seconds() < timeoutS) &&
  607. (robot.motorLeft2.isBusy() && robot.motorRight1.isBusy())) {
  608.  
  609. // Display it for the driver.
  610. telemetry.addData("Path1", "Running to %7d :%7d", newLeftTarget, newRightTarget);
  611. telemetry.addData("Path2", "Running at %7d :%7d",
  612. robot.motorLeft2.getCurrentPosition(),
  613. robot.motorRight2.getCurrentPosition());
  614. telemetry.update();
  615. }
  616.  
  617. // Stop all motion;
  618. robot.motorLeft2.setPower(0);
  619. robot.motorRight2.setPower(0);
  620. robot.motorLeft1.setPower(0);
  621. robot.motorRight1.setPower(0);
  622. // Turn off RUN_TO_POSITION
  623. robot.motorLeft1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  624. robot.motorRight1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  625. robot.motorLeft2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  626. robot.motorRight2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  627.  
  628. // sleep(250); // optional pause after each move
  629. }
  630. }
  631. private void FlipSuckerDown()
  632. {
  633. double time = runtime.milliseconds();
  634. robot.MineralFlip.setPosition(0.52f);
  635.  
  636. robot.suckerFlip.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  637. robot.suckerFlip.setTargetPosition(728);
  638. robot.suckerFlip.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  639. robot.suckerFlip.setPower(0.25f);
  640. while (robot.suckerFlip.isBusy() && runtime.milliseconds() - time < 1200.0 && !isStopRequested()) {
  641.  
  642. }
  643. robot.suckerFlip.setPower(0f);
  644. }
  645.  
  646.  
  647. private void FlipSuckerUp()
  648. {
  649.  
  650. if (robot.suckerFlip.getCurrentPosition() > 350) {
  651. robot.MineralFlip.setPosition(0.61f);
  652. robot.suckerFlip.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  653. robot.suckerFlip.setTargetPosition(350);
  654. robot.suckerFlip.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  655. robot.suckerFlip.setPower(-0.5f);
  656. while (robot.suckerFlip.isBusy() && !isStopRequested()) {
  657.  
  658.  
  659. }
  660. }
  661.  
  662.  
  663. robot.suckerFlip.setTargetPosition(145);
  664. robot.suckerFlip.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  665. robot.suckerFlip.setPower(-0.1f);
  666. while (robot.suckerFlip.isBusy() && !isStopRequested()) {
  667.  
  668.  
  669. }
  670.  
  671.  
  672. robot.suckerFlip.setPower(0f);
  673. robot.suckerFlip.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  674. }
  675. public void landing() {
  676. int newTargetPos = 16200; //-39650
  677. robot.carlig.setPower(1f);
  678. plasareMinerale(20);
  679. while (robot.carlig.getCurrentPosition() < newTargetPos && !isStopRequested()) {
  680.  
  681. }
  682.  
  683. robot.carlig.setPower(0f);
  684. }
  685. public void plasareMinerale(int mmToExtend) {
  686. int newTargetPos = robot.mineralRiser.getCurrentPosition() + (mmToExtend * robot.extendMotorCountsPerRev60 / robot.mmPerRotationMineralRiser);// 216 mm o rotatie completa
  687. robot.mineralRiser.setTargetPosition(newTargetPos);
  688. robot.mineralRiser.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  689. robot.mineralRiser.setPower(1f * Math.signum(mmToExtend));
  690. while (robot.mineralRiser.isBusy() && !isStopRequested()) {
  691.  
  692. }
  693.  
  694. robot.mineralRiser.setPower(0f);
  695. robot.mineralRiser.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  696. }
  697.  
  698. private void FlipSuckerMiddle()
  699. {
  700.  
  701. robot.suckerFlip.setTargetPosition(200);
  702. robot.suckerFlip.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  703. robot.suckerFlip.setPower(0.2f);
  704. while (robot.suckerFlip.isBusy() && !isStopRequested()) {
  705.  
  706.  
  707. }
  708.  
  709.  
  710. robot.suckerFlip.setPower(0f);
  711. robot.suckerFlip.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  712. }
  713. private void Sugere()
  714. {
  715. robot.Sucker.setPosition(0f);
  716. sleep(1000);
  717. robot.Sucker.setPosition(0.49);
  718. }
  719. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!