daily pastebin goal
89%
SHARE
TWEET

Untitled

a guest Feb 17th, 2019 222 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode;
  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.ColorSensor;
  8. import com.qualcomm.robotcore.hardware.DcMotor;
  9. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  10. import com.qualcomm.robotcore.hardware.DistanceSensor;
  11. import com.qualcomm.robotcore.hardware.Servo;
  12. import com.qualcomm.robotcore.util.ElapsedTime;
  13.  
  14. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  15. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  16. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  17. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  18. import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
  19. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  20. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  21. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  22. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  23.  
  24. import java.util.List;
  25.  
  26. @Autonomous(name="Autonoma_Bila_Crater", group="Autonoma_Final")
  27.  
  28. public class Autonoma_Bila_Crater extends LinearOpMode
  29. {
  30.     //Timp
  31.     private ElapsedTime runtime = new ElapsedTime();
  32.  
  33.     //Motoare
  34.  
  35.     DcMotor motorDF;
  36.     DcMotor motorDS;
  37.     DcMotor motorSF;
  38.     DcMotor motorSS;
  39.  
  40.     DcMotor motorTija;
  41.     DcMotor motorExten_fata;
  42.     DcMotor motorExten_spate;
  43.     DcMotor motorSwitch;
  44.  
  45.     //Senzori
  46.  
  47.     BNO055IMU imu;
  48.     DistanceSensor senzorTija;
  49.     DistanceSensor senzorFata;
  50.     ColorSensor senzorCuloare;
  51.  
  52.     //Servo
  53.  
  54.     Servo servo_colect;
  55.     Servo servo_lander;
  56.  
  57.     //Variabile pentru detectare
  58.  
  59.     private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
  60.     private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
  61.     private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
  62.     private static final String VUFORIA_KEY = "Ad94F5T/////AAABmXHK2XiLFk+MoiPfQGybpl8fbSSDPMTEpMn4lwDMSxox2HG3QIgFb3MFGa/wO+tOLca7Tw/dvKNqD2IaQMLInqAnRPZPozTL/NE7fvbpOviIVdjNcRP3bsFzS4umkgbgtTdMahlIRW+KPSbFWnSZZ1diFpfqhIPLdMiUGBDNCU/Cizx7E1y8RlwQYH7n5nPfYskKSomRW4ZpFxTRkrDgxNbY6g+6xF/QAPEXj72MWYqjDfHi6UMaP8pfXPYqwotNlf3BDZbHiHLEBLY93jsr4Y/aXUo2bt6qWZEk/kvxiCoFTRAJFXPJwbFqWvwO+ajBbabdbTCrdd/IwtXu4CjxuHJB5veBRBIIwcQlU7+PocgD";
  63.     private VuforiaLocalizer vuforia;
  64.     private TFObjectDetector tfod;
  65.  
  66.     @Override
  67.     public void runOpMode()
  68.     {
  69.         initMotoare();
  70.         initSenzori();
  71.  
  72.         detect();
  73.  
  74.         waitForStart();
  75.         if (opModeIsActive())
  76.         {
  77.             coborare(17200, 8.6, 8);
  78.             Movement_Lateral_Timp(0.6, 0.7);
  79.             Movement_Fata_Culoare(0.4, 1.5);
  80.  
  81.             angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  82.             unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  83.  
  84.             sleep(100);
  85.             Punctare();
  86.  
  87.             sleep(100);
  88.             gyroTurn_left(-70 ,-0.3, -0.8);
  89.  
  90.             sleep(100);
  91.             Miscare_Timp(0.8, 1);
  92.  
  93.             sleep(100);
  94.             gyroTurn_left(-35, -0.8, -0.8 );
  95.  
  96.             sleep(100);
  97.             Movement_Lateral_Timp(-0.8, 2);
  98.  
  99.             sleep(100);
  100.             Miscare_Distanta(0.8, 15, 2);
  101.  
  102.             sleep(100);
  103.             gyroTurn_left(-170, -0.8, -0.8);
  104.  
  105.             sleep(100);
  106.             Miscare_Distanta(0.8, 30, 4);
  107.         }
  108.     }
  109.    
  110.     public void ExtindereBrat_Team_Marker()
  111.     {
  112.        
  113.     }
  114.    
  115.     public void ExtindereBrat_Sampling()
  116.     {
  117.        
  118.     }
  119.    
  120.     int a;
  121.    
  122.     int slide_fata = 0;
  123.    
  124.     public void Miscare_Slide(double viteza, int tickuri, double timeout)
  125.     {
  126.         /*
  127.             Daca e inchis la apelare se va deschide si reciproc
  128.         */
  129.         runtime.reset();
  130.        
  131.         if(slide_fata == 0)
  132.         {
  133.             motor.Exten_fata.setPower(viteza);
  134.             while(motor.Exten_fata.getCurrentPosition() <= tickuri && runtime.seconds <= timeout);
  135.             motor.Exten_fata.setPower(0);
  136.             slide_fata = 1;
  137.         {
  138.         else
  139.         {
  140.             motor.Exten_fata.setPower(- viteza);
  141.             while(motor.Exten_fata.getCurrentPosition() >= 10 && runtime.seconds <= timeout);
  142.             motor.Exten_fata.setPower(0);
  143.             slide_fata = 0;
  144.         }
  145.     }
  146.    
  147.     int sistem_colectare = -1;
  148.    
  149.     public void Coborare_Sistem_Colectare(double viteza, int tickuri, double timeout)
  150.     {
  151.         runtime.reset();
  152.         motorSwitch.setPower(viteza);
  153.         while(motor.Swtich.getCurrentPosition() <= tickuri && runtime.seconds <=timeout);
  154.         motorSwitch.setPower(0);
  155.         sistem_colectare = 1;
  156.     }
  157.    
  158.     public void Miscare_Sistem colectare(double viteza, int tickuri_coborare, double timeout)
  159.     {
  160.         runtime.reset();
  161.        
  162.         if(sistem_colectare == 0)
  163.         {
  164.             motorSwitch.setPower(viteza);
  165.             while(motorSwitch.getCurrentPosition() <= tickuri && runtime.seconds <=timeout);
  166.             motorSwitch.setPower(0);
  167.             sistem_colectare = 1;
  168.         }
  169.         else if(sistem_colectare == 1)
  170.         {
  171.             motorSwitch.setPower(- viteza)
  172.             while(motorSwitch.getCurrentPosition() >= 0 && runtime.seconds <= timeout);
  173.             motorSwitch.setPower(0);
  174.             sistem_colectare = 0;
  175.         }
  176.     }
  177.    
  178.  
  179.     double distantaIntitiala;
  180.  
  181.     public void Miscare_Timp(double viteza, double timeout)
  182.     {
  183.         runtime.reset();
  184.  
  185.         motorDS.setPower(viteza);
  186.         motorDF.setPower(viteza);
  187.         motorSS.setPower(viteza);
  188.         motorSF.setPower(viteza);
  189.  
  190.         while (runtime.seconds() <= timeout);
  191.  
  192.         motorDS.setPower(0);
  193.         motorDF.setPower(0);
  194.         motorSS.setPower(0);
  195.         motorSF.setPower(0);
  196.     }
  197.  
  198.     public void Miscare_Distanta(double distanta, double viteza, double timeout)
  199.     {
  200.         runtime.reset();
  201.  
  202.         distantaIntitiala = senzorFata.getDistance(DistanceUnit.CM);
  203.  
  204.         motorDS.setPower(viteza);
  205.         motorDF.setPower(viteza);
  206.         motorSS.setPower(viteza);
  207.         motorSF.setPower(viteza);
  208.  
  209.         while (senzorFata.getDistance(DistanceUnit.CM) >= distantaIntitiala - distanta && runtime.seconds() <= timeout )
  210.         {
  211.             telemetry.addData("DISTANTA", senzorFata.getDistance(DistanceUnit.CM));
  212.             telemetry.addData("MAXIM", distantaIntitiala - distanta);
  213.             telemetry.update();
  214.         }
  215.  
  216.         motorDS.setPower(0);
  217.         motorDF.setPower(0);
  218.         motorSS.setPower(0);
  219.         motorSF.setPower(0);
  220.     }
  221.  
  222.     Orientation angles;
  223.     double unghiCurent, unghiFinal;
  224.  
  225.     public void gyroTurn_right(double angle, double viteza_left, double viteza_right) // unghi si viteza pozitive
  226.     {
  227.         runtime.reset();
  228.         angle = -angle;
  229.         if (unghiCurent + angle < -180)
  230.         {
  231.             unghiFinal = 360 + (unghiCurent + angle);
  232.  
  233.             motorDF.setPower(-viteza_right);
  234.             motorDS.setPower(-viteza_right);
  235.             motorSF.setPower(viteza_left);
  236.             motorSS.setPower(viteza_left);
  237.  
  238.             angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  239.             unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  240.             while((unghiCurent < 0 || unghiCurent > unghiFinal) && runtime.seconds() < 5)
  241.             {
  242.                 angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  243.                 unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  244.                 telemetry.addData("Unghi_final", unghiFinal);
  245.                 telemetry.addData("Curent", unghiCurent);
  246.                 telemetry.update();
  247.             }
  248.  
  249.             motorDF.setPower(0);
  250.             motorDS.setPower(0);
  251.             motorSF.setPower(0);
  252.             motorSS.setPower(0);
  253.             telemetry.update();
  254.         }
  255.         else
  256.         {
  257.             unghiFinal = unghiCurent + angle;
  258.  
  259.             motorDF.setPower(-viteza_right);
  260.             motorDS.setPower(-viteza_right);
  261.             motorSF.setPower(viteza_left);
  262.             motorSS.setPower(viteza_left);
  263.  
  264.             angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  265.             unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  266.  
  267.             while(unghiFinal < unghiCurent && runtime.seconds() < 5)
  268.             {
  269.                 angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  270.                 unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  271.                 telemetry.addData("Unghi_final", unghiFinal);
  272.                 telemetry.addData("Curent", unghiCurent);
  273.                 telemetry.update();
  274.             }
  275.  
  276.             motorDF.setPower(0);
  277.             motorDS.setPower(0);
  278.             motorSF.setPower(0);
  279.             motorSS.setPower(0);
  280.             telemetry.update();
  281.         }
  282.         unghiCurent = unghiFinal;
  283.     }
  284.  
  285.     public void gyroTurn_left(double angle, double viteza_left, double viteza_right)
  286.     {
  287.         runtime.reset();
  288.  
  289.         angle = -angle;
  290.         if (unghiCurent + angle > 180)
  291.         {
  292.             unghiFinal = -360 + (unghiCurent + angle);
  293.  
  294.             motorDF.setPower(-viteza_right);
  295.             motorDS.setPower(-viteza_right);
  296.             motorSF.setPower(viteza_left);
  297.             motorSS.setPower(viteza_left);
  298.  
  299.             angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  300.             unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  301.             while((unghiCurent > 0 || unghiCurent < unghiFinal) && runtime.seconds() < 5)
  302.             {
  303.                 angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  304.                 unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  305.                 telemetry.addData("Unghi_final", unghiFinal);
  306.                 telemetry.addData("Curent", unghiCurent);
  307.                 telemetry.update();
  308.             }
  309.  
  310.             motorDF.setPower(0);
  311.             motorDS.setPower(0);
  312.             motorSF.setPower(0);
  313.             motorSS.setPower(0);
  314.             telemetry.update();
  315.         }
  316.         else
  317.         {
  318.             unghiFinal = unghiCurent + angle;
  319.  
  320.             motorDF.setPower(-viteza_right);
  321.             motorDS.setPower(-viteza_right);
  322.             motorSF.setPower(viteza_left);
  323.             motorSS.setPower(viteza_left);
  324.  
  325.             angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  326.             unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  327.  
  328.             while(unghiFinal > unghiCurent && runtime.seconds() < 5)
  329.             {
  330.                 angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  331.                 unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
  332.                 telemetry.addData("Unghi_final", unghiFinal);
  333.                 telemetry.addData("Curent", unghiCurent);
  334.                 telemetry.update();
  335.             }
  336.  
  337.             motorDF.setPower(0);
  338.             motorDS.setPower(0);
  339.             motorSF.setPower(0);
  340.             motorSS.setPower(0);
  341.             telemetry.update();
  342.         }
  343.         unghiCurent = unghiFinal;
  344.     }
  345.  
  346.     double formatAngle(AngleUnit angleUnit, double angle) {return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));}
  347.     double formatDegrees(double degrees){return AngleUnit.DEGREES.normalize(degrees); }
  348.  
  349.     int pozitieCurenta;
  350.     double distantaCurenta;
  351.  
  352.     public void Movement_Lateral_Timp(double viteza, double timeout)
  353.     {
  354.         runtime.reset();
  355.  
  356.         motorSF.setPower(-viteza);
  357.         motorDF.setPower( viteza);
  358.         motorDS.setPower(-viteza);
  359.         motorSS.setPower( viteza);
  360.  
  361.         while (runtime.seconds() <= timeout);
  362.  
  363.         motorSF.setPower(0);
  364.         motorDF.setPower(0);
  365.         motorDS.setPower(0);
  366.         motorSS.setPower(0);
  367.     }
  368.  
  369.     int rosu, verde, albastru;
  370.  
  371.     public void Movement_Fata_Culoare(double viteza, double timeout)
  372.     {
  373.         runtime.reset();
  374.  
  375.         telemetry.addData("Red  ", senzorCuloare.red());
  376.         telemetry.addData("Green", senzorCuloare.green());
  377.         telemetry.addData("Blue ", senzorCuloare.blue());
  378.  
  379.         rosu = senzorCuloare.red();
  380.         verde = senzorCuloare.green();
  381.         albastru = senzorCuloare.blue();
  382.  
  383.         while (senzorCuloare.red() <= rosu + 10 && senzorCuloare.blue() <= albastru + 10 && runtime.seconds() <= timeout)
  384.         {
  385.             motorSF.setPower( viteza);
  386.             motorDF.setPower( viteza);
  387.             motorDS.setPower( viteza);
  388.             motorSS.setPower( viteza);
  389.  
  390.             telemetry.addData("Red  ", senzorCuloare.red());
  391.             telemetry.addData("Green", senzorCuloare.green());
  392.             telemetry.addData("Blue ", senzorCuloare.blue());
  393.             telemetry.update();
  394.         }
  395.  
  396.         motorSF.setPower(0);
  397.         motorDF.setPower(0);
  398.         motorDS.setPower(0);
  399.         motorSS.setPower(0);
  400.  
  401.     }
  402.  
  403.  
  404.     public void coborare(int tickuri_encoder,double distanta, double timeout )
  405.     {
  406.         runtime.reset();
  407.  
  408.         pozitieCurenta = motorTija.getCurrentPosition();
  409.         distantaCurenta = senzorTija.getDistance(DistanceUnit.CM);
  410.         telemetry.addData("Distanta", distantaCurenta);
  411.         telemetry.addData("Encoder", pozitieCurenta);
  412.  
  413.         motorTija.setPower(1);
  414.  
  415.         while (runtime.seconds() <= timeout && (pozitieCurenta <= tickuri_encoder || distantaCurenta >= distanta + 0.1))
  416.         {
  417.             pozitieCurenta = motorTija.getCurrentPosition();
  418.             distantaCurenta = senzorTija.getDistance(DistanceUnit.CM);
  419.             telemetry.addData("Distanta", distantaCurenta);
  420.             telemetry.addData("Encoder", pozitieCurenta);
  421.             telemetry.update();
  422.         }
  423.  
  424.         motorTija.setPower(0);
  425.     }
  426.  
  427.     public void Punctare()
  428.     {
  429.         if (poz == 2)
  430.         {
  431.             gyroTurn_right(10, 0.4, 0.4);
  432.             //
  433.             sleep(100);
  434.             gyroTurn_left(-10, -0.4, -0.4);
  435.         }
  436.         else if (poz == 1)
  437.         {
  438.             gyroTurn_right(40, 0.4, 0.4);
  439.             //
  440.             sleep(100);
  441.             gyroTurn_left(-40, -0.4, -0.4);
  442.         }
  443.         else
  444.         {
  445.             gyroTurn_left(-25, -0.4, -0.4);
  446.             //
  447.             sleep(100);
  448.             gyroTurn_right(10, 0.4, 0.4);
  449.         }
  450.  
  451.     }
  452.  
  453.     int poz = 0;
  454.  
  455.     public void detect ()
  456.     {
  457.  
  458.         initVuforia();
  459.  
  460.         if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
  461.             initTfod();
  462.         } else {
  463.             telemetry.addData("Sorry!", "This device is not compatible with TFOD");
  464.         }
  465.  
  466.         if (tfod != null) {
  467.             tfod.activate();
  468.         }
  469.  
  470.         while(!opModeIsActive())
  471.         {
  472.             if (tfod != null)
  473.             {
  474.                 List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
  475.                 if (updatedRecognitions != null)
  476.                 {
  477.                     telemetry.addData("# Object Detected", updatedRecognitions.size());
  478.                     if (updatedRecognitions.size() >= 1) {
  479.                         int goldMineralX = -1;
  480.                         for (Recognition recognition : updatedRecognitions)
  481.                         {
  482.                             if (recognition.getLabel().equals(LABEL_GOLD_MINERAL))
  483.                             {
  484.                                 goldMineralX = (int) recognition.getTop();
  485.                             }
  486.                         }
  487.                         telemetry.addData("GOLD", goldMineralX);
  488.                         if(goldMineralX != -1)
  489.                         {
  490.                             if (goldMineralX <= 300)
  491.                             {
  492.                                 poz = 1;
  493.                             } else if (goldMineralX <= 500)
  494.                             {
  495.                                 poz = 2;
  496.                             } else
  497.                             {
  498.                                 poz = 3;
  499.                             }
  500.                         }
  501.                     }
  502.                     telemetry.update();
  503.                 }
  504.             }
  505.         }
  506.         tfod.shutdown();
  507.     }
  508.  
  509.     private void initVuforia() {
  510.  
  511.         VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  512.  
  513.         parameters.vuforiaLicenseKey = VUFORIA_KEY;
  514.         parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
  515.  
  516.         vuforia = ClassFactory.getInstance().createVuforia(parameters);
  517.     }
  518.  
  519.     private void initTfod() {
  520.         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  521.                 "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  522.         TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  523.         tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  524.         tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
  525.     }
  526.  
  527.     public void initMotoare()
  528.     {
  529.         motorTija = hardwareMap.get(DcMotor.class, "motorTija");
  530.         motorTija.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  531.         motorTija.setDirection(DcMotorSimple.Direction.REVERSE);
  532.         motorTija.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  533.  
  534.         motorExten_fata = hardwareMap.get(DcMotor.class, "motorExten_fata");
  535.         motorExten_fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  536.         motorExten_fata.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  537.  
  538.         motorExten_spate = hardwareMap.get(DcMotor.class, "motorExte_spate");
  539.         motorExten_spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  540.         motorExten_spate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  541.  
  542.         motorSwitch = hardwareMap.get(DcMotor.class, "motorSwitch");
  543.         motorSwitch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  544.         motorSwitch.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  545.  
  546.         motorDS = hardwareMap.get(DcMotor.class, "motorDS");
  547.         motorDS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  548.  
  549.         motorSS = hardwareMap.get(DcMotor.class, "motorSS");
  550.         motorSS.setDirection(DcMotor.Direction.REVERSE);
  551.         motorSS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  552.  
  553.         motorDF = hardwareMap.get(DcMotor.class, "motorDF");
  554.         motorSS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  555.  
  556.         motorSF = hardwareMap.get(DcMotor.class, "motorSF");
  557.         motorSF.setDirection(DcMotor.Direction.REVERSE);
  558.         motorSF.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  559.  
  560.         servo_colect = hardwareMap.get(Servo.class, "servo_colect");
  561.         servo_lander = hardwareMap.get(Servo.class, "servo_lander");
  562.     }
  563.  
  564.     public void initSenzori()
  565.     {
  566.         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  567.         parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
  568.         parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  569.         parameters.calibrationDataFile = "BNO055IMUCalibration.json";
  570.         parameters.loggingEnabled      = true;
  571.         parameters.loggingTag          = "IMU";
  572.         parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  573.  
  574.         imu = hardwareMap.get(BNO055IMU.class, "imu");
  575.         imu.initialize(parameters);
  576.  
  577.         senzorTija = hardwareMap.get(DistanceSensor.class, "senzorTija");
  578.         senzorFata = hardwareMap.get(DistanceSensor.class, "senzorFata");
  579.         senzorCuloare = hardwareMap.get(ColorSensor.class, "senzorCuloare");
  580.     }
  581. }
RAW Paste Data
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
 
Top