SHARE
TWEET

Turn Class

a guest Nov 16th, 2019 97 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import android.graphics.drawable.GradientDrawable;
  4.  
  5. import com.qualcomm.hardware.bosch.BNO055IMU;
  6. import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
  7. import com.qualcomm.hardware.lynx.commands.core.LynxGetMotorPIDControlLoopCoefficientsCommand;
  8. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  9. import com.qualcomm.robotcore.hardware.DcMotor;
  10. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  11. import com.qualcomm.robotcore.hardware.PIDCoefficients;
  12.  
  13. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  14. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  15. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  16. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  17.  
  18. import java.util.concurrent.TimeUnit;
  19.  
  20. public class Turn
  21. {
  22.     LinearOpMode opMode;
  23.     public DcMotor BDL;
  24.     public DcMotor BDR;
  25.     public DcMotor FDR;
  26.     public DcMotor FDL;
  27.     BNO055IMU imu;
  28.     Orientation orientation;
  29.     double current;
  30.  
  31.  
  32.     public Turn(LinearOpMode ahwMap)
  33.     {
  34.         this.opMode = ahwMap;
  35.         BDL = ahwMap.hardwareMap.dcMotor.get("BDL");
  36.         BDR = ahwMap.hardwareMap.dcMotor.get("BDR");
  37.         FDL = ahwMap.hardwareMap.dcMotor.get("FDL");
  38.         FDR = ahwMap.hardwareMap.dcMotor.get("FDR");
  39.         imu = ahwMap.hardwareMap.get(BNO055IMU.class, "imu");
  40.         BDL.setDirection(DcMotor.Direction.REVERSE);
  41.         FDL.setDirection(DcMotor.Direction.REVERSE);
  42.  
  43.         BDL.setPower(0);
  44.         BDR.setPower(0);
  45.         FDL.setPower(0);
  46.         FDR.setPower(0);
  47.  
  48.         // Set all otors to run without encoders.
  49.         // May want to use RUN_USING_ENCODERS if encoders are installed.
  50.         FDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  51.         FDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  52.         BDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  53.         BDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  54.  
  55.         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  56.         parameters.angleUnit           = BNO055IMU.AngleUnit.DEGREES;
  57.         parameters.accelUnit           = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  58.         parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
  59.         parameters.loggingEnabled      = true;
  60.         parameters.loggingTag          = "IMU";
  61.         parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  62.  
  63.         // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
  64.         // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
  65.         // and named "imu".
  66.         imu.initialize(parameters);
  67.     }
  68.  
  69.     public void resetAngle()
  70.     {
  71.         orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX , AngleUnit.DEGREES);
  72.         current = 0;
  73.     }
  74.  
  75.     public void IMUTurning (double target , double speed)
  76.     {
  77.         if(target > current)
  78.         {
  79.             opMode.telemetry.addData("toGo", current - target);
  80.             while ((current + target) - current > 2 && opMode.opModeIsActive())
  81.             {
  82.                 orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  83.                 current = orientation.firstAngle;
  84.                 opMode.telemetry.addData("toGo", target - current);
  85.                 opMode.telemetry.update();
  86.                 FDR.setPower(speed);
  87.                 FDL.setPower(-speed);
  88.                 BDR.setPower(-speed);
  89.                 BDL.setPower(speed);
  90.                 opMode.telemetry.update();
  91.             }
  92.         }
  93.         else
  94.         {
  95.             opMode.telemetry.addData("toGo", target - current);
  96.             while (current - (target + current) > 2 && opMode.opModeIsActive())
  97.             {
  98.                 orientation = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  99.                 current = orientation.firstAngle; //getHeading
  100.                 opMode.telemetry.addData("toGo", current - target);
  101.                 opMode.telemetry.update();
  102.                 FDR.setPower(-speed);
  103.                 FDL.setPower(speed);
  104.                 BDR.setPower(speed);
  105.                 BDL.setPower(-speed);
  106.                 opMode.telemetry.update();
  107.             }
  108.         }
  109.         resetAngle();
  110.     }
  111. }
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