API tools faq
paste
Advertisement
Guest User

forgeorgica

a guest
Jan 20th, 2019
72
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 12.69 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2. package org.firstinspires.ftc.robotcontroller.external.samples;
  3.  
  4. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  5. import com.qualcomm.robotcore.hardware.ColorSensor;
  6. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  7. import com.qualcomm.robotcore.hardware.Gyroscope;
  8. import com.qualcomm.robotcore.hardware.Blinker;
  9. import com.qualcomm.robotcore.hardware.Servo;
  10. import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  11. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  12. import com.qualcomm.robotcore.hardware.DcMotor;
  13. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  14. import com.qualcomm.robotcore.util.ElapsedTime;
  15. import com.qualcomm.robotcore.hardware.I2cAddr;
  16. import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
  17. import java.util.List;
  18. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  19. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  20. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
  21. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  22. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  23.  
  24.  
  25.  
  26. @Autonomous
  27.  
  28. public class Autonomus extends LinearOpMode {
  29.     private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
  30.     private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
  31.     private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
  32.     private static final String VUFORIA_KEY = "AY+P56D/////AAABmSI9S7qmYEBRiexvSqpmyeJ7+EL9HqT1F38xiBmq5OyL+bgPAKCEY+Gq0DrzmKRaQ+C6j9Dq8OChZdw0oyIebsHi9f/UFLq8zTwrMmWqKUnfVlC5d7SWprkeXiWdg/0AX3eLMf15EYCkBlxIHOrjaSOzUotlkkUlJCt6btbNokBKQjrtug7s5W1FygD+4UQsYclavq/1dne9bGwE6LqJiFoAsFMiAiV3XJhYeS4V0MWNBT8E495/0tCwpowb45hyGo19lA/pDQdTfUwuHeDs1ojiu9UcRsNGMi/I0CkVz4MGzfFCS0gf+WmnWWt28Ua0BzgT6HqOZsC57W9qnVlFzs4Spdt0OdL5v9jt8uQYboqN";
  33.     private VuforiaLocalizer vuforia;
  34.     private TFObjectDetector tfod;
  35.     private ColorSensor senzor_culoare;
  36.     private Gyroscope imu_1;
  37.     private DcMotor mSS;
  38.     private DcMotor mSF;
  39.     private DcMotor mDS;
  40.     private DcMotor mDF;
  41.     private DcMotor brat;
  42.     private DcMotor carlig;
  43.     private DcMotor lant;
  44.     private Blinker expansion_Hub_1;
  45.     private Blinker expansion_Hub_2;
  46.     private Servo cutie;
  47.     private Servo l;
  48.     private Servo r;
  49.     private Servo art;
  50.  
  51.  
  52.  
  53.     @Override
  54.  
  55.     public void runOpMode() {
  56.         imu_1 = hardwareMap.get(Gyroscope.class, "imu 1");
  57.         mSS = hardwareMap.get(DcMotor.class, "MSS");
  58.         mSF = hardwareMap.get(DcMotor.class, "MSF");
  59.         mDS = hardwareMap.get(DcMotor.class, "MDS");
  60.         mDF = hardwareMap.get(DcMotor.class, "MDF");
  61.         brat = hardwareMap.get(DcMotor.class, "brat");
  62.         carlig = hardwareMap.get(DcMotor.class, "carlig");
  63.         lant = hardwareMap.get(DcMotor.class, "lant");
  64.         expansion_Hub_1 = hardwareMap.get(Blinker.class, "Expansion Hub 1");
  65.         expansion_Hub_2 = hardwareMap.get(Blinker.class, "Expansion Hub 2");
  66.         cutie = hardwareMap.get(Servo.class, "cutie");
  67.         l = hardwareMap.get(Servo.class, "l");
  68.         r = hardwareMap.get(Servo.class, "r");
  69.         art = hardwareMap.get(Servo.class, "art");
  70.         senzor_culoare=hardwareMap.get(ColorSensor.class,"senzor_culoare");
  71.         initVuforia();
  72.         /**
  73.          * Initialize the Vuforia localization engine.
  74.          */
  75.         private void initVuforia() {
  76.             /*
  77.              * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
  78.              */
  79.             VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  80.             parameters.vuforiaLicenseKey = VUFORIA_KEY;
  81.             parameters.cameraDirection = CameraDirection.BACK;
  82.             vuforia = ClassFactory.getInstance().createVuforia(parameters);
  83.         }
  84.         if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  85.             initTfod();
  86.         }
  87.         else {
  88.             telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  89.         }
  90.         tfodParameters.minimumConfidence = 0.75;
  91.         /**
  92.          * Initialize the Tensor Flow Object Detection engine.
  93.          */
  94.         private void initTfod() {
  95.             int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  96.                     "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  97.             TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  98.             tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  99.             tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  100.         }
  101.         telemetry.addData("Status", "Initialized");
  102.         telemetry.update();
  103.         // Wait for the game to start (driver presses PLAY)
  104.         waitForStart();
  105.         int stare=1;
  106.         /*
  107.             1-Start;
  108.             2-coborare
  109.             3-misca la stang
  110.             4- Mergi la bile
  111.             5- Verifica bila din stanga
  112.             6- mergi la dreapta
  113.             7- rotire stanga
  114.             8- atins cub
  115.             9- rotire dreapta
  116.         */
  117.         double anterior=0;
  118.         double Start_Time=System.currentTimeMillis();
  119.         float timp_de_coborare=8000;
  120.         float timp_miscat_stanga=500;
  121.         float timp_miscat_fata=500;
  122.         float timp_miscat_dreapta=500;
  123.         float timp_rotire_stanga=200;
  124.         float timp_atins_cub=1000;
  125.         float timp_rotire_dreapta=200;
  126.         double viteza=0.5;
  127.         double vit_rot=0.25;
  128.         int activat;
  129.         if (tfod != null) {
  130.             tfod.activate();
  131.         }
  132.         switch(stare){
  133.             case 1:stare=2;break;
  134.             case 2:{
  135.                 //Coborarea in sine
  136.                 if(System.currentTimeMillis()-Start_Time<anterior+timp_de_coborare){
  137.                     carlig.setPower(-1);
  138.                     telemetry.addData("Status",Start_Time);
  139.                 }
  140.                 else{
  141.                     anterior+=timp_de_coborare;
  142.                     carlig.setPower(0);
  143.                     stare=3;
  144.                 }
  145.             }break;
  146.             case 3:{
  147.                 if(System.currentTimeMillis()-Start_Time<anterior+timp_miscat_stanga){
  148.                     //iesi din carlig
  149.                     mDF.setPower(-viteza);
  150.                     mDS.setPower(viteza);
  151.                     mSF.setPower(-viteza);
  152.                     mSS.setPower(viteza);
  153.                 }
  154.                 else{
  155.                     anterior+=timp_miscat_stanga;
  156.                     //stop
  157.                     mDF.setPower(0);
  158.                     mDS.setPower(0);
  159.                     mSF.setPower(0);
  160.                     mSS.setPower(0);
  161.                     stare=4;
  162.                 }
  163.             }break;
  164.             case 4:{
  165.                 //mergi in fata
  166.                 if(System.currentTimeMillis()-Start_Time<anterior+timp_miscat_fata){
  167.                     mDF.setPower(viteza);
  168.                     mDS.setPower(viteza);
  169.                     mSF.setPower(-viteza);
  170.                     mSS.setPower(-viteza);
  171.                 }
  172.                 else{
  173.                     anterior+=timp_miscat_fata;
  174.                     //stop
  175.                     mDF.setPower(0);
  176.                     mDS.setPower(0);
  177.                     mSF.setPower(0);
  178.                     mSS.setPower(0);
  179.                     stare=5;
  180.                 }
  181.             }break;
  182.             case 5:{
  183.                 if(System.currentTimeMillis()-Start_Time<anterior+timp_miscat_dreapta){
  184.                     //du te n centru
  185.                     mDF.setPower(viteza);
  186.                     mDS.setPower(-viteza);
  187.                     mSF.setPower(viteza);
  188.                     mSS.setPower(-viteza);
  189.                 }
  190.                 else{
  191.                     anterior+=timp_miscat_dreapta;
  192.                     //stop
  193.                     mDF.setPower(0);
  194.                     mDS.setPower(0);
  195.                     mSF.setPower(0);
  196.                     mSS.setPower(0);
  197.                     stare=6;
  198.                 }
  199.             }break;
  200.             case 6:{
  201.                 //verifica pozitia cubului
  202.                 activat=1;
  203.                 while (activat!=0) {
  204.                     if (tfod != null) {
  205.                         // getUpdatedRecognitions() will return null if no new information is available since
  206.                         // the last time that call was made.
  207.                         List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  208.                         if (updatedRecognitions != null) {
  209.                             telemetry.addData("# Object Detected", updatedRecognitions.size());
  210.                             if (updatedRecognitions.size() == 3) {
  211.                                 int goldMineralX = -1;
  212.                                 int silverMineral1X = -1;
  213.                                 int silverMineral2X = -1;
  214.                                 for (Recognition recognition : updatedRecognitions) {
  215.                                     if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
  216.                                         goldMineralX = (int) recognition.getLeft();
  217.                                     } else if (silverMineral1X == -1) {
  218.                                         silverMineral1X = (int) recognition.getLeft();
  219.                                     } else {
  220.                                         silverMineral2X = (int) recognition.getLeft();
  221.                                     }
  222.                                 }
  223.                                 if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
  224.                                     activat == 0;
  225.                                     if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
  226.                                         telemetry.addData("Gold Mineral Position", "Left");
  227.                                         stare = 7;
  228.                                     } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
  229.                                         telemetry.addData("Gold Mineral Position", "Right");
  230.                                         stare = 9;
  231.                                     } else {
  232.                                         telemetry.addData("Gold Mineral Position", "Center");
  233.                                         stare = 8;
  234.                                     }
  235.                                 }
  236.                             }
  237.                             telemetry.update();
  238.                         }
  239.                     }
  240.                 }
  241.             }break;
  242.             case 7:{
  243.                 if(System.currentTimeMillis()-Start_Time<anterior+timp_rotire_stanga){
  244.                     //roteste te spre stanga
  245.                     mDF.setPower(-vit_rot);
  246.                     mDS.setPower(-vit_rot);
  247.                     mSF.setPower(-vit_rot);
  248.                     mSS.setPower(-vit_rot);
  249.                 }
  250.                 else{
  251.                     anterior+=timp_rotire_stanga;
  252.                     //stop
  253.                     mDF.setPower(0);
  254.                     mDS.setPower(0);
  255.                     mSF.setPower(0);
  256.                     mSS.setPower(0);
  257.                     stare=8;
  258.                 }
  259.             }break;
  260.             case 8:{
  261.                 if(System.currentTimeMillis()-Start_Time<anterior+timp_atins_cub){
  262.                     //atinge cubul
  263.                     mDF.setPower(viteza);
  264.                     mDS.setPower(viteza);
  265.                     mSF.setPower(-viteza);
  266.                     mSS.setPower(-viteza);
  267.                 }
  268.                 else{
  269.                     anterior+=timp_atins_cubul;
  270.                     //stop
  271.                     mDF.setPower(0);
  272.                     mDS.setPower(0);
  273.                     mSF.setPower(0);
  274.                     mSS.setPower(0);
  275.                 }
  276.             }break;
  277.             case 9:{
  278.                 if(System.currentTimeMillis()-Start_Time<anterior+timp_rotire_dreapta){
  279.                     //roteste te spre dreapta
  280.                     mDF.setPower(vit_rot);
  281.                     mDS.setPower(vit_rot);
  282.                     mSF.setPower(vit_rot);
  283.                     mSS.setPower(vit_rot);
  284.                 }
  285.                 else{
  286.                     anterior+=timp_rotire_dreapta;
  287.                     //stop
  288.                     mDF.setPower(0);
  289.                     mDS.setPower(0);
  290.                     mSF.setPower(0);
  291.                     mSS.setPower(0);
  292.                     stare=8;
  293.                 }
  294.             }break;
  295.         }
  296.         telemetry.update();
  297.     }
  298.     if (tfod != null){
  299.         tfod.shutdown();
  300.     }
  301. }
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!