API tools faq
paste
Advertisement
Golden_Dragoon

Hermes

Golden_Dragoon
Feb 20th, 2019
105
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 31.87 KB | None | 0 0
raw download clone embed print report
  1. AccelUnit.METERS_PERSEC_PERSEC;
  2.         parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
  3.         parameters.loggingEnabled = true;
  4.         parameters.loggingTag = "IMU";
  5.         parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  6.         imu = hardwareMap.get(BNO055IMU.class, "imu");
  7.         imu.initialize(parameters);
  8.  
  9.         telemetry.addData("Status", "IMU Initialized");
  10.         telemetry.update();
  11.  
  12.         //Init hardware
  13.         ForwardLeft = hardwareMap.get(DcMotor.class, "ForwardLeft");
  14.         ForwardLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  15.         ForwardLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  16.  
  17.         ForwardRight = hardwareMap.get(DcMotor.class, "ForwardRight");
  18.         ForwardRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  19.         ForwardRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  20.  
  21.         BackLeft = hardwareMap.get(DcMotor.class, "BackLeft");
  22.         BackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  23.         BackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  24.  
  25.         BackRight = hardwareMap.get(DcMotor.class, "BackRight");
  26.         BackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  27.         BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  28.  
  29.         EncoderY = hardwareMap.get(DcMotor.class, "EncoderY");
  30.         EncoderY.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  31.         EncoderY.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  32.  
  33.         EncoderX = hardwareMap.get(DcMotor.class, "EncoderX");
  34.         EncoderX.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  35.         EncoderX.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  36.  
  37.         LeftLift = hardwareMap.get(DcMotor.class, "EncoderY");
  38.         LeftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  39.         LeftLift.setDirection(DcMotor.Direction.REVERSE);
  40.         LeftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  41.  
  42.         RightLift = hardwareMap.get(DcMotor.class, "RightLift");
  43.         RightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  44.         RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  45.         RightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  46.  
  47.         CableSpool = hardwareMap.get(DcMotor.class, "CableSpool");
  48.         CableSpool.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  49.         BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  50.         CableSpool.setDirection(DcMotor.Direction.REVERSE);
  51.         CableSpool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  52.  
  53.         Intake = hardwareMap.get(DcMotor.class, "EncoderX");
  54.         Intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  55.         Intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  56.  
  57.         Flap = hardwareMap.get(Servo.class, "Flap");
  58.  
  59.         ForwardLeft.setDirection(DcMotor.Direction.FORWARD);
  60.         ForwardRight.setDirection(DcMotor.Direction.REVERSE);
  61.         BackLeft.setDirection(DcMotor.Direction.FORWARD);
  62.         BackRight.setDirection(DcMotor.Direction.REVERSE);
  63.  
  64.         ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  65.         ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  66.         BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  67.         BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  68.  
  69.         telemetry.addData("Status", "Hardware Initialized");
  70.         telemetry.update();
  71.  
  72.         //Init Vuforia
  73.         initVuforia();
  74.  
  75.         telemetry.addData("Status", "Vuforia Initialized");
  76.  
  77.         //Init TFOD
  78.         if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  79.             initTfod();
  80.         } else {
  81.             telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  82.             telemetry.update();
  83.         }
  84.  
  85.         telemetry.addData("Status", "TFOD Initialized");
  86.         telemetry.update();
  87.  
  88.         //Activate TFOD
  89.         if (tfod != null) {
  90.             tfod.activate();
  91.         }
  92.  
  93.         telemetry.addData("Status", "TFOD Activated. Beginning Recognition");
  94.         telemetry.update();
  95.  
  96.         //Begin Recognition steps
  97.         while (isStopRequested() != true && !opModeIsActive()) {
  98.             if (tfod != null) {
  99.                 List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  100.                 if (updatedRecognitions != null) {
  101.                     telemetry.addData("# Object Detected", updatedRecognitions.size());
  102.                     if (updatedRecognitions.size() == 3) {
  103.                         int goldMineralX = -1;
  104.                         int silverMineral1X = -1;
  105.                         int silverMineral2X = -1;
  106.                         for (Recognition recognition : updatedRecognitions) {
  107.                             if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
  108.                                 goldMineralX = (int) recognition.getLeft();
  109.                             } else if (silverMineral1X == -1) {
  110.                                 silverMineral1X = (int) recognition.getLeft();
  111.                             } else {
  112.                                 silverMineral2X = (int) recognition.getLeft();
  113.                             }
  114.                         }
  115.                         if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
  116.                             if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
  117.                                 Position = "LEFT";
  118.                             } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
  119.                                 Position = "RIGHT";
  120.                             } else {
  121.                                 Position = "CENTER";
  122.                             }
  123.                         }
  124.                     }
  125.                 }
  126.             }
  127.             telemetry.addData("Position", Position);
  128.             telemetry.addData("Status", "VARaTiFf DLNN is running");
  129.             telemetry.update();
  130.         }
  131.  
  132.         telemetry.addData("Status", "No longer running VARaTiFf DLNN");
  133.         telemetry.update();
  134.  
  135.         // +=---------------------------------------------------------------------------------------------------------=+
  136.  
  137.  
  138.         waitForStart();
  139.  
  140.         tfod.deactivate();
  141.  
  142.         //todo: I don't really know if the drop code will work right anymore? Besides it was kind of crappy to begin with. Try to find a better solution, moron.
  143.  
  144.         try {
  145.             Thread.sleep(500);
  146.         } catch (InterruptedException ex) {
  147.             Thread.currentThread().interrupt();
  148.         }
  149.  
  150.         zAxis = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  151.         telemetry.addData("Rotation zAxis", zAxis);
  152.  
  153.         telemetry.addData("Status", "All systems online, Running");
  154.         telemetry.update();
  155.  
  156.         RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  157.         changeMode(5);
  158.  
  159.         try {
  160.             Thread.sleep(200);
  161.         } catch (InterruptedException ex) {
  162.             Thread.currentThread().interrupt();
  163.         }
  164.  
  165.         RightLift.setPower(-.4);
  166.         LeftLift.setPower(-.4);
  167.         RightLift.setTargetPosition(-2800);
  168.  
  169.         BackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  170.         BackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  171.         BackLeft.setPower(.2);
  172.         BackRight.setPower(.2);
  173.  
  174.         try {
  175.             Thread.sleep(3000);
  176.         } catch (InterruptedException ex) {
  177.             Thread.currentThread().interrupt();
  178.         }
  179.  
  180.         BackRight.setPower(0);
  181.         BackLeft.setPower(0);
  182.         RightLift.setPower(0);
  183.         LeftLift.setPower(0);
  184.         LeftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  185.         RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  186.  
  187.         //xAxis = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle;
  188.  
  189.         changeMode(1);
  190.  
  191.         try {
  192.             Thread.sleep(900);
  193.         } catch (InterruptedException ex) {
  194.             Thread.currentThread().interrupt();
  195.         }
  196.  
  197.         changeMode(4);
  198.  
  199.         ForwardLeft.setPower(.4);
  200.         ForwardRight.setPower(.4);
  201.         BackLeft.setPower(.4);
  202.         BackRight.setPower(.4);
  203.  
  204.         BackRight.setTargetPosition(400);
  205.         BackLeft.setTargetPosition(400);
  206.         ForwardRight.setTargetPosition(400);
  207.         ForwardLeft.setTargetPosition(400);
  208.  
  209.         while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  210.  
  211.         }
  212.  
  213.         changeMode(1);
  214.  
  215.         ForwardLeft.setTargetPosition(-240);
  216.         ForwardRight.setTargetPosition(240);
  217.         BackLeft.setTargetPosition(240);
  218.         BackRight.setTargetPosition(-240);
  219.  
  220.         while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  221.  
  222.         }
  223.  
  224.         /*try {
  225.             Thread.sleep(1000);
  226.         } catch (InterruptedException ex) {
  227.             Thread.currentThread().interrupt();
  228.         }*/
  229.  
  230.         changeMode(1);
  231.  
  232.         BackRight.setTargetPosition(-200);
  233.         BackLeft.setTargetPosition(-200);
  234.         ForwardRight.setTargetPosition(-200);
  235.         ForwardLeft.setTargetPosition(-200);
  236.         RightLift.setPower(1);
  237.         RightLift.setTargetPosition(-1500);
  238.  
  239.         while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  240.  
  241.         }
  242.  
  243.         /*try {
  244.             Thread.sleep(400);
  245.         } catch (InterruptedException ex) {
  246.             Thread.currentThread().interrupt();
  247.         }*/
  248.  
  249.         changeMode(1);
  250.  
  251.         ForwardLeft.setTargetPosition(240);
  252.         ForwardRight.setTargetPosition(-240);
  253.         BackLeft.setTargetPosition(-240);
  254.         BackRight.setTargetPosition(240);
  255.  
  256.         while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  257.  
  258.         }
  259.  
  260.         /*try {
  261.             Thread.sleep(1000);
  262.         } catch (InterruptedException ex) {
  263.             Thread.currentThread().interrupt();
  264.         }*/
  265.  
  266.         //Have burned through 8 seconds so far
  267.  
  268.         if (Position == "CENTER" || Position == "NULL") {
  269.             changeMode(1);
  270.  
  271.             ForwardLeft.setPower(.3);
  272.             ForwardRight.setPower(.3);
  273.             BackLeft.setPower(.3);
  274.             BackRight.setPower(.3);
  275.  
  276.             ForwardLeft.setTargetPosition(250);
  277.             ForwardRight.setTargetPosition(-250);
  278.             BackLeft.setTargetPosition(250);
  279.             BackRight.setTargetPosition(-250);
  280.  
  281.             while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  282.  
  283.             }
  284.  
  285.             changeMode(1);
  286.  
  287.             ForwardLeft.setTargetPosition(-4000);
  288.             ForwardRight.setTargetPosition(-4000);
  289.             BackLeft.setTargetPosition(-4000);
  290.             BackRight.setTargetPosition(-4000);
  291.  
  292.             while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  293.  
  294.             }
  295.  
  296.             RightLift.setPower(1);
  297.             LeftLift.setPower(.2);
  298.             RightLift.setTargetPosition(-1000);
  299.             Intake.setPower(1);
  300.  
  301.             try {
  302.                 Thread.sleep(1000);
  303.             } catch (InterruptedException ex) {
  304.                 Thread.currentThread().interrupt();
  305.             }
  306.  
  307.             LeftLift.setPower(0);
  308.             RightLift.setTargetPosition(-1900);
  309.             Intake.setPower(0);
  310.  
  311.             changeMode(1);
  312.  
  313.             ForwardLeft.setTargetPosition(-1250);
  314.             ForwardRight.setTargetPosition(1250);
  315.             BackLeft.setTargetPosition(-1250);
  316.             BackRight.setTargetPosition(1250);
  317.  
  318.             while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  319.  
  320.             }
  321.  
  322.             changeMode(1);
  323.  
  324.             ForwardLeft.setTargetPosition(500);
  325.             ForwardRight.setTargetPosition(-500);
  326.             BackLeft.setTargetPosition(-500);
  327.             BackRight.setTargetPosition(500);
  328.  
  329.             while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  330.  
  331.             }
  332.  
  333.             changeMode(1);
  334.  
  335.             ForwardLeft.setTargetPosition(1100);
  336.             ForwardRight.setTargetPosition(1100);
  337.             BackLeft.setTargetPosition(1100);
  338.             BackRight.setTargetPosition(1100);
  339.  
  340.             while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  341.  
  342.             }
  343.  
  344.             changeMode(1);
  345.  
  346.             ForwardLeft.setTargetPosition(400);
  347.             ForwardRight.setTargetPosition(-400);
  348.             BackLeft.setTargetPosition(400);
  349.             BackRight.setTargetPosition(-400);
  350.  
  351.             while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  352.  
  353.             }
  354.  
  355.             changeMode(1);
  356.  
  357.             ForwardLeft.setPower(.6);
  358.             ForwardRight.setPower(.6);
  359.             BackLeft.setPower(.6);
  360.             BackRight.setPower(.6);
  361.  
  362.             ForwardLeft.setTargetPosition(5000);
  363.             ForwardRight.setTargetPosition(5000);
  364.             BackLeft.setTargetPosition(5000);
  365.             BackRight.setTargetPosition(5000);
  366.  
  367.             while(ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy()){
  368.  
  369.             }
  370.  
  371.         }
  372.  
  373.  
  374.         if (Position == "RIGHT") {
  375.             changeMode(1);
  376.  
  377.             ForwardLeft.setPower(.3);
  378.             ForwardRight.setPower(.3);
  379.             BackLeft.setPower(.3);
  380.             BackRight.setPower(.3);
  381.  
  382.             ForwardLeft.setTargetPosition(-300);
  383.             ForwardRight.setTargetPosition(-300);
  384.             BackLeft.setTargetPosition(-300);
  385.             BackRight.setTargetPosition(-300);
  386.  
  387.             try {
  388.                 Thread.sleep(1000);
  389.             } catch (InterruptedException ex) {
  390.                 Thread.currentThread().interrupt();
  391.             }
  392.  
  393.             BackRight.setTargetPosition(-4000);
  394.             ForwardLeft.setTargetPosition(-4000);
  395.  
  396.             try {
  397.                 Thread.sleep(3300);
  398.             } catch (InterruptedException ex) {
  399.                 Thread.currentThread().interrupt();
  400.             }
  401.  
  402.             changeMode(1);
  403.  
  404.             ForwardLeft.setTargetPosition(600);
  405.             ForwardRight.setTargetPosition(-600);
  406.             BackLeft.setTargetPosition(600);
  407.             BackRight.setTargetPosition(-600);
  408.  
  409.             try {
  410.                 Thread.sleep(1000);
  411.             } catch (InterruptedException ex) {
  412.                 Thread.currentThread().interrupt();
  413.             }
  414.  
  415.             changeMode(1);
  416.  
  417.             ForwardLeft.setTargetPosition(-2000);
  418.             ForwardRight.setTargetPosition(-2000);
  419.             BackLeft.setTargetPosition(-2000);
  420.             BackRight.setTargetPosition(-2000);
  421.  
  422.             try {
  423.                 Thread.sleep(1500);
  424.             } catch (InterruptedException ex) {
  425.                 Thread.currentThread().interrupt();
  426.             }
  427.  
  428.             RightLift.setPower(1);
  429.             LeftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  430.             LeftLift.setPower(.2);
  431.             RightLift.setTargetPosition(-1000);
  432.  
  433.             try {
  434.                 Thread.sleep(500);
  435.             } catch (InterruptedException ex) {
  436.                 Thread.currentThread().interrupt();
  437.             }
  438.  
  439.             Intake.setPower(1);
  440.  
  441.             try {
  442.                 Thread.sleep(2000);
  443.             } catch (InterruptedException ex) {
  444.                 Thread.currentThread().interrupt();
  445.             }
  446.  
  447.             LeftLift.setPower(0);
  448.             RightLift.setTargetPosition(-1900);
  449.             Intake.setPower(0);
  450.  
  451.             changeMode(1);
  452.  
  453.             ForwardLeft.setTargetPosition(-500);
  454.             ForwardRight.setTargetPosition(500);
  455.             BackLeft.setTargetPosition(-500);
  456.             BackRight.setTargetPosition(500);
  457.  
  458.             try {
  459.                 Thread.sleep(1000);
  460.             } catch (InterruptedException ex) {
  461.                 Thread.currentThread().interrupt();
  462.             }
  463.  
  464.             changeMode(1);
  465.  
  466.             ForwardLeft.setTargetPosition(1900);
  467.             ForwardRight.setTargetPosition(-1900);
  468.             BackLeft.setTargetPosition(-1900);
  469.             BackRight.setTargetPosition(1900);
  470.  
  471.             try {
  472.                 Thread.sleep(4000);
  473.             } catch (InterruptedException ex) {
  474.                 Thread.currentThread().interrupt();
  475.             }
  476.  
  477.             changeMode(1);
  478.  
  479.             ForwardLeft.setTargetPosition(500);
  480.             ForwardRight.setTargetPosition(-500);
  481.             BackLeft.setTargetPosition(500);
  482.             BackRight.setTargetPosition(-500);
  483.  
  484.             try {
  485.                 Thread.sleep(1000);
  486.             } catch (InterruptedException ex) {
  487.                 Thread.currentThread().interrupt();
  488.             }
  489.  
  490.             changeMode(1);
  491.  
  492.             ForwardLeft.setTargetPosition(10000);
  493.             ForwardRight.setTargetPosition(-10000);
  494.             BackLeft.setTargetPosition(-10000);
  495.             BackRight.setTargetPosition(10000);
  496.  
  497.             try {
  498.                 Thread.sleep(7000);
  499.             } catch (InterruptedException ex) {
  500.                 Thread.currentThread().interrupt();
  501.             }
  502.  
  503.         }
  504.  
  505.  
  506.         if (Position == "LEFT") {
  507.             changeMode(1);
  508.  
  509.             ForwardLeft.setPower(.3);
  510.             ForwardRight.setPower(.3);
  511.             BackLeft.setPower(.3);
  512.             BackRight.setPower(.3);
  513.  
  514.             ForwardLeft.setTargetPosition(-300);
  515.             ForwardRight.setTargetPosition(-300);
  516.             BackLeft.setTargetPosition(-300);
  517.             BackRight.setTargetPosition(-300);
  518.  
  519.             try {
  520.                 Thread.sleep(1000);
  521.             } catch (InterruptedException ex) {
  522.                 Thread.currentThread().interrupt();
  523.             }
  524.  
  525.             ForwardRight.setPower(.4);
  526.             BackLeft.setPower(.4);
  527.             ForwardRight.setTargetPosition(-4000);
  528.             BackLeft.setTargetPosition(-4000);
  529.  
  530.             try {
  531.                 Thread.sleep(3300);
  532.             } catch (InterruptedException ex) {
  533.                 Thread.currentThread().interrupt();
  534.             }
  535.  
  536.             changeMode(1);
  537.  
  538.  
  539.             ForwardRight.setPower(.3);
  540.             BackLeft.setPower(.3);
  541.  
  542.             ForwardLeft.setTargetPosition(-300);
  543.             ForwardRight.setTargetPosition(300);
  544.             BackLeft.setTargetPosition(-300);
  545.             BackRight.setTargetPosition(300);
  546.  
  547.             try {
  548.                 Thread.sleep(1000);
  549.             } catch (InterruptedException ex) {
  550.                 Thread.currentThread().interrupt();
  551.             }
  552.  
  553.             changeMode(1);
  554.  
  555.             ForwardLeft.setTargetPosition(-2000);
  556.             ForwardRight.setTargetPosition(-2000);
  557.             BackLeft.setTargetPosition(-2000);
  558.             BackRight.setTargetPosition(-2000);
  559.  
  560.             try {
  561.                 Thread.sleep(3000);
  562.             } catch (InterruptedException ex) {
  563.                 Thread.currentThread().interrupt();
  564.             }
  565.  
  566.  
  567.             RightLift.setPower(1);
  568.             LeftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  569.             LeftLift.setPower(.2);
  570.             RightLift.setTargetPosition(-1000);
  571.  
  572.             try {
  573.                 Thread.sleep(500);
  574.             } catch (InterruptedException ex) {
  575.                 Thread.currentThread().interrupt();
  576.             }
  577.  
  578.             Intake.setPower(1);
  579.  
  580.             try {
  581.                 Thread.sleep(2000);
  582.             } catch (InterruptedException ex) {
  583.                 Thread.currentThread().interrupt();
  584.             }
  585.  
  586.             LeftLift.setPower(0);
  587.             RightLift.setTargetPosition(-1900);
  588.             Intake.setPower(0);
  589.  
  590.             changeMode(1);
  591.  
  592.             ForwardLeft.setTargetPosition(-600);
  593.             ForwardRight.setTargetPosition(600);
  594.             BackLeft.setTargetPosition(-600);
  595.             BackRight.setTargetPosition(600);
  596.  
  597.             try {
  598.                 Thread.sleep(1900);
  599.             } catch (InterruptedException ex) {
  600.                 Thread.currentThread().interrupt();
  601.             }
  602.  
  603.             changeMode(1);
  604.  
  605.             ForwardLeft.setTargetPosition(10000);
  606.             ForwardRight.setTargetPosition(10000);
  607.             BackLeft.setTargetPosition(10000);
  608.             BackRight.setTargetPosition(10000);
  609.  
  610.             try {
  611.                 Thread.sleep(6000);
  612.             } catch (InterruptedException ex) {
  613.                 Thread.currentThread().interrupt();
  614.             }
  615.         }
  616.  
  617.  
  618.         telemetry.addData("Status", "Completed Detection Execution");
  619.         telemetry.update();
  620.  
  621.  
  622.     }
  623.  
  624.     private void initVuforia() {
  625.         /*
  626.          * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  627.          */
  628.         VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  629.  
  630.         parameters.vuforiaLicenseKey = VUFORIA_KEY;
  631.         parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
  632.  
  633.         //  Instantiate the Vuforia engine
  634.         vuforia = ClassFactory.getInstance().createVuforia(parameters);
  635.  
  636.         // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
  637.     }
  638.  
  639.     private void initTfod() {
  640.         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  641.                 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  642.         TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  643.         tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  644.         tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  645.     }
  646.  
  647.     void changeMode(int Mode) {
  648.         if (Mode == 1) {
  649.             ForwardLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  650.             BackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  651.             ForwardRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  652.             BackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  653.             ForwardLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  654.             BackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  655.             ForwardRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  656.             BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  657.         }
  658.         if (Mode == 2) {
  659.             ForwardLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  660.             BackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  661.             ForwardRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  662.             BackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  663.         }
  664.         if (Mode == 3) {
  665.             ForwardLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  666.             BackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  667.             ForwardRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  668.             BackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  669.         }
  670.         if (Mode == 4) {
  671.             ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  672.             ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  673.             BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  674.             BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  675.         }
  676.         if (Mode == 5) {
  677.             ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  678.             ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  679.             BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  680.             BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  681.         }
  682.     }
  683.  
  684.     /**
  685.      * @param speed    can be any number UNDER 1, the lower the better
  686.      * @param maxThres must be equal to 1 - speed, no more or less
  687.      * @param distance is how far you want to move, in ticks.
  688.      * @param heading  is the wanted heading in degrees
  689.      */
  690.  
  691.     /*public void driveByPID(double speed, double maxThres, int distance, double heading) {
  692.         //This grabs the error, and passes it the heading.
  693.         double error = getError(heading);
  694.         //This creates the proportional multiplier.
  695.         double multiplier = (error / 180) * maxThres;
  696.  
  697.         //I forget if it works exactly like this or not. Guess we'll find you.
  698.  
  699.         ForwardRight.setTargetPosition(distance);
  700.         BackRight.setTargetPosition(distance);
  701.         ForwardLeft.setTargetPosition(distance);
  702.         BackLeft.setTargetPosition(distance);
  703.  
  704.         //The theory behind this is that it will turn like a PID would.
  705.         while (ForwardRight.isBusy() && BackRight.isBusy() && ForwardLeft.isBusy() && BackLeft.isBusy()) {
  706.             //This grabs the error, and passes it the heading.
  707.             error = getError(heading);
  708.             //This creates the proportional multiplier.
  709.             multiplier = (error / 180) * maxThres;
  710.             ForwardRight.setPower(speed - multiplier);
  711.             BackRight.setPower(speed - multiplier);
  712.             ForwardLeft.setPower(speed + multiplier);
  713.             BackLeft.setPower(speed + multiplier);
  714.         }
  715.  
  716.     }
  717.  
  718.     public double getError(double heading) {
  719.         double error;
  720.         error = heading - imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  721.         return error;
  722.     }*/
  723.  
  724.     /*
  725.      *  Method to drive on a fixed compass bearing (angle), based on encoder counts.
  726.      *  Move will stop if either of these conditions occur:
  727.      *  1) Move gets to the desired position
  728.      *  2) Driver stops the opmode running.
  729.      *
  730.      * @param speed      Target speed for forward motion.  Should allow for _/- variance for adjusting heading
  731.      * @param distance   Distance (in inches) to move from current position.  Negative distance means move backwards.
  732.      * @param angle      Absolute Angle (in Degrees) relative to last gyro reset.
  733.      *                   0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
  734.      *                   If a relative angle is required, add/subtract from current heading.
  735.      *
  736.     public void gyroDrive(double speed, double distance, double angle) {
  737.  
  738.         int newLeftTarget;
  739.         int newRightTarget;
  740.         int moveCounts;
  741.         double max;
  742.         double error;
  743.         double steer;
  744.         double leftSpeed;
  745.         double rightSpeed;
  746.  
  747.         //todo yeah like all of this is probs broken soooo fix it
  748.  
  749.         // Ensure that the opmode is still active
  750.         if (opModeIsActive()) {
  751.  
  752.             // Determine new target position, and pass to motor controller
  753.             moveCounts = (int) (distance * COUNTS_PER_INCH);
  754.             newLeftTarget = EncoderY.getCurrentPosition() + moveCounts;
  755.             newRightTarget = EncoderX.getCurrentPosition() + moveCounts;
  756.  
  757.             // Set Target and Turn On RUN_TO_POSITION
  758.             ForwardLeft.setTargetPosition(newLeftTarget);
  759.             ForwardRight.setTargetPosition(newRightTarget);
  760.             BackLeft.setTargetPosition(newLeftTarget);
  761.             BackRight.setTargetPosition(newRightTarget);
  762.  
  763.             ForwardLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  764.             ForwardRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  765.             BackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  766.             BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  767.  
  768.  
  769.             // start motion.
  770.             speed = Range.clip(Math.abs(speed), 0.0, 1.0);
  771.             ForwardLeft.setPower(speed);
  772.             ForwardRight.setPower(speed);
  773.             BackLeft.setPower(speed);
  774.             BackRight.setPower(speed);
  775.  
  776.  
  777.             // keep looping while we are still active, and BOTH motors are running.
  778.             while (opModeIsActive() &&
  779.                     (ForwardLeft.isBusy() && ForwardRight.isBusy() && BackLeft.isBusy() && BackRight.isBusy())) {
  780.  
  781.                 // adjust relative speed based on heading error.
  782.                 error = getError(angle);
  783.                 steer = getSteer(error, P_DRIVE_COEFF);
  784.  
  785.                 // if driving in reverse, the motor correction also needs to be reversed
  786.                 if (distance < 0)
  787.                     steer *= -1.0;
  788.  
  789.                 leftSpeed = speed - steer;
  790.                 rightSpeed = speed + steer;
  791.  
  792.                 // Normalize speeds if either one exceeds +/- 1.0;
  793.                 max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
  794.                 if (max > 1.0) {
  795.                     leftSpeed /= max;
  796.                     rightSpeed /= max;
  797.                 }
  798.  
  799.                 //todo: this part is messed up too probably
  800.  
  801.                 ForwardLeft.setPower(rightSpeed);
  802.                 ForwardRight.setPower(leftSpeed);
  803.                 BackLeft.setPower(rightSpeed);
  804.                 BackRight.setPower(leftSpeed);
  805.  
  806.                 // Display drive status for the driver.
  807.                 telemetry.addData("Err/St", "%5.1f/%5.1f", error, steer);
  808.                 telemetry.addData("Target", "%7d:%7d", newLeftTarget, newRightTarget);
  809.                 telemetry.addData("Actual", "%7d:%7d" /*robot.leftDrive.getCurrentPosition(),
  810.                         robot.rightDrive.getCurrentPosition());
  811.                 telemetry.addData("Speed", "%5.2f:%5.2f", leftSpeed, rightSpeed);
  812.                 telemetry.update();
  813.             }
  814.  
  815.             // Stop all motion;
  816.             ForwardLeft.setPower(0);
  817.             ForwardRight.setPower(0);
  818.             BackLeft.setPower(0);
  819.             BackRight.setPower(0);
  820.  
  821.  
  822.         }
  823.     }
  824.  
  825.     public void gyroTurn(double speed, double angle) {
  826.  
  827.         // keep looping while we are still active, and not on heading.
  828.         while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) {
  829.             // Update telemetry & Allow time for other processes to run.
  830.             telemetry.update();
  831.         }
  832.     }
  833.  
  834.  
  835.     boolean onHeading(double speed, double angle, double PCoeff) {
  836.         double error;
  837.         double steer;
  838.         boolean onTarget = false;
  839.         double leftSpeed;
  840.         double rightSpeed;
  841.  
  842.         // determine turn power based on +/- error
  843.         error = getError(angle);
  844.  
  845.         if (Math.abs(error) <= HEADING_THRESHOLD) {
  846.             steer = 0.0;
  847.             leftSpeed = 0.0;
  848.             rightSpeed = 0.0;
  849.             onTarget = true;
  850.         } else {
  851.             steer = getSteer(error, PCoeff);
  852.             rightSpeed = speed * steer;
  853.             leftSpeed = -rightSpeed;
  854.         }
  855.  
  856.         // Send desired speeds to motors.
  857.         //todo: Fix this, cause it's most likely broken...
  858.         ForwardLeft.setPower(rightSpeed);
  859.         ForwardRight.setPower(leftSpeed);
  860.         BackLeft.setPower(rightSpeed);
  861.         BackRight.setPower(leftSpeed);
  862.  
  863.         // Display it for the driver.
  864.         telemetry.addData("Target", "%5.2f", angle);
  865.         telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
  866.         telemetry.addData("Speed.", "%5.2f:%5.2f", leftSpeed, rightSpeed);
  867.  
  868.         return onTarget;
  869.     }*/
  870.  
  871.     private double getIntegratedHeading() {
  872.         double currentHeading = imu.getAngularOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  873.         double deltaHeading = currentHeading - previousHeading;
  874.  
  875.         if (deltaHeading < -180) {
  876.             deltaHeading += 360;
  877.         } else if (deltaHeading >= 180) {
  878.             deltaHeading -= 360;
  879.         }
  880.  
  881.         integratedHeading += deltaHeading;
  882.         previousHeading = currentHeading;
  883.  
  884.         return integratedHeading;
  885.     }
  886.  
  887.     /*public double getError(double targetAngle) {
  888.  
  889.         double robotError;
  890.  
  891.         // calculate error in -179 to +180 range  (
  892.         robotError = targetAngle - getIntegratedHeading();
  893.         while (robotError > 180) robotError -= 360;
  894.         while (robotError <= -180) robotError += 360;
  895.         return robotError;
  896.     }
  897.  
  898.     public double getSteer(double error, double PCoeff) {
  899.         return Range.clip(error * PCoeff, -1, 1);
  900.     }*/
  901.  
  902. }
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!