API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Feb 24th, 2018
180
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 11.74 KB | None | 0 0
raw download clone embed print report
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
  4. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  5. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  6. import com.qualcomm.robotcore.hardware.DcMotor;
  7. import com.qualcomm.robotcore.hardware.HardwareMap;
  8. import com.qualcomm.robotcore.util.Range;
  9.  
  10. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  11. import org.firstinspires.ftc.robotcore.external.Telemetry;
  12. import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
  13. import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
  14. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  15. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
  16. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
  17. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
  18.  
  19.  
  20.  
  21. @Autonomous (name = "pls workey")
  22. public class net extends LinearOpMode {
  23.  
  24.     public static class Initialization {
  25.  
  26.         public DcMotor Roata_Stanga_Fata = null;
  27.         public DcMotor Roata_Stanga_Spate = null;
  28.         public DcMotor Roata_Dreapta_Fata = null;
  29.         public DcMotor Roata_Dreapta_Spate = null;
  30.         HardwareMap hwMap = null;
  31.  
  32.  
  33.         public Initialization() {
  34.  
  35.         }
  36.  
  37.  
  38.         /* Initialize standard Hardware interfaces */
  39.         public void init(HardwareMap ahwMap) {
  40.             // Save reference to Hardware map
  41.             hwMap = ahwMap;
  42.  
  43.             // Define and Initialize Motors
  44.             Roata_Stanga_Fata = hwMap.get(DcMotor.class, "sf");
  45.             Roata_Stanga_Spate = hwMap.get(DcMotor.class, "ss");
  46.             Roata_Dreapta_Fata = hwMap.get(DcMotor.class, "df");
  47.             Roata_Dreapta_Spate = hwMap.get(DcMotor.class, "ds");
  48.  
  49.             Roata_Stanga_Fata.setDirection(DcMotor.Direction.FORWARD);
  50.             Roata_Stanga_Spate.setDirection(DcMotor.Direction.FORWARD);
  51.             Roata_Dreapta_Fata.setDirection(DcMotor.Direction.REVERSE);
  52.             Roata_Dreapta_Spate.setDirection(DcMotor.Direction.REVERSE);
  53.  
  54.             // Set all motors to zero power
  55.             Roata_Stanga_Fata.setPower(0);
  56.             Roata_Stanga_Spate.setPower(0);
  57.             Roata_Dreapta_Fata.setPower(0);
  58.             Roata_Dreapta_Spate.setPower(0);
  59.  
  60.         }
  61.  
  62.  
  63.     }
  64.  
  65.     Initialization robot = new Initialization();
  66.     ModernRoboticsI2cGyro gyro = null;
  67.  
  68.     static final double COUNTS_PER_MOTOR_REV = 1440;
  69.     static final double WHEEL_DIAMETER_INCHES = 4.0;
  70.     static final double COUNTS_PER_INCH = COUNTS_PER_MOTOR_REV /
  71.             (WHEEL_DIAMETER_INCHES * 3.1415);
  72.  
  73.  
  74.  
  75.     @Override
  76.     public void runOpMode() {
  77.  
  78.        /* int Cod;
  79.         Cod = ceva();*/
  80.  
  81.         robot.init(hardwareMap);
  82.         gyro = (ModernRoboticsI2cGyro)hardwareMap.gyroSensor.get("gyro");
  83.  
  84.         telemetry.addData(">", "Giroscopul se calibreaza");
  85.         telemetry.update();
  86.  
  87.         gyro.calibrate();
  88.         gyro.resetZAxisIntegrator();
  89.  
  90.         while(!isStopRequested() && gyro.isCalibrating())  {
  91.             sleep(50);
  92.             idle();
  93.         }
  94.  
  95.         telemetry.addData(">", "Robot Pregatit.");
  96.         telemetry.update();
  97.  
  98.         waitForStart();
  99.         robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  100.         robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  101.         robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  102.         robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  103.  
  104.         robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  105.         robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  106.         robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  107.         robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  108.  
  109.         while(!isStarted())
  110.         {
  111.             telemetry.addData(">", "Orientarea robotului = %d grade", gyro.getHeading());
  112.             telemetry.update();
  113.         }
  114.  
  115.         gyro.calibrate();
  116.         //Drive(0.175, 10.0);
  117.         //sleep(5000);
  118.         RotireDreapta(0.1, 180);
  119.         stop();
  120.  
  121.  
  122.     }
  123.  
  124.  
  125.     public void Drive(double speed,
  126.                       double distance)
  127.     {
  128.  
  129.         int stanga;
  130.         int dreapta;
  131.         int moveCounts;
  132.  
  133.         if(opModeIsActive()) {
  134.             moveCounts = (int) (distance * COUNTS_PER_INCH);
  135.             double directie;
  136.             directie = gyro.getHeading();
  137.             telemetry.addData("moveCounts= ", moveCounts);
  138.             telemetry.update();
  139.             stanga = robot.Roata_Stanga_Spate.getCurrentPosition() + moveCounts;
  140.             dreapta = robot.Roata_Dreapta_Spate.getCurrentPosition() + moveCounts;
  141.  
  142.             /*micsorare viteza cand suntem aproape
  143.               if (Math.abs(target - rightFlipMotor.getTargetPosition()) < 100)
  144.                  speed = 0.1;
  145.             else
  146.                 speed = 0.8;
  147.              */
  148.             /*robot.Roata_Stanga_Fata.setTargetPosition(stanga);
  149.             robot.Roata_Stanga_Spate.setTargetPosition(stanga);
  150.             robot.Roata_Dreapta_Fata.setTargetPosition(dreapta);
  151.             robot.Roata_Dreapta_Spate.setTargetPosition(dreapta);
  152.  
  153.             robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  154.             robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  155.             robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  156.             robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_TO_POSITION);*/
  157.  
  158.             while (robot.Roata_Dreapta_Fata.getCurrentPosition() < dreapta && robot.Roata_Dreapta_Spate.getCurrentPosition() < dreapta
  159.                     && robot.Roata_Stanga_Fata.getCurrentPosition() < stanga && robot.Roata_Dreapta_Spate.getCurrentPosition() < stanga && opModeIsActive()) {
  160.                 robot.Roata_Stanga_Fata.setPower(-speed);
  161.                 robot.Roata_Stanga_Spate.setPower(-speed);
  162.                 robot.Roata_Dreapta_Spate.setPower(-speed);
  163.                 robot.Roata_Dreapta_Fata.setPower(-speed);
  164.                 Corectare(directie);
  165.  
  166.             }
  167.  
  168.            /* while(robot.Roata_Dreapta_Fata.isBusy() && robot.Roata_Dreapta_Spate.isBusy() &&
  169.                     robot.Roata_Stanga_Spate.isBusy() && robot.Roata_Stanga_Fata.isBusy() && opModeIsActive()) {
  170.             */
  171.  
  172.             Stop();
  173.  
  174.             robot.Roata_Stanga_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  175.             robot.Roata_Stanga_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  176.             robot.Roata_Dreapta_Fata.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  177.             robot.Roata_Dreapta_Spate.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  178.  
  179.         }
  180.  
  181.     }
  182.  
  183.     public void Stop()
  184.     {
  185.  
  186.         robot.Roata_Stanga_Fata.setPower(0);
  187.         robot.Roata_Stanga_Spate.setPower(0);
  188.         robot.Roata_Dreapta_Fata.setPower(0);
  189.         robot.Roata_Dreapta_Spate.setPower(0);
  190.     }
  191.  
  192.     public void Stanga(double speed)
  193.     {
  194.         if (opModeIsActive())
  195.         {
  196.             robot.Roata_Stanga_Fata.setPower(-speed);
  197.             robot.Roata_Stanga_Spate.setPower(-speed);
  198.             robot.Roata_Dreapta_Spate.setPower(speed);
  199.             robot.Roata_Dreapta_Fata.setPower(speed);
  200.         }
  201.     }
  202.  
  203.     public void Dreapta(double speed)
  204.     {
  205.         if(opModeIsActive()) {
  206.             robot.Roata_Stanga_Fata.setPower(speed);
  207.             robot.Roata_Stanga_Spate.setPower(speed);
  208.             robot.Roata_Dreapta_Spate.setPower(-speed);
  209.             robot.Roata_Dreapta_Fata.setPower(-speed);
  210.         }
  211.     }
  212.  
  213.     public void Corectare(double directie) {
  214.  
  215.         if (opModeIsActive() && gyro.getHeading()!=directie)
  216.         {
  217.             if(gyro.getHeading() < directie)
  218.             {
  219.                 Dreapta(0.200);
  220.             }
  221.             if(gyro.getHeading() > directie)
  222.             {
  223.                 Stanga(0.200);
  224.             }
  225.         }
  226.  
  227.     }
  228.  
  229.  
  230.     public void RotireDreapta(double speed, double angle)
  231.     {
  232.         int goalReached = 0;
  233.         double goalAngle;
  234.         goalAngle=gyro.getHeading()+angle;
  235.         if(goalAngle>359)
  236.             goalAngle=goalAngle%360;
  237.         int aux=0;
  238.         while (goalReached==0 && opModeIsActive()) {
  239.             if(gyro.getHeading()!=goalAngle && gyro.getHeading()+1!=goalAngle && gyro.getHeading()-1!=goalAngle) {
  240.                 if(aux==0) {
  241.                     robot.Roata_Stanga_Fata.setPower(speed);
  242.                     robot.Roata_Stanga_Spate.setPower(speed);
  243.                     robot.Roata_Dreapta_Spate.setPower(-speed);
  244.                     robot.Roata_Dreapta_Fata.setPower(-speed);
  245.                     aux=1;
  246.                 }
  247.                 telemetry.addData(">", "Orientarea robotului = %d grade", gyro.getHeading());
  248.                 telemetry.addData(">", "%.3f", goalAngle);
  249.                 telemetry.update();
  250.             }
  251.             else {
  252.                 goalReached = 1;
  253.                 stop();
  254.             }
  255.         }
  256.         stop();
  257.     }
  258.  
  259.     public void RotireStanga(double speed)
  260.     {
  261.         boolean goalReached = false;
  262.         if (!gyro.isCalibrating() && opModeIsActive()) {
  263.             if (gyro.getHeading() > 90) {
  264.                 goalReached = false;
  265.             }
  266.             if (goalReached) {
  267.                 Stop();
  268.             }
  269.             if (!goalReached) {
  270.                 Dreapta(speed);
  271.             }
  272.         }
  273.     }
  274.  
  275.     VuforiaLocalizer vuforia;
  276.  
  277.     public int ceva(){
  278.         int col=-1;
  279.         int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  280.         VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
  281.  
  282.         parameters.vuforiaLicenseKey = "AVR36zX/////AAAAmf4wKl7jz04GkSKCzFf6A/gIfIqXUXFiD8Q8smknNL1/7mQ7bfNDDX/GKq/vejsAFCypbnHQwmHunWd2pk/dOZY2Wi4Nj64UbWmDawkA3Jy9oiIfS4C7sfViotjeuhQZuuUuHOEDh63tJEX54rWgXUHYYpBUApz+2pB4ijjg+YipO05M9EEInFeUqL3rpEu1vuLq942L/tc7r2C/Am9W37dHlCMlIBYJ2f1RpTdi/6+WVFuiD5lhIyL6hGU9OMhmRzBrZeJWF0GNFHqi21JxiD9IpRIsk6KDqMdS/gYEWg20Lkk/vPEDbKXCrdm9iAQjlBAzjPq0gIE6i785JI8y61qzxohgI4PXEF0ciUqiC2UD";
  283.  
  284.  
  285.         parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
  286.         this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
  287.  
  288.         VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
  289.         VuforiaTrackable relicTemplate = relicTrackables.get(0);
  290.         relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary
  291.  
  292.         telemetry.update();
  293.         waitForStart();
  294.  
  295.         relicTrackables.activate();
  296.  
  297.         while (opModeIsActive()) {
  298.  
  299.             RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
  300.             if(vuMark==RelicRecoveryVuMark.LEFT) {
  301.                 telemetry.addData("left", vuMark);
  302.                 OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicTemplate.getListener()).getPose();
  303.                 col=0;
  304.                 return col;
  305.             }
  306.             if(vuMark==RelicRecoveryVuMark.CENTER) {
  307.                 telemetry.addData("center", vuMark);
  308.                 OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicTemplate.getListener()).getPose();
  309.                 col=1;
  310.                 return col;
  311.             }
  312.             if(vuMark==RelicRecoveryVuMark.RIGHT) {
  313.                 telemetry.addData("right", vuMark);
  314.                 OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicTemplate.getListener()).getPose();
  315.                 col=2;
  316.                 return col;
  317.             }
  318.             telemetry.update();
  319.  
  320.         }
  321.         return col;
  322.  
  323.     }
  324.  
  325. }
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!