API tools faq
paste
AsianInvasion

CurrentAutonCode

AsianInvasion
Feb 20th, 2019
150
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 33.27 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 = "BrendanAuton23478910111245")
  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. private double gyroRange;
  70. private double minSpeed;
  71. private double addSpeed;
  72. private double power;
  73. private double distance;
  74. private long startTime;
  75. private int encoderSign;
  76.  
  77.  
  78.  
  79.  
  80. @Override
  81. public void init() {
  82. // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
  83. // first.
  84.  
  85. telemetry.addData("Status", "Initialized");
  86. telemetry.update();
  87.  
  88. upperLeftDrive = hardwareMap.get(DcMotor.class, "front left");
  89. upperRightDrive = hardwareMap.get(DcMotor.class, "front right");
  90. lowerLeftDrive = hardwareMap.get(DcMotor.class, "back left");
  91. lowerRightDrive = hardwareMap.get(DcMotor.class, "back right");
  92. teamMarker = hardwareMap.get(Servo.class, "team marker");
  93. hook = hardwareMap.get(DcMotor.class, "hook");
  94. intakeArm = hardwareMap.get(DcMotor.class, "intake arm");
  95. imu = hardwareMap.get(BNO055IMU.class, "imu");
  96.  
  97. //initialization
  98. upperLeftDrive.setDirection(DcMotor.Direction.FORWARD);
  99. upperRightDrive.setDirection(DcMotor.Direction.REVERSE);
  100. lowerLeftDrive.setDirection(DcMotor.Direction.FORWARD);
  101. lowerRightDrive.setDirection(DcMotor.Direction.REVERSE);
  102.  
  103. }
  104.  
  105. @Override
  106. public void init_loop() {
  107. }
  108.  
  109. @Override
  110. public void start() {
  111. initVuforia();
  112. }
  113.  
  114. //if opMode is active
  115.  
  116. //assumes there's a bug in the code because there is no communication between the robot and the phones
  117. //state machine, upon finishing a state, one can go to another state
  118. //similar to a flow chart, its linear
  119. //make one variable in case 0, but in each case, make it different things, different ways to refer to the same object
  120. //do hook and vuforia
  121. //break makes it exit the switch statement
  122. //make a variable that is 8400
  123. @Override
  124. //you can make a varible hold different objects if you want, for example different motors
  125. public void loop() {
  126.  
  127. switch(step) {
  128. case - 2:
  129. long currentTime = System.currentTimeMillis();
  130. if (((gyroTarget - getGyroRotation(unit) + 360.0) % 360.0) < 2.5 ||
  131. currentTime - startTime > 3000) {
  132. this.turn(0);
  133. step = nextstep;
  134. }
  135. else {
  136. gyroCorrect(gyroTarget, gyroRange, getGyroRotation(unit), minSpeed, addSpeed);
  137. }
  138. telemetry.addLine(this.getGyroRotation(unit) + "");
  139. telemetry.update();
  140.  
  141. break;
  142.  
  143. case -1:
  144. fl = Math.abs(upperLeftDrive.getCurrentPosition()) >= Math.abs(position) - 20;
  145. fr = Math.abs(upperRightDrive.getCurrentPosition()) >= Math.abs(position) - 20;
  146. dl = Math.abs(lowerLeftDrive.getCurrentPosition()) >= Math.abs(position) - 20;
  147. dr = Math.abs(lowerRightDrive.getCurrentPosition()) >= Math.abs(position) - 20;
  148.  
  149. if (fl && fr && dl && dr) {
  150. //update to see if they are still under the target position
  151. step = nextstep;
  152.  
  153. }
  154. else {
  155. upperLeftDrive.setPower(power);
  156. upperRightDrive.setPower(power);
  157. lowerLeftDrive.setPower(power);
  158. lowerRightDrive.setPower(power);
  159. }
  160.  
  161. telemetry.addLine(upperLeftDrive.getCurrentPosition() + "");
  162. telemetry.addLine(upperRightDrive.getCurrentPosition() + "");
  163. telemetry.addLine(lowerLeftDrive.getCurrentPosition() + "");
  164. telemetry.addLine(lowerRightDrive.getCurrentPosition() + "");
  165. telemetry.update();
  166.  
  167. break;
  168.  
  169. case 0:
  170. if (hook.getCurrentPosition() >= 8400 - 20) {
  171. step = nextstep;
  172. }
  173. telemetry.addLine("" + hook.getCurrentPosition());
  174. telemetry.update();
  175.  
  176. break;
  177. case 1:
  178. DrivePosition(8400);
  179. nextstep = 2;
  180. step = 0;
  181. break;
  182. case 2:
  183. //here, position is 0
  184. power = powerScaleDistance(5);
  185. encoderSign = this.setDirection(Direction.BACKWARDS);
  186. position = DriveTrain(5);
  187. nextstep = 3;
  188. step = -1;
  189. break;
  190. case 3:
  191. gyroTarget = 270.0;
  192. gyroRange = 5;
  193. minSpeed = .075;
  194. addSpeed = .075;
  195. upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  196. upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  197. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  198. lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  199. gyroCorrect(270, 5, getGyroRotation(unit), .075, .075);
  200. startTime = System.currentTimeMillis();
  201. nextstep = 4;
  202. step = -2;
  203. break;
  204. case 4:
  205. //this is where the actual tfod recognition takes place
  206. if (tfod != null) {
  207. tfod.activate();
  208.  
  209.  
  210. telemetry.addLine(tfod != null ? "tfod is activated" : "tfod is not activated");
  211. telemetry.update();
  212.  
  213.  
  214. // getUpdatedRecognitions() will return null if no new information is available since
  215. // the last time that call was made.
  216. List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  217. if (updatedRecognitions != null) {
  218. telemetry.addData("# Object Detected", updatedRecognitions.size());
  219. if (updatedRecognitions != null) {
  220. for (int i = 0; i < updatedRecognitions.size(); i++) {
  221. if (updatedRecognitions.get(i).getWidth() * updatedRecognitions.get(i).getHeight() < 10000 ||
  222. updatedRecognitions.get(i).getWidth() - updatedRecognitions.get(i).getHeight() > 50) {
  223. //If the difference between height and width is larger than 50 it is unlikely to be a mineral
  224. updatedRecognitions.remove(i); // remove the recognition
  225. i--;
  226. }
  227. }
  228.  
  229. //there should be three minerals left
  230. if (updatedRecognitions.size() == 3) {
  231.  
  232. //these initializations are just to check that, later on, the values
  233. //are different and thus minerals have been recognized
  234. int goldMineralX = -1;
  235. int silverMineral1X = -1;
  236. int silverMineral2X = -1;
  237.  
  238. //a for each loop to run through each recognition to check for
  239. //the gold mineral
  240.  
  241. for (Recognition recognition : updatedRecognitions) {
  242.  
  243. //get the label of the recognition according to the neural
  244. //network
  245.  
  246. //save the x coordinate of each identification to subsequent
  247. //mineral variable
  248. if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
  249. goldMineralX = (int) recognition.getLeft();
  250.  
  251. } else if (silverMineral1X == -1) {
  252. silverMineral1X = (int) recognition.getLeft();
  253.  
  254. } else {
  255. silverMineral2X = (int) recognition.getLeft();
  256. }
  257. }
  258.  
  259. //if all three minerals were identified
  260. if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
  261. if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
  262. telemetry.addData("Gold Mineral Position", "Left");
  263. mineralPosition = "left";
  264.  
  265. } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
  266. telemetry.addData("Gold Mineral Position", "Right");
  267. mineralPosition = "right";
  268.  
  269. } else {
  270. telemetry.addData("Gold Mineral Position", "Center");
  271. mineralPosition = "center";
  272.  
  273. }
  274.  
  275. }
  276.  
  277. }
  278.  
  279. }
  280. }
  281. }
  282. telemetry.addLine(tfod != null ? "tfod is activated" : "tfod is not activated");
  283. telemetry.update();
  284. break;
  285.  
  286.  
  287. }
  288. //all other telemetry has been moved up to save time
  289. telemetry.addLine(step + "");
  290. telemetry.update();
  291.  
  292.  
  293.  
  294. /*
  295.  
  296. //move the hook
  297. DrivePosition(8400);
  298. if (hook.getCurrentPosition() <= 8400 - 20) {
  299. telemetry.addLine(hook.getCurrentPosition() + "");
  300. telemetry.update();
  301. } else if (hook.getCurrentPosition() >= 8400 - 20) {
  302. hook.setPower(0);
  303. } else {
  304. hook.setPower(0);
  305. }
  306.  
  307. //drive off
  308. DriveTrain(5, "backwards");
  309.  
  310. try {
  311. Thread.sleep(10000);
  312. }
  313. catch (Exception e) {
  314. telemetry.addLine("Error, weeb");
  315. telemetry.update();
  316. }
  317.  
  318. long startTime = System.currentTimeMillis(); //log when you started to turn
  319.  
  320. //point the phone to the minerals
  321. while (true) {
  322. gyroCorrect(270, 5, getGyroRotation(unit), .1, .4);
  323.  
  324. long currentTime = System.currentTimeMillis(); // get current time
  325. if (currentTime - startTime > 5000) { // if more than 5 seconds has passed, stop turning and procede
  326. break; //exit loop
  327. }
  328. }
  329.  
  330. //if tfod can be initiated
  331. if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  332. initTfod();
  333. } else {
  334. telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  335. }
  336.  
  337. telemetry.update();
  338.  
  339. // Activate Tensor Flow Object Detection.
  340. if (tfod != null) {
  341. tfod.activate();
  342.  
  343. //allocate time for tfod to work
  344. try {
  345. Thread.sleep(1000);
  346. }
  347. catch (Exception e) {
  348. telemetry.addLine("Sorry, error with the code.");
  349. }
  350.  
  351.  
  352. // getUpdatedRecognitions() will return null if no new information is available since
  353. // the last time that call was made.
  354. List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  355. if (updatedRecognitions != null) {
  356. telemetry.addData("# Object Detected", updatedRecognitions.size());
  357. if (updatedRecognitions != null) {
  358. for (int i = 0; i < updatedRecognitions.size(); i++) {
  359. if (updatedRecognitions.get(i).getWidth() * updatedRecognitions.get(i).getHeight() < 10000 ||
  360. updatedRecognitions.get(i).getWidth() - updatedRecognitions.get(i).getHeight() > 50) {
  361. //If the difference between height and width is larger than 50 it is unlikely to be a mineral
  362. updatedRecognitions.remove(i); // remove the recognition
  363. i--;
  364. }
  365. }
  366.  
  367. //there should be three minerals left
  368. if (updatedRecognitions.size() == 3) {
  369.  
  370. //these initializations are just to check that, later on, the values
  371. //are different and thus minerals have been recognized
  372. int goldMineralX = -1;
  373. int silverMineral1X = -1;
  374. int silverMineral2X = -1;
  375.  
  376. //a for each loop to run through each recognition to check for
  377. //the gold mineral
  378.  
  379. for (Recognition recognition : updatedRecognitions) {
  380.  
  381. //get the label of the recognition according to the neural
  382. //network
  383.  
  384. //save the x coordinate of each identification to subsequent
  385. //mineral variable
  386. if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
  387. goldMineralX = (int) recognition.getLeft();
  388.  
  389. } else if (silverMineral1X == -1) {
  390. silverMineral1X = (int) recognition.getLeft();
  391.  
  392. } else {
  393. silverMineral2X = (int) recognition.getLeft();
  394. }
  395. }
  396.  
  397. //if all three minerals were identified
  398. if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
  399. if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
  400. telemetry.addData("Gold Mineral Position", "Left");
  401. mineralPosition = "left";
  402.  
  403. } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
  404. telemetry.addData("Gold Mineral Position", "Right");
  405. mineralPosition = "right";
  406.  
  407. } else {
  408. telemetry.addData("Gold Mineral Position", "Center");
  409. mineralPosition = "center";
  410.  
  411. }
  412.  
  413. }
  414.  
  415. }
  416.  
  417. //strafe to go closer to the minerals
  418. strafeleft(24);
  419.  
  420. //depending on each mineral location,
  421.  
  422. if (mineralPosition.equals("left")) { //if the gold mineral is all the way on the left (from the perspective
  423. //of the camera)
  424.  
  425. //go backwards towards the leftmost mineral
  426. DriveTrain(30, "backwards");
  427.  
  428. //strafe to hit the mineral
  429. strafeleft(12);
  430.  
  431. //strafe back out
  432. straferight(12);
  433.  
  434. } else if (mineralPosition.equals("right")) {
  435.  
  436. //go backwards towards the rightmost mineral
  437. DriveTrain(30, "forwards");
  438.  
  439. //strafe to hit the mineral
  440. strafeleft(12);
  441.  
  442. //strafe back out
  443. straferight(12);
  444.  
  445. //go back to the left to get ready for the weird turn
  446. DriveTrain(60, "backwards");
  447.  
  448. } else { //most likely, the mineral will be in the center if the above
  449. //if statements fail , but in case tfod doesn't
  450. //detect any minerals, this gives us a fail-safe
  451.  
  452. //strafe to hit the mineral
  453. strafeleft(12);
  454.  
  455. //strafe back out
  456. straferight(12);
  457.  
  458. DriveTrain(30, "backwards");
  459. }
  460.  
  461.  
  462. //do the weird turn
  463. weirdTurn(24);
  464.  
  465. //strafe closer to hit the wall
  466. strafeleft(28);
  467.  
  468. //go towards the depot square
  469. DriveTrain(36, "backwards");
  470.  
  471. //turns towards the depot square
  472. startTime = System.currentTimeMillis(); //log when you started to turn
  473.  
  474. while (true) {
  475. gyroCorrect(-90, 5, getGyroRotation(unit), .1, .4);
  476. //correct to the angle of 90
  477. long currentTime = System.currentTimeMillis(); // get current time
  478. if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
  479. break; //exit loop
  480. }
  481. }
  482.  
  483. //deploy the teamMarker
  484. teamMarker.setPosition(0);
  485.  
  486. //strafe towards the crater
  487. straferight(36);
  488.  
  489. //turn towards the crater
  490. startTime = System.currentTimeMillis(); //log when you started to turn
  491.  
  492. //point the phone to the minerals
  493. while (true) {
  494. gyroCorrect(90, 5, getGyroRotation(unit), .1, .4);
  495. //correct to the angle of 90
  496. long currentTime = System.currentTimeMillis(); // get current time
  497. if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
  498. break; //exit loop
  499. }
  500. }
  501.  
  502. //move the arm to the crater
  503. DrivePositionArm(-360);
  504. telemetry.update();
  505. }
  506. }
  507. }
  508.  
  509. */
  510. }
  511.  
  512. @Override
  513. public void stop() {
  514. if (tfod != null) {
  515. tfod.shutdown();
  516. }
  517. telemetry.addLine("Long live the russian regime! (aka Sasha)");
  518. telemetry.update();
  519. }
  520.  
  521. private void initVuforia() {
  522. //Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  523. //config the vuforia with parameters and variables
  524.  
  525. VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  526.  
  527. parameters.vuforiaLicenseKey = VUFORIA_KEY;
  528. parameters.cameraDirection = CameraDirection.FRONT;
  529.  
  530. // Instantiate the Vuforia engine
  531. vuforia = ClassFactory.getInstance().createVuforia(parameters);
  532.  
  533. // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
  534. }
  535.  
  536. /**
  537. * Initialize the Tensor Flow Object Detection engine.
  538. */
  539. private void initTfod() {
  540. //stores an id for where to later draw the image, a window per se
  541. int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  542. "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  543.  
  544. TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  545.  
  546. //implements the parameters, and passes the image to tfod
  547. tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  548.  
  549. /* professor Wen states that this is a neural network, so each parameter is linked
  550. to a specific identification system and we are simply giving names to those final
  551. identifications to reference them later on */
  552. tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  553. }
  554.  
  555. public void DrivePosition(int position) {
  556. //apparently using negative or positive power here doesn't matter
  557. hook.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  558.  
  559. hook.setTargetPosition(position);
  560.  
  561. hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  562.  
  563. hook.setPower(1);
  564. }
  565.  
  566. public void DrivePositionArm(int position) {
  567. intakeArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  568.  
  569. intakeArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  570.  
  571. intakeArm.setTargetPosition(position);
  572.  
  573. intakeArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  574.  
  575. intakeArm.setPower(1);
  576. }
  577.  
  578. //for forwards, power = -1, and for backwards, power = 1
  579. public int DriveTrain(double distance) {
  580. //example gear ratio, 2:1
  581. //example circumference
  582. //distance is in inches
  583.  
  584. double circumferenceTraveled = distance / DriveCircumference;
  585.  
  586. int position = (int) (ticksPerRotation * circumferenceTraveled);
  587.  
  588. position = encoderSign * position;
  589.  
  590. double power = powerScaleDistance(distance);
  591.  
  592. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  593. upperLeftDrive.setTargetPosition(position);
  594. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  595.  
  596. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  597. lowerLeftDrive.setTargetPosition(position);
  598. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  599.  
  600. upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  601. upperRightDrive.setTargetPosition(position);
  602. upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  603.  
  604. lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  605. lowerRightDrive.setTargetPosition(position);
  606. lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  607.  
  608. upperRightDrive.setPower(power);
  609. lowerLeftDrive.setPower(power);
  610. upperRightDrive.setPower(power);
  611. lowerRightDrive.setPower(power);
  612.  
  613. return position;
  614. }
  615.  
  616. //power scale according to distance
  617. public double powerScaleDistance(double distance) {
  618. double DrivePower = 0;
  619. if (distance <= 24) {
  620. DrivePower = 0.5;
  621. }
  622. else if (distance > 24) {
  623. DrivePower = 1;
  624. }
  625. return DrivePower;
  626. }
  627.  
  628. public void strafeleft(double distance) {
  629. //guess and check, then check formulas later on
  630. //assuming a similar relationship from distance to ticks in the drive train, probably going to
  631. //be much less
  632.  
  633. //how much the wheel actually needs to turn
  634. double circumferenceTraveled = distance / DriveCircumference;
  635.  
  636. //here position must be positive
  637. int position = (int) (ticksPerRotation * circumferenceTraveled);
  638.  
  639. double power = powerScaleDistance(distance);
  640.  
  641. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  642. upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  643. upperLeftDrive.setTargetPosition(position);
  644. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  645. upperLeftDrive.setPower(powerScaleDistance(power));
  646.  
  647. upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  648. upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  649. upperRightDrive.setTargetPosition(-position);
  650. upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  651. upperRightDrive.setPower(powerScaleDistance(power));
  652.  
  653. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  654. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  655. lowerLeftDrive.setTargetPosition(-position);
  656. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  657. lowerLeftDrive.setPower(powerScaleDistance(power));
  658.  
  659. lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  660. lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  661. lowerRightDrive.setTargetPosition(position);
  662. lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  663. lowerRightDrive.setPower(powerScaleDistance(power));
  664. }
  665.  
  666. public void straferight(double distance) {
  667. //guess and check, then check formulas later on
  668. //assuming a similar relationship from distance to ticks in the drive train, probably going to
  669. //be much less
  670.  
  671. double circumferenceTraveled = distance / DriveCircumference;
  672.  
  673. int position = (int) (ticksPerRotation * circumferenceTraveled);
  674.  
  675. double power = powerScaleDistance(distance);
  676.  
  677. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  678. upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  679. upperLeftDrive.setTargetPosition(-position);
  680. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  681. upperLeftDrive.setPower(powerScaleDistance(power));
  682.  
  683. upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  684. upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  685. upperRightDrive.setTargetPosition(position);
  686. upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  687. upperRightDrive.setPower(powerScaleDistance(power));
  688.  
  689. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  690. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  691. lowerLeftDrive.setTargetPosition(position);
  692. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  693. lowerLeftDrive.setPower(powerScaleDistance(power));
  694.  
  695. lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  696. lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  697. lowerRightDrive.setTargetPosition(-position);
  698. lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  699. lowerRightDrive.setPower(powerScaleDistance(power));
  700. }
  701.  
  702. public void gyroCorrect(double gyroTarget, double gyroRange, double gyroActual, double minSpeed,
  703. double addSpeed) {
  704.  
  705. //this makes it an actual angle between 0 and 360 anyways, so it doesn't matter if the target
  706. //is weird for the gyro or actually between 0 and 360
  707. double delta = (gyroTarget - gyroActual + 360.0) % 360.0; // in case it is negative
  708.  
  709. if (delta > 180.0) {
  710. delta -= 360.0; // delta becomes between -180 and 180
  711. //because the range is from 0-> 180 and -180-> 0 instead of 0-> 360
  712. }
  713.  
  714. if (Math.abs(delta) > gyroRange) {
  715. double gyroMod = delta / 45.0; // if delta is less than 45 and bigger than -45, this will make a scale from
  716. // -1 to 1
  717.  
  718. if (Math.abs(gyroMod) > 1.0) {
  719. gyroMod = Math.signum(gyroMod); //makes gyroMod -1 or 1 if error is more than 45
  720. // or less than -45 degrees
  721. }
  722.  
  723. //if the error is more than 180, then the power is positive, and it turns to the left
  724. //if the error is less than 180, the power in the turn in negative, and it turns to the
  725. //right
  726. //if the error is larger, faster speed
  727.  
  728. this.turn(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod);
  729.  
  730. telemetry.addLine(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod + "");
  731. telemetry.update();
  732.  
  733. } else {
  734. turn(0.0);
  735.  
  736. }
  737. }
  738.  
  739.  
  740. public void turn(double power) { //this is opposite to the varsity because our gear train makes
  741. //forwards negative and backwards positive
  742. upperLeftDrive.setPower(power);
  743. upperRightDrive.setPower(-power);
  744. lowerLeftDrive.setPower(power);
  745. lowerRightDrive.setPower(-power);
  746. }
  747.  
  748. //gets current angle position
  749. public float getGyroRotation(AngleUnit unit) {
  750. return imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, unit).firstAngle;
  751. }
  752.  
  753. //this is a weird turn because only the left half of the motors move
  754. public void weirdTurn(double distance) {
  755. //example gear ratio, 2:1
  756. //example circumference
  757. //distance is in inches
  758.  
  759. double circumferenceTraveled = distance / DriveCircumference;
  760.  
  761. int position = (int) (ticksPerRotation * circumferenceTraveled);
  762.  
  763. //if it is forwards or backwards, change sign of position
  764. double power = powerScaleDistance(distance);
  765.  
  766. //position is positive because it will make the motors go backwards
  767. upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  768. upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  769. upperLeftDrive.setTargetPosition(position);
  770. upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  771. upperLeftDrive.setPower(powerScaleDistance(power));
  772.  
  773. lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  774. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  775. lowerLeftDrive.setTargetPosition(position);
  776. lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  777. lowerLeftDrive.setPower(power);
  778. }
  779.  
  780. //depending on the Direction, the position is either positive or negative for the encoders
  781. public int setDirection(Direction direction) {
  782. int positionDirection = 0;
  783. switch (direction) {
  784. case FORWARDS:
  785. positionDirection = 1;
  786. break;
  787. case BACKWARDS:
  788. positionDirection = -1;
  789. break;
  790. }
  791. return positionDirection;
  792. }
  793. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!