Advertisement
Guest User

crater

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