API tools faq
paste
Advertisement
Golden_Dragoon

Hades

Golden_Dragoon
Feb 20th, 2019
95
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 11.75 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode.Autonomous;
  2.  
  3. import com.qualcomm.hardware.bosch.BNO055IMU;
  4. import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
  5. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  6. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  7. import com.qualcomm.robotcore.hardware.DcMotor;
  8. import com.qualcomm.robotcore.hardware.Servo;
  9. import com.qualcomm.robotcore.util.ElapsedTime;
  10. import com.qualcomm.robotcore.util.Range;
  11.  
  12. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  13. import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
  14. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  15. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  16. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  17. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  18. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  19. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  20. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  21.  
  22. import java.util.List;
  23.  
  24. @Autonomous(name = "Hades - Facing Depot - Pos 2", group = "Coeus")
  25. public class Hades extends LinearOpMode {
  26.  
  27.     private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
  28.     private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
  29.     private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
  30.     private static final String VUFORIA_KEY = "AXSqUwf/////AAABmVi9ifY9YkX7hla6dk//kFRSGKk+yWO9wuL666vmsKegzx+5Io14hzrDHPPSIOUIuBd89kbyiEj6ckbCr0Ak3uWm37MVO6WuHj2iyDGBiFASfDpXymVBKsTiT12M0KxuGVLdxg7JBLeNMq2f7lV/vtdmBf+UOVaruaibuICmy0jdCALIN7Edd3WcSYlY8V6VCxMPqLw4MrRbkPSshdxQ2WVZsDSZJgPqBE43qSmCFHhPwggb4+ZFSe6nn6SureHq5pNgyxeUXCaCsaIDbwHqEL7NrhOT6sqQTrWCsay3dFs6uVPY4YqJEP6+YGq7vYG7aJOC6nro3Z9NnVBhfwZEM07o6imRE0nkNd380XSO46rZ";
  31.     DcMotor ForwardRight;
  32.     DcMotor ForwardLeft;
  33.     DcMotor BackLeft;
  34.     DcMotor BackRight;
  35.     DcMotor EncoderY;
  36.     DcMotor EncoderX;
  37.     DcMotor LeftLift;
  38.     DcMotor RightLift;
  39.     DcMotor CableSpool;
  40.     DcMotor Intake;
  41.     Servo Flap;
  42.     BNO055IMU imu;
  43.     private String Position = "NULL";
  44.     private VuforiaLocalizer vuforia;
  45.     private TFObjectDetector tfod;
  46.  
  47.     @Override
  48.     public void runOpMode() {
  49.  
  50.         //Init hardware
  51.         ForwardLeft = hardwareMap.get(DcMotor.class, "ForwardLeft");
  52.         ForwardLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  53.         ForwardLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  54.  
  55.         ForwardRight = hardwareMap.get(DcMotor.class, "ForwardRight");
  56.         ForwardRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  57.         ForwardRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  58.  
  59.         BackLeft = hardwareMap.get(DcMotor.class, "BackLeft");
  60.         BackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  61.         BackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  62.  
  63.         BackRight = hardwareMap.get(DcMotor.class, "BackRight");
  64.         BackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  65.         BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  66.  
  67.         EncoderY = hardwareMap.get(DcMotor.class, "EncoderY");
  68.         EncoderY.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  69.         EncoderY.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  70.  
  71.         EncoderX = hardwareMap.get(DcMotor.class, "EncoderX");
  72.         EncoderX.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  73.         EncoderX.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  74.  
  75.         LeftLift = hardwareMap.get(DcMotor.class, "EncoderY");
  76.         LeftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  77.         LeftLift.setDirection(DcMotor.Direction.REVERSE);
  78.         LeftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  79.  
  80.         RightLift = hardwareMap.get(DcMotor.class, "RightLift");
  81.         RightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  82.         RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  83.         RightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  84.  
  85.         CableSpool = hardwareMap.get(DcMotor.class, "CableSpool");
  86.         CableSpool.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  87.         BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  88.         CableSpool.setDirection(DcMotor.Direction.REVERSE);
  89.         CableSpool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  90.  
  91.         Intake = hardwareMap.get(DcMotor.class, "EncoderX");
  92.         Intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  93.         Intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  94.  
  95.         Flap = hardwareMap.get(Servo.class, "Flap");
  96.  
  97.         ForwardLeft.setDirection(DcMotor.Direction.FORWARD);
  98.         ForwardRight.setDirection(DcMotor.Direction.REVERSE);
  99.         BackLeft.setDirection(DcMotor.Direction.FORWARD);
  100.         BackRight.setDirection(DcMotor.Direction.REVERSE);
  101.  
  102.         ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  103.         ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  104.         BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  105.         BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  106.  
  107.         telemetry.addData("Status", "Hardware Initialized");
  108.         telemetry.update();
  109.  
  110.         //Init Vuforia
  111.         initVuforia();
  112.  
  113.         telemetry.addData("Status", "Vuforia Initialized");
  114.  
  115.         //Init TFOD
  116.         if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  117.             initTfod();
  118.         } else {
  119.             telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  120.             telemetry.update();
  121.         }
  122.         telemetry.addData("Status", "TFOD Initialized");
  123.         telemetry.update();
  124.  
  125.         //Activate TFOD
  126.         if (tfod != null) {
  127.             tfod.activate();
  128.         }
  129.  
  130.         telemetry.addData("Status", "TFOD Activated. Beginning Recognition");
  131.         telemetry.update();
  132.  
  133.         //Begin Recognition steps
  134.         while (isStopRequested() != true && !opModeIsActive()) {
  135.             if (tfod != null) {
  136.                 List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  137.                 if (updatedRecognitions != null) {
  138.                     telemetry.addData("# Object Detected", updatedRecognitions.size());
  139.                     if (updatedRecognitions.size() == 3) {
  140.                         int goldMineralX = -1;
  141.                         int silverMineral1X = -1;
  142.                         int silverMineral2X = -1;
  143.                         for (Recognition recognition : updatedRecognitions) {
  144.                             if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
  145.                                 goldMineralX = (int) recognition.getLeft();
  146.                             } else if (silverMineral1X == -1) {
  147.                                 silverMineral1X = (int) recognition.getLeft();
  148.                             } else {
  149.                                 silverMineral2X = (int) recognition.getLeft();
  150.                             }
  151.                         }
  152.                         if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
  153.                             if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
  154.                                 Position = "LEFT";
  155.                             } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
  156.                                 Position = "RIGHT";
  157.                             } else {
  158.                                 Position = "CENTER";
  159.                             }
  160.                         }
  161.                     }
  162.                 }
  163.             }
  164.             telemetry.addData("Position", Position);
  165.             telemetry.addData("Status", "VARaTiFf DLNN is running");
  166.             telemetry.update();
  167.         }
  168.  
  169.         telemetry.addData("Status", "No longer running VARaTiFf DLNN");
  170.         telemetry.update();
  171.  
  172.         /**
  173.          *
  174.          * Initialization stuff above this
  175.          *
  176.          */
  177.  
  178.         waitForStart();
  179.  
  180.         tfod.deactivate();
  181.  
  182.         telemetry.addData("Status", "All systems online, Running");
  183.  
  184.         ForwardLeft.setPower(.3);
  185.         ForwardRight.setPower(.3);
  186.         BackLeft.setPower(.3);
  187.         BackRight.setPower(.3);
  188.  
  189.         ForwardLeft.setTargetPosition(-1000);
  190.  
  191.         while(opModeIsActive()){
  192.             telemetry.addData("Currently Supposed To Run", "Forward Left");
  193.             telemetry.addData("ForwardLeft", ForwardLeft.getCurrentPosition());
  194.             telemetry.addData("ForwardRight", ForwardRight.getCurrentPosition());
  195.             telemetry.addData("BackLeft", BackLeft.getCurrentPosition());
  196.             telemetry.addData("BackRight", BackRight.getCurrentPosition());
  197.             telemetry.update();
  198.         }
  199.  
  200.     }
  201.  
  202.     private void initVuforia() {
  203.         /*
  204.          * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  205.          */
  206.         VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  207.  
  208.         parameters.vuforiaLicenseKey = VUFORIA_KEY;
  209.         parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
  210.  
  211.         //  Instantiate the Vuforia engine
  212.         vuforia = ClassFactory.getInstance().createVuforia(parameters);
  213.  
  214.         // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
  215.     }
  216.  
  217.     private void initTfod() {
  218.         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  219.                 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  220.         TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  221.         tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  222.         tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  223.     }
  224.  
  225.     void changeMode(int Mode) {
  226.         if (Mode == 1) {
  227.             ForwardLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  228.             BackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  229.             ForwardRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  230.             BackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  231.             ForwardLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  232.             BackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  233.             ForwardRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  234.             BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  235.         }
  236.         if (Mode == 2) {
  237.             ForwardLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  238.             BackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  239.             ForwardRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  240.             BackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  241.         }
  242.         if (Mode == 3) {
  243.             ForwardLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  244.             BackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  245.             ForwardRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  246.             BackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  247.         }
  248.         if (Mode == 4) {
  249.             ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  250.             ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  251.             BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  252.             BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  253.         }
  254.         if (Mode == 5) {
  255.             ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  256.             ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  257.             BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  258.             BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  259.         }
  260.     }
  261. }
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!