BookSerpent

Roboot.java

Dec 2nd, 2021
907
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. package org.firstinspires.ftc.teamcode;
  3.  
  4. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  5. import com.qualcomm.robotcore.util.Range;
  6. import com.qualcomm.robotcore.hardware.LED;
  7. import com.qualcomm.robotcore.hardware.CRServo;
  8. import com.qualcomm.robotcore.util.Hardware;
  9. import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
  10. import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
  11. import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
  12. import com.qualcomm.robotcore.eventloop.opmode.OpMode;
  13. import com.qualcomm.robotcore.eventloop.opmode.Disabled;
  14. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  15. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  16. import java.util.List;
  17. import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  18. import com.qualcomm.robotcore.hardware.DcMotor;
  19. import org.firstinspires.ftc.robotcore.external.Func;
  20. import com.qualcomm.hardware.bosch.BNO055IMU;
  21. import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
  22. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  23. import java.util.Locale;
  24. import org.firstinspires.ftc.robotcore.external.ClassFactory;
  25. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  26. import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
  27. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  28. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  29. import org.firstinspires.ftc.robotcore.external.navigation.Position;
  30. import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
  31.  
  32. import com.qualcomm.robotcore.hardware.DcMotor;
  33. import com.qualcomm.robotcore.hardware.Servo;
  34. import com.qualcomm.robotcore.hardware.HardwareMap;
  35. import com.qualcomm.robotcore.util.ElapsedTime;
  36. import com.qualcomm.hardware.rev.RevBlinkinLedDriver.BlinkinPattern;
  37. import com.qualcomm.robotcore.hardware.HardwareDevice;
  38.  
  39. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
  40. import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
  41. import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
  42. import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
  43.  
  44.  
  45.    
  46. public class Roboot {
  47.     public ElapsedTime runtime = new ElapsedTime();
  48.     public static DcMotor FL = null;
  49.     public static DcMotor FR = null;
  50.     public static DcMotor BL = null;
  51.     public static DcMotor BR = null;
  52.     public static DcMotor DUCK = null;
  53.     public static DcMotor Intake = null;
  54.     public static DcMotor Arm = null;
  55.     BNO055IMU imu1;
  56.     ModernRoboticsI2cRangeSensor RS;
  57.    
  58. /*    
  59.     public DcMotor Intake1 = null;
  60.     public DcMotor Intake2 = null;
  61.     public DcMotor Shooter1 = null;
  62.     public DcMotor Shooter2 = null;
  63.     public CRServo Conveyor1 = null;
  64.     public CRServo WGoal = null;
  65.     public Servo WGLatch = null;
  66.     public TFObjectDetector tfod;
  67.     public VuforiaLocalizer vuforia;
  68.     ModernRoboticsI2cRangeSensor Range1;
  69. */
  70.    
  71.     public double SPF = 1;
  72.  
  73.  
  74.    
  75.     Orientation angles;
  76.     Acceleration gravity;
  77.     double globalAngle, power = .30, Correction;
  78.     Orientation  lastAngles = new Orientation();
  79.     private ElapsedTime period  = new ElapsedTime();
  80.     public int ObjectSize = 0;
  81.  //   public String Stack = "";
  82. //  public RevBlinkinLedDriver Led;
  83.  
  84.  
  85.    public double circumference = 9.6*3.14;
  86.    public double count_per_motor = 537.7;
  87.    public double ogtick = count_per_motor/circumference;
  88. // correction is distance/test distance
  89.    public double correction = 0.97;
  90.    public double tick = ogtick/correction;
  91.    public double strafeCorrection = 100/90;
  92.    public double StrafeTick = ogtick/strafeCorrection;
  93.    public int duckSpin = 0;
  94.    
  95.  
  96.    
  97.  
  98.    HardwareMap hardwareMap = null;
  99.    
  100.   public void RobotInit(HardwareMap hardwareMap){
  101.  
  102.        
  103.         FL  = hardwareMap.get(DcMotor.class, "FL");
  104.         FR = hardwareMap.get(DcMotor.class, "FR");
  105.         BL  = hardwareMap.get(DcMotor.class, "BL");
  106.         BR = hardwareMap.get(DcMotor.class, "BR");
  107.         DUCK = hardwareMap.get(DcMotor.class, "DUCK");
  108.         imu1 = hardwareMap.get(BNO055IMU.class, "imu1");
  109.         Intake = hardwareMap.get(DcMotor.class, "Intake");
  110.         Arm = hardwareMap.get(DcMotor.class, "Arm");
  111.         RS = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "RS");
  112.         /*    
  113.         Conveyor1 = hardwareMap.get(CRServo.class, "C1");
  114.         WGLatch = hardwareMap.get(Servo.class, "WG2");
  115.         Led = hardwareMap.get(RevBlinkinLedDriver.class, "LED");
  116.         Range1 = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "RS1");
  117.  */      
  118.    
  119.         FR.setDirection(DcMotor.Direction.REVERSE);
  120.         BR.setDirection(DcMotor.Direction.REVERSE);
  121.        
  122.        
  123.        
  124.         //Set motors to float
  125.         FL.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  126.         BR.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  127.         BL.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  128.         FR.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  129.        
  130.         //Run using encoders to be more precise (built in PID)
  131.         FL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  132.         BR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  133.         BL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  134.         FR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  135.  
  136. /*    
  137.         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  138.         parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
  139.         parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  140.         parameters.calibrationDataFile = "BNO055IMUCalibration.json";
  141.         parameters.loggingEnabled      = true;
  142.         parameters.loggingTag          = "IMU";
  143.         parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  144.         imu1.initialize(parameters);
  145.         }
  146.         String formatAngle(AngleUnit angleUnit, double angle) {
  147.                 return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
  148.             }
  149.        
  150.             String formatDegrees(double degrees){
  151.                 return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
  152.             }
  153.             private void resetAngle()
  154.             {
  155.                 Orientation lastAngles = imu1.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  156.        
  157.                 globalAngle = 0;    
  158.             }
  159.            
  160.    
  161.    
  162.     private double getAngle()
  163.     {
  164.  
  165.         Orientation angles = imu1.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  166.  
  167.         double deltaAngle = angles.firstAngle - lastAngles.firstAngle;
  168.  
  169.         if (deltaAngle < -180)
  170.             deltaAngle += 360;
  171.         else if (deltaAngle > 180)
  172.             deltaAngle -= 360;
  173.  
  174.         globalAngle += deltaAngle;
  175.  
  176.         lastAngles = angles;
  177.  
  178.         return globalAngle;
  179.     }
  180.            
  181.  
  182.  
  183.  
  184.    
  185.    
  186.    
  187.     public void rotate (double degrees, double power){
  188.         double FLPow;
  189.         double BRPow;
  190.         double BLPow;
  191.         double FRPow;
  192.      
  193.         FL.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  194.        
  195.         BR.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  196.        
  197.         BL.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  198.        
  199.         FR.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  200.       //positive degrees is left turn, negative is right
  201.       if(degrees>0){
  202.           //degrees is correction, adjust if necessary
  203.            degrees = degrees -15;
  204.            FLPow = -power;
  205.            BRPow = power;
  206.            FRPow = power;
  207.            BLPow = -power;
  208.          }else if (degrees < 0){
  209.            degrees = degrees +10;
  210.            FLPow = power;
  211.            BRPow = -power;
  212.            FRPow = -power;
  213.            BLPow = power;
  214.          }else return;
  215.         FL.setPower(FLPow);
  216.         BR.setPower(BRPow);
  217.         BL.setPower(BLPow);
  218.         FR.setPower(FRPow);
  219.        
  220.         while (getAngle() == 0) {}
  221.              
  222.               if(degrees<0){
  223.    
  224.                 while (getAngle() > degrees) {}
  225.               }else if (degrees >0){
  226.            
  227.                 while (getAngle() < degrees) {}
  228.               }
  229.             FL.setPower(0);
  230.             BR.setPower(0);
  231.             BL.setPower(0);
  232.             FR.setPower(0);
  233.    
  234.            
  235.             resetAngle();
  236.     }
  237.  
  238.  
  239. */
  240.        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  241.         parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
  242.         parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  243.         parameters.calibrationDataFile = "BNO055IMUCalibration.json";
  244.         parameters.loggingEnabled      = true;
  245.         parameters.loggingTag          = "IMU";
  246.         parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  247.         imu1.initialize(parameters);
  248.         }
  249.         String formatAngle(AngleUnit angleUnit, double angle) {
  250.                 return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
  251.             }
  252.        
  253.             String formatDegrees(double degrees){
  254.                 return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
  255.             }
  256.             private void resetAngle()
  257.             {
  258.                 Orientation lastAngles = imu1.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  259.        
  260.                 globalAngle = 0;
  261.             }
  262.            
  263.    
  264.    
  265.     private double getAngle()
  266.     {
  267.  
  268.         Orientation angles = imu1.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  269.  
  270.         double deltaAngle = angles.firstAngle - lastAngles.firstAngle;
  271.  
  272.         if (deltaAngle < -180)
  273.             deltaAngle += 360;
  274.         else if (deltaAngle > 180)
  275.             deltaAngle -= 360;
  276.  
  277.         globalAngle += deltaAngle;
  278.  
  279.         lastAngles = angles;
  280.  
  281.         return globalAngle;
  282.     }
  283.    
  284.    
  285.    
  286.    
  287.     public void rotate (double degrees, double power){
  288.         double FLPow;
  289.         double BRPow;
  290.         double BLPow;
  291.         double FRPow;
  292.      
  293.       FL.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  294.        
  295.         BR.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  296.        
  297.         BL.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  298.        
  299.         FR.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  300.      
  301.       if(degrees>0){
  302.           degrees = degrees -15;
  303.            FLPow = -power;
  304.            BRPow = power;
  305.            FRPow = power;
  306.            BLPow = -power;
  307.          }else if (degrees < 0){
  308.            degrees = degrees +10;
  309.            FLPow = power;
  310.            BRPow = -power;
  311.            FRPow = -power;
  312.            BLPow = power;
  313.          }else return;
  314.         FL.setPower(FLPow);
  315.         BR.setPower(BRPow);
  316.         BL.setPower(BLPow);
  317.         FR.setPower(FRPow);
  318.        
  319.         while (getAngle() == 0) {}
  320.              
  321.               if(degrees<0){
  322.    
  323.                 while (getAngle() > degrees) {}
  324.               }else if (degrees >0){
  325.            
  326.                 while (getAngle() < degrees) {}
  327.               }
  328.             FL.setPower(0);
  329.             BR.setPower(0);
  330.             BL.setPower(0);
  331.             FR.setPower(0);
  332.    
  333.            
  334.             resetAngle();
  335.     }
  336.  
  337.  
  338.  
  339.  
  340.     /*
  341.    
  342.     public static final String VUFORIA_KEY =
  343.             "AfHBP+b/////AAABme/fHUiCZURxi3UJy7vnhS1D6Bh6XJmgXsQl9Ga3ByF7FTSk5JkgpO6pTtkJXX36sUQQNnmQ4CvHg7OPss29JxVOVmDHj/S6M0AqokCSluGffJSjCWh/jl47hN7eL6K3XiLR/8CMdHJQhWS8g+qZssG9t8VDH/aXnFX71rzs6ITnItrceB9qraxrU9t1A29U1FXjU4CQmzduh2r6m4OCvOjhxicggXqh3qOmmMzUbN99SvN+l/ZRcQvxwVrVXzzS+8ooid2z9fN2CplhLrj++17ZqgBH0IttuzMLL7iYt3n3qkP786KFgsijeP23yw3Z4qcMEmEzbX6v0e4NtB8b3wTOV/Vq9roe8UKEw8qKESNk";
  344.    
  345.     public void TFInit(){
  346.         initVuforia();
  347.         initTfod();
  348.     }
  349.     //if (tfod != null) {
  350.     //        tfod.activate();
  351.     //    }
  352. */
  353.  
  354.  
  355.     public void movecm(int CM,double power){
  356.     int FLTP = FL.getCurrentPosition() - (int)(CM*tick);
  357.     int BLTP = BL.getCurrentPosition() - (int)(CM*tick);
  358.     int FRTP = FR.getCurrentPosition() - (int)(CM*tick);
  359.     int BRTP = BR.getCurrentPosition() - (int)(CM*tick);
  360.    
  361.     FL.setTargetPosition(FLTP);
  362.     BL.setTargetPosition(BLTP);
  363.     FR.setTargetPosition(FRTP);
  364.     BR.setTargetPosition(BRTP);
  365.    
  366.     FL.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  367.     BL.setMode(DcMotor.RunMode.RUN_TO_POSITION);    
  368.     FR.setMode(DcMotor.RunMode.RUN_TO_POSITION);  
  369.     BR.setMode(DcMotor.RunMode.RUN_TO_POSITION);    
  370.    
  371.     FL.setPower(Math.abs(power));
  372.     BR.setPower(Math.abs(power));
  373.     BL.setPower(Math.abs(power));
  374.     FR.setPower(Math.abs(power));    
  375.  
  376.     while(FL.isBusy() && BL.isBusy() && FR.isBusy() && BR.isBusy())
  377.     {}
  378.    
  379.     FL.setPower(0);
  380.     BL.setPower(0);
  381.     FR.setPower(0);
  382.     BR.setPower(0);
  383.    
  384.     FL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  385.     BL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  386.     FR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  387.     BR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  388.     }
  389.  
  390.    
  391.    
  392.     public void strafe(int CM, double power) {
  393.     //+ = left, - = right
  394.     int FLTP = FL.getCurrentPosition() + (int)(CM*StrafeTick);
  395.     int BLTP = BL.getCurrentPosition() - (int)(CM*StrafeTick);
  396.     int FRTP = FR.getCurrentPosition() - (int)(CM*StrafeTick);
  397.     int BRTP = BR.getCurrentPosition() + (int)(CM*StrafeTick);
  398.    
  399.     FL.setTargetPosition(FLTP);
  400.     BL.setTargetPosition(BLTP);
  401.     FR.setTargetPosition(FRTP);
  402.     BR.setTargetPosition(BRTP);
  403.    
  404.     FL.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  405.     BL.setMode(DcMotor.RunMode.RUN_TO_POSITION);    
  406.     FR.setMode(DcMotor.RunMode.RUN_TO_POSITION);  
  407.     BR.setMode(DcMotor.RunMode.RUN_TO_POSITION);    
  408.    
  409.     FL.setPower(Math.abs(power));
  410.     BR.setPower(Math.abs(power));
  411.     BL.setPower(Math.abs(power));
  412.     FR.setPower(Math.abs(power));    
  413.    
  414.    
  415.     while(FL.isBusy() && BL.isBusy() && FR.isBusy() && BR.isBusy())
  416.     {}
  417.     FL.setPower(0);
  418.     BL.setPower(0);
  419.     FR.setPower(0);
  420.     BR.setPower(0);
  421.    
  422.     FL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  423.     BL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  424.     FR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  425.     BR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  426.      }
  427.    
  428.  
  429.  
  430. /*
  431.     public void LED (String Color){
  432.               if (Color == "Blue"){
  433.                   Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
  434.               }
  435.               if (Color == "Aqua"){
  436.                   Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.AQUA);
  437.               }
  438.               if (Color == "Rainbow"){
  439.                   Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.RAINBOW_RAINBOW_PALETTE);
  440.               }
  441.               if (Color == "Red"){
  442.                   Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED);
  443.               }
  444.               if (Color == "Slow"){
  445.                   Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_GRAY);
  446.               }
  447.               if (Color == "Fast"){
  448.                   Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE);
  449.               }
  450.               if (Color == "Green"){
  451.                  Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
  452.               }
  453.               if (Color == "White"){
  454.                  Led.setPattern(RevBlinkinLedDriver.BlinkinPattern.WHITE);
  455.               }
  456.           }
  457.  
  458.    
  459.  
  460. */
  461. /*
  462.     public void initVuforia() {
  463.         VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
  464.         parameters.vuforiaLicenseKey = VUFORIA_KEY;
  465.         parameters.cameraDirection = CameraDirection.BACK;
  466.         vuforia = ClassFactory.getInstance().createVuforia(parameters);
  467.     }
  468.     public void initTfod() {
  469.         int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
  470.             "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
  471.         TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
  472.         tfodParameters.minResultConfidence = 0.8f;
  473.         tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
  474.         tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
  475.     }
  476. */  
  477.    
  478.    
  479.    
  480.    
  481. }      
  482.        
  483.  
  484.          
  485.  
  486.          
  487.        
  488.  
  489.  
RAW Paste Data