API tools faq
paste
Advertisement
AsianInvasion

CurrentAutonCode

AsianInvasion
Feb 19th, 2019
110
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 27.14 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.robotcore.eventloop.opmode.OpMode;
  4. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  5. import java.util.List;
  6. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  7. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  8. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  9. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  10. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
  11. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  12. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  13. import com.qualcomm.robotcore.hardware.Servo;
  14. import com.qualcomm.robotcore.hardware.DcMotor;
  15. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  16. import com.qualcomm.hardware.bosch.BNO055IMU;
  17. // use "import com.qualcomm.robotcore.hardware.Bot;" later, for robot.gyro.position or whatever but
  18. // not now
  19. // calibrate robot when it is on the ground
  20.  
  21.  
  22. //fellow comrades, please note that the encoder ticks below are just for reference, since they
  23. //must be calculated later on
  24. //this is for crater
  25.  
  26. @Autonomous(name = "Julia is the NewBest9")
  27.  
  28. //@Disabled
  29. public class TensorFlowIterative extends OpMode {
  30.  
  31. //Declare OpMode members
  32. private DcMotor upperLeftDrive = null;
  33. private DcMotor lowerLeftDrive = null;
  34. private DcMotor upperRightDrive = null;
  35. private DcMotor lowerRightDrive = null;
  36. private DcMotor hook = null;
  37. private Servo teamMarker = null;
  38. private DcMotor intakeArm = null;
  39. private BNO055IMU imu;
  40. private AngleUnit unit = AngleUnit.DEGREES;
  41.  
  42.  
  43. //these are example ratios and circumferences for the drive train,
  44. //change accordingly, btw for 2:1 it is 2, 80 tooth to 40 tooth
  45. //for gyro, i am assuming that the head of the robot is at 0 initially, and goes clockwise
  46.  
  47. private double DriveGearRatio = 1.0 / 2.0;
  48. private double DriveCircumference = (1.97 * 2) * Math.PI;
  49.  
  50. //not for the actual motor, but for the final gear
  51. private int ticksPerRotation = (int) (DriveGearRatio * 1120);
  52.  
  53. private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
  54. private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
  55. private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
  56. private static final String VUFORIA_KEY = "AbZUuPf/////AAAAGUmS0Chan00iu7rnRhzu63+JgDtPo889M6dNtjvv+WKxiMJ8w2DgSJdM2/zEI+a759I7DlPj++D2Ryr5sEHAg4k1bGKdo3BKtkSeh8hCy78w0SIwoOACschF/ImuyP/V259ytjiFtEF6TX4teE8zYpQZiVkCQy0CmHI9Ymoa7NEvFEqfb3S4P6SicguAtQ2NSLJUX+Fdn49SEJKvpSyhwyjbrinJbak7GWqBHcp7fGh7TNFcfPFMacXg28XxlvVpQaVNgkvuqolN7wkTiR9ZMg6Fnm0zN4Xjr5lRtDHeE51Y0bZoBUbyLWSA+ts3SyDjDPPUU7GMI+Ed/ifb0csVpM12aOiNr8d+HsfF2Frnzrj2";
  57. private VuforiaLocalizer vuforia;
  58. private TFObjectDetector tfod;
  59. private String mineralPosition = "";
  60. private int step = 1;
  61. private int nextstep = 0;
  62. private int targetPosition = 0;
  63. private int position = 1;
  64. private boolean fl;
  65. private boolean fr;
  66. private boolean dl;
  67. private boolean dr;
  68. private double gyroTarget;
  69.  
  70.  
  71. @Override
  72. public void init() {
  73. // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
  74. // first.
  75.  
  76. telemetry.addData("Status", "Initialized");
  77. telemetry.update();
  78.  
  79. upperLeftDrive = hardwareMap.get(DcMotor.class, "front left");
  80. upperRightDrive = hardwareMap.get(DcMotor.class, "front right");
  81. lowerLeftDrive = hardwareMap.get(DcMotor.class, "back left");
  82. lowerRightDrive = hardwareMap.get(DcMotor.class, "back right");
  83. teamMarker = hardwareMap.get(Servo.class, "team marker");
  84. hook = hardwareMap.get(DcMotor.class, "hook");
  85. intakeArm = hardwareMap.get(DcMotor.class, "intake arm");
  86. imu = hardwareMap.get(BNO055IMU.class, "imu");
  87.  
  88. //initialization
  89. upperLeftDrive.setDirection(DcMotor.Direction.FORWARD);
  90. upperRightDrive.setDirection(DcMotor.Direction.REVERSE);
  91. lowerLeftDrive.setDirection(DcMotor.Direction.FORWARD);
  92. lowerRightDrive.setDirection(DcMotor.Direction.REVERSE);
  93.  
  94. }
  95.  
  96. @Override
  97. public void init_loop() {
  98. }
  99.  
  100. @Override
  101. public void start() {
  102. initVuforia();
  103. }
  104.  
  105. //if opMode is active
  106.  
  107. //assumes there's a bug in the code because there is no communication between the robot and the phones
  108. //state machine, upon finishing a state, one can go to another state
  109. //similar to a flow chart, its linear
  110. //make one variable in case 0, but in each case, make it different things, different ways to refer to the same object
  111. //do hook and vuforia
  112. //break makes it exit the switch statement
  113. //make a variable that is 8400
  114. @Override
  115. public void loop() {
  116.  
  117. switch(step) {
  118. case - 2:
  119. if (((gyroTarget - getGyroRotation(unit) + 360.0) % 360.0) < 0.5) {
  120. this.turn(0);
  121. step = nextstep;
  122. }
  123. break;
  124. case -1:
  125. fl = upperLeftDrive.getCurrentPosition() >= position - 20;
  126. fr = upperRightDrive.getCurrentPosition() >= position - 20;
  127. dl = lowerLeftDrive.getCurrentPosition() >= position - 20;
  128. dr = lowerRightDrive.getCurrentPosition() >= position - 20;
  129.  
  130. if (fl && fr && dl && dr) {
  131. //update to see if they are still under the target position
  132. step = nextstep;
  133. }
  134. break;
  135.  
  136. case 0:
  137. if (hook.getCurrentPosition() >= 8400 - 20) {
  138. step = nextstep;
  139. }
  140. break;
  141. case 1:
  142. DrivePosition(8400);
  143. nextstep = 2;
  144. step = 0;
  145. break;
  146. case 2:
  147. //here, position is 0
  148. position = DriveTrain(5, "backwards");
  149. nextstep = 3;
  150. step = -1;
  151. break;
  152. case 3:
  153. gyroTarget = 270.0;
  154. gyroCorrect(270, 5, getGyroRotation(unit), .1, .4);
  155. nextstep = 4;
  156. step = -2;
  157. break;
  158. }
  159.  
  160. telemetry.addLine("" + hook.getCurrentPosition());
  161. telemetry.addLine(upperLeftDrive.getCurrentPosition() + "");
  162. telemetry.addLine(upperRightDrive.getCurrentPosition() + "");
  163. telemetry.addLine(lowerLeftDrive.getCurrentPosition() + "");
  164. telemetry.addLine(lowerRightDrive.getCurrentPosition() + "");
  165. telemetry.addLine(step + "");
  166. telemetry.addLine(position + "");
  167. telemetry.update();
  168.  
  169.  
  170.  
  171. /*
  172.  
  173. //move the hook
  174. DrivePosition(8400);
  175. if (hook.getCurrentPosition() <= 8400 - 20) {
  176. telemetry.addLine(hook.getCurrentPosition() + "");
  177. telemetry.update();
  178. } else if (hook.getCurrentPosition() >= 8400 - 20) {
  179. hook.setPower(0);
  180. } else {
  181. hook.setPower(0);
  182. }
  183.  
  184. //drive off
  185. DriveTrain(5, "backwards");
  186.  
  187. try {
  188. Thread.sleep(10000);
  189. }
  190. catch (Exception e) {
  191. telemetry.addLine("Error, weeb");
  192. telemetry.update();
  193. }
  194.  
  195. long startTime = System.currentTimeMillis(); //log when you started to turn
  196.  
  197. //point the phone to the minerals
  198. while (true) {
  199. gyroCorrect(270, 5, getGyroRotation(unit), .1, .4);
  200.  
  201. long currentTime = System.currentTimeMillis(); // get current time
  202. if (currentTime - startTime > 5000) { // if more than 5 seconds has passed, stop turning and procede
  203. break; //exit loop
  204. }
  205. }
  206.  
  207. //if tfod can be initiated
  208. if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  209. initTfod();
  210. } else {
  211. telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  212. }
  213.  
  214. telemetry.update();
  215.  
  216. // Activate Tensor Flow Object Detection.
  217. if (tfod != null) {
  218. tfod.activate();
  219.  
  220. //allocate time for tfod to work
  221. try {
  222. Thread.sleep(1000);
  223. }
  224. catch (Exception e) {
  225. telemetry.addLine("Sorry, error with the code.");
  226. }
  227.  
  228.  
  229. // getUpdatedRecognitions() will return null if no new information is available since
  230. // the last time that call was made.
  231. List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  232. if (updatedRecognitions != null) {
  233. telemetry.addData("# Object Detected", updatedRecognitions.size());
  234. if (updatedRecognitions != null) {
  235. for (int i = 0; i < updatedRecognitions.size(); i++) {
  236. if (updatedRecognitions.get(i).getWidth() * updatedRecognitions.get(i).getHeight() < 10000 ||
  237. updatedRecognitions.get(i).getWidth() - updatedRecognitions.get(i).getHeight() > 50) {
  238. //If the difference between height and width is larger than 50 it is unlikely to be a mineral
  239. updatedRecognitions.remove(i); // remove the recognition
  240. i--;
  241. }
  242. }
  243.  
  244. //there should be three minerals left
  245. if (updatedRecognitions.size() == 3) {
  246.  
  247. //these initializations are just to check that, later on, the values
  248. //are different and thus minerals have been recognized
  249. int goldMineralX = -1;
  250. int silverMineral1X = -1;
  251. int silverMineral2X = -1;
  252.  
  253. //a for each loop to run through each recognition to check for
  254. //the gold mineral
  255.  
  256. for (Recognition recognition : updatedRecognitions) {
  257.  
  258. //get the label of the recognition according to the neural
  259. //network
  260.  
  261. //save the x coordinate of each identification to subsequent
  262. //mineral variable
  263. if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
  264. goldMineralX = (int) recognition.getLeft();
  265.  
  266. } else if (silverMineral1X == -1) {
  267. silverMineral1X = (int) recognition.getLeft();
  268.  
  269. } else {
  270. silverMineral2X = (int) recognition.getLeft();
  271. }
  272. }
  273.  
  274. //if all three minerals were identified
  275. if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
  276. if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
  277. telemetry.addData("Gold Mineral Position", "Left");
  278. mineralPosition = "left";
  279.  
  280. } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
  281. telemetry.addData("Gold Mineral Position", "Right");
  282. mineralPosition = "right";
  283.  
  284. } else {
  285. telemetry.addData("Gold Mineral Position", "Center");
  286. mineralPosition = "center";
  287.  
  288. }
  289.  
  290. }
  291.  
  292. }
  293.  
  294. //strafe to go closer to the minerals
  295. strafeleft(24);
  296.  
  297. //depending on each mineral location,
  298.  
  299. if (mineralPosition.equals("left")) { //if the gold mineral is all the way on the left (from the perspective
  300. //of the camera)
  301.  
  302. //go backwards towards the leftmost mineral
  303. DriveTrain(30, "backwards");
  304.  
  305. //strafe to hit the mineral
  306. strafeleft(12);
  307.  
  308. //strafe back out
  309. straferight(12);
  310.  
  311. } else if (mineralPosition.equals("right")) {
  312.  
  313. //go backwards towards the rightmost mineral
  314. DriveTrain(30, "forwards");
  315.  
  316. //strafe to hit the mineral
  317. strafeleft(12);
  318.  
  319. //strafe back out
  320. straferight(12);
  321.  
  322. //go back to the left to get ready for the weird turn
  323. DriveTrain(60, "backwards");
  324.  
  325. } else { //most likely, the mineral will be in the center if the above
  326. //if statements fail , but in case tfod doesn't
  327. //detect any minerals, this gives us a fail-safe
  328.  
  329. //strafe to hit the mineral
  330. strafeleft(12);
  331.  
  332. //strafe back out
  333. straferight(12);
  334.  
  335. DriveTrain(30, "backwards");
  336. }
  337.  
  338.  
  339. //do the weird turn
  340. weirdTurn(24);
  341.  
  342. //strafe closer to hit the wall
  343. strafeleft(28);
  344.  
  345. //go towards the depot square
  346. DriveTrain(36, "backwards");
  347.  
  348. //turns towards the depot square
  349. startTime = System.currentTimeMillis(); //log when you started to turn
  350.  
  351. while (true) {
  352. gyroCorrect(-90, 5, getGyroRotation(unit), .1, .4);
  353. //correct to the angle of 90
  354. long currentTime = System.currentTimeMillis(); // get current time
  355. if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
  356. break; //exit loop
  357. }
  358. }
  359.  
  360. //deploy the teamMarker
  361. teamMarker.setPosition(0);
  362.  
  363. //strafe towards the crater
  364. straferight(36);
  365.  
  366. //turn towards the crater
  367. startTime = System.currentTimeMillis(); //log when you started to turn
  368.  
  369. //point the phone to the minerals
  370. while (true) {
  371. gyroCorrect(90, 5, getGyroRotation(unit), .1, .4);
  372. //correct to the angle of 90
  373. long currentTime = System.currentTimeMillis(); // get current time
  374. if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
  375. break; //exit loop
  376. }
  377. }
  378.  
  379. //move the arm to the crater
  380. DrivePositionArm(-360);
  381. telemetry.update();
  382. }
  383. }
  384. }
  385.  
  386. */
  387. }
  388.  
  389. @Override
  390. public void stop() {
  391. if (tfod != null) {
  392. tfod.shutdown();
  393. }
  394. telemetry.addLine("Long live the russian regime! (aka Sasha)");
  395. telemetry.update();
  396. }
  397.  
  398. private void initVuforia() {
  399. //Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  400. //config the vuforia with parameters and variables
  401.  
  402. VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  403.  
  404. parameters.vuforiaLicenseKey = VUFORIA_KEY;
  405. parameters.cameraDirection = CameraDirection.FRONT;
  406.  
  407. // Instantiate the Vuforia engine
  408. vuforia = ClassFactory.getInstance().createVuforia(parameters);
  409.  
  410. // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
  411. }
  412.  
  413. /**
  414. * Initialize the Tensor Flow Object Detection engine.
  415. */
  416. private void initTfod() {
  417. //stores an id for where to later draw the image, a window per se
  418. int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  419. "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  420.  
  421. TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  422.  
  423. //implements the parameters, and passes the image to tfod
  424. tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  425.  
  426. /* professor Wen states that this is a neural network, so each parameter is linked
  427. to a specific identification system and we are simply giving names to those final
  428. identifications to reference them later on */
  429. tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  430. }
  431.  
  432. public void DrivePosition(int position) {
  433. //apparently using negative or positive power here doesn't matter
  434. hook.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  435.  
  436. hook.setTargetPosition(position);
  437.  
  438. hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  439.  
  440. hook.setPower(1);
  441. }
  442.  
  443. public void DrivePositionArm(int position) {
  444. intakeArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  445.  
  446. intakeArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  447.  
  448. intakeArm.setTargetPosition(position);
  449.  
  450. intakeArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  451.  
  452. intakeArm.setPower(1);
  453. }
  454.  
  455. //for forwards, power = -1, and for backwards, power = 1
  456. public int DriveTrain(double distance, String direction) {
  457. //example gear ratio, 2:1
  458. //example circumference
  459. //distance is in inches
  460.  
  461. double circumferenceTraveled = distance / DriveCircumference;
  462.  
  463. int position = (int) (ticksPerRotation * circumferenceTraveled);
  464.  
  465. //if it is forwards or backwards, change sign of position
  466. if (direction.equalsIgnoreCase("forwards")) {
  467. position = -position;
  468. }
  469. else if (direction.equalsIgnoreCase("backwards")) {
  470. //position is equal to itself
  471. }
  472.  
  473. double power = powerScaleDistance(distance);
  474.  
  475. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  476. upperLeftDrive.setTargetPosition(position);
  477. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  478.  
  479. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  480. lowerLeftDrive.setTargetPosition(position);
  481. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  482.  
  483. upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  484. upperRightDrive.setTargetPosition(position);
  485. upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  486.  
  487. lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  488. lowerRightDrive.setTargetPosition(position);
  489. lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  490.  
  491. upperRightDrive.setPower(power);
  492. lowerLeftDrive.setPower(power);
  493. upperRightDrive.setPower(power);
  494. lowerRightDrive.setPower(power);
  495.  
  496. return position;
  497. }
  498.  
  499. //power scale according to distance
  500. public double powerScaleDistance(double distance) {
  501. double DrivePower = 0;
  502. if (distance <= 24) {
  503. DrivePower = 0.5;
  504. }
  505. else if (distance > 24) {
  506. DrivePower = 1;
  507. }
  508. return DrivePower;
  509. }
  510.  
  511. public void strafeleft(double distance) {
  512. //guess and check, then check formulas later on
  513. //assuming a similar relationship from distance to ticks in the drive train, probably going to
  514. //be much less
  515.  
  516. //how much the wheel actually needs to turn
  517. double circumferenceTraveled = distance / DriveCircumference;
  518.  
  519. //here position must be positive
  520. int position = (int) (ticksPerRotation * circumferenceTraveled);
  521.  
  522. double power = powerScaleDistance(distance);
  523.  
  524. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  525. upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  526. upperLeftDrive.setTargetPosition(position);
  527. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  528. upperLeftDrive.setPower(powerScaleDistance(power));
  529.  
  530. upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  531. upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  532. upperRightDrive.setTargetPosition(-position);
  533. upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  534. upperRightDrive.setPower(powerScaleDistance(power));
  535.  
  536. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  537. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  538. lowerLeftDrive.setTargetPosition(-position);
  539. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  540. lowerLeftDrive.setPower(powerScaleDistance(power));
  541.  
  542. lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  543. lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  544. lowerRightDrive.setTargetPosition(position);
  545. lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  546. lowerRightDrive.setPower(powerScaleDistance(power));
  547. }
  548.  
  549. public void straferight(double distance) {
  550. //guess and check, then check formulas later on
  551. //assuming a similar relationship from distance to ticks in the drive train, probably going to
  552. //be much less
  553.  
  554. double circumferenceTraveled = distance / DriveCircumference;
  555.  
  556. int position = (int) (ticksPerRotation * circumferenceTraveled);
  557.  
  558. double power = powerScaleDistance(distance);
  559.  
  560. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  561. upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  562. upperLeftDrive.setTargetPosition(-position);
  563. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  564. upperLeftDrive.setPower(powerScaleDistance(power));
  565.  
  566. upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  567. upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  568. upperRightDrive.setTargetPosition(position);
  569. upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  570. upperRightDrive.setPower(powerScaleDistance(power));
  571.  
  572. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  573. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  574. lowerLeftDrive.setTargetPosition(position);
  575. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  576. lowerLeftDrive.setPower(powerScaleDistance(power));
  577.  
  578. lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  579. lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  580. lowerRightDrive.setTargetPosition(-position);
  581. lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  582. lowerRightDrive.setPower(powerScaleDistance(power));
  583. }
  584.  
  585. public void gyroCorrect(double gyroTarget, double gyroRange, double gyroActual, double minSpeed,
  586. double addSpeed) {
  587.  
  588.  
  589.  
  590. //this makes it an actual angle between 0 and 360 anyways, so it doesn't matter if the target
  591. //is weird for the gyro or actually between 0 and 360
  592. double delta = (gyroTarget - gyroActual + 360.0) % 360.0; // in case it is negative
  593.  
  594. if (delta > 180.0) {
  595. delta -= 360.0; // delta becomes between -180 and 180
  596. //because the range is from 0-> 180 and -180-> 0 instead of 0-> 360
  597. }
  598.  
  599. if (Math.abs(delta) > gyroRange) {
  600. double gyroMod = delta / 45.0; // if delta is less than 45 and bigger than -45, this will make a scale from
  601. // -1 to 1
  602.  
  603. if (Math.abs(gyroMod) > 1.0) {
  604. gyroMod = Math.signum(gyroMod); //makes gyroMod -1 or 1 if error is more than 45
  605. // or less than -45 degrees
  606. }
  607.  
  608. //if the error is more than 180, then the power is positive, and it turns to the left
  609. //if the error is less than 180, the power in the turn in negative, and it turns to the
  610. //right
  611. //if the error is larger, faster speed
  612. this.turn(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod);
  613.  
  614. telemetry.addLine(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod + "");
  615. telemetry.update();
  616.  
  617. } else {
  618. turn(0.0);
  619.  
  620. }
  621. }
  622.  
  623.  
  624. public void turn(double power) { //this is opposite to the varsity because our gear train makes
  625. //forwards negative and backwards positive
  626. upperLeftDrive.setPower(power);
  627. upperRightDrive.setPower(-power);
  628. lowerLeftDrive.setPower(power);
  629. lowerRightDrive.setPower(-power);
  630. }
  631.  
  632. //gets current angle position
  633. public float getGyroRotation(AngleUnit unit) {
  634. return imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, unit).firstAngle;
  635. }
  636.  
  637. //this is a weird turn because only the left half of the motors move
  638. public void weirdTurn(double distance) {
  639. //example gear ratio, 2:1
  640. //example circumference
  641. //distance is in inches
  642.  
  643. double circumferenceTraveled = distance / DriveCircumference;
  644.  
  645. int position = (int) (ticksPerRotation * circumferenceTraveled);
  646.  
  647. //if it is forwards or backwards, change sign of position
  648. double power = powerScaleDistance(distance);
  649.  
  650. //position is positive because it will make the motors go backwards
  651. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  652. upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  653. upperLeftDrive.setTargetPosition(position);
  654. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  655. upperLeftDrive.setPower(powerScaleDistance(power));
  656.  
  657. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  658. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  659. lowerLeftDrive.setTargetPosition(position);
  660. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  661. lowerLeftDrive.setPower(power);
  662. }
  663. }
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!