SHARE
TWEET

Return To Position Code

Golden_Dragoon Feb 17th, 2019 70 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. while (gamepad2.y) {
  2.  
  3.                 DriverControlled = false;
  4.                 getIntegratedHeading();
  5.                 int m_MODPOSY = EncoderY.getCurrentPosition() - (int) getIntegratedHeading();
  6.                 int m_MODPOSX = EncoderX.getCurrentPosition() - (int) getIntegratedHeading();
  7.                 long zAxisExtrap = Math.abs((int) zAxis);
  8.  
  9.                 telemetry.addData("Mod Pos Y", m_MODPOSY);
  10.                 telemetry.addData("Mod Pos X", m_MODPOSX);
  11.                 telemetry.update();
  12.  
  13.                 //This chunk works well
  14.  
  15.                 while (zAxis < -5) {
  16.                     angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  17.                     zAxis = angles.firstAngle;
  18.                     ForwardLeft.setPower(.3);
  19.                     ForwardRight.setPower(-.3);
  20.                     BackLeft.setPower(.3);
  21.                     BackRight.setPower(-.3);
  22.                 }
  23.  
  24.                 while (zAxis > 5) {
  25.                     angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
  26.                     zAxis = angles.firstAngle;
  27.                     ForwardLeft.setPower(-.3);
  28.                     ForwardRight.setPower(.3);
  29.                     BackLeft.setPower(-.3);
  30.                     BackRight.setPower(.3);
  31.                 }
  32.  
  33.                 try {
  34.                     Thread.sleep(zAxisExtrap);
  35.                 } catch (InterruptedException ex) {
  36.                     Thread.currentThread().interrupt();
  37.                 }
  38.  
  39.                 long waitfor = Math.abs(m_MODPOSX + m_MODPOSY + (int) zAxis);
  40.  
  41.                 while (m_MODPOSX <= -40) {
  42.                     ForwardLeft.setPower(-.4);
  43.                     ForwardRight.setPower(.4);
  44.                     BackLeft.setPower(.4);
  45.                     BackRight.setPower(-.4);
  46.                     m_MODPOSY = EncoderY.getCurrentPosition();
  47.                     m_MODPOSX = EncoderX.getCurrentPosition();
  48.                 }
  49.  
  50.                 while (m_MODPOSX >= 40) {
  51.                     ForwardLeft.setPower(.4);
  52.                     ForwardRight.setPower(-.4);
  53.                     BackLeft.setPower(-.4);
  54.                     BackRight.setPower(.4);
  55.                     m_MODPOSY = EncoderY.getCurrentPosition();
  56.                     m_MODPOSX = EncoderX.getCurrentPosition();
  57.                 }
  58.  
  59.                 if (m_MODPOSX <= 40 || m_MODPOSX >= -40) {
  60.                     ForwardLeft.setPower(0);
  61.                     ForwardRight.setPower(0);
  62.                     BackLeft.setPower(0);
  63.                     BackRight.setPower(0);
  64.                     m_MODPOSY = EncoderY.getCurrentPosition();
  65.                     m_MODPOSX = EncoderX.getCurrentPosition();
  66.                 }
  67.  
  68.                 try {
  69.                     Thread.sleep(waitfor / 2);
  70.                 } catch (InterruptedException ex) {
  71.                     Thread.currentThread().interrupt();
  72.                 }
  73.  
  74.                 while (m_MODPOSY <= -40) {
  75.                     ForwardLeft.setPower(-.4);
  76.                     ForwardRight.setPower(-.4);
  77.                     BackLeft.setPower(-.4);
  78.                     BackRight.setPower(-.4);
  79.                     m_MODPOSY = EncoderY.getCurrentPosition();
  80.                     m_MODPOSX = EncoderX.getCurrentPosition();
  81.                 }
  82.  
  83.                 while (m_MODPOSY >= 40) {
  84.                     ForwardLeft.setPower(.4);
  85.                     ForwardRight.setPower(.4);
  86.                     BackLeft.setPower(.4);
  87.                     BackRight.setPower(.4);
  88.                     m_MODPOSY = EncoderY.getCurrentPosition();
  89.                     m_MODPOSX = EncoderX.getCurrentPosition();
  90.                 }
  91.  
  92.                 if (m_MODPOSY <= 40 || m_MODPOSY >= -40) {
  93.                     ForwardLeft.setPower(0);
  94.                     ForwardRight.setPower(0);
  95.                     BackLeft.setPower(0);
  96.                     BackRight.setPower(0);
  97.                     m_MODPOSY = EncoderY.getCurrentPosition();
  98.                     m_MODPOSX = EncoderX.getCurrentPosition();
  99.                 }
  100.  
  101.                 try {
  102.                     Thread.sleep(waitfor / 2);
  103.                 } catch (InterruptedException ex) {
  104.                     Thread.currentThread().interrupt();
  105.                 }
  106.  
  107.                 ForwardLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  108.                 ForwardRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  109.                 BackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  110.                 BackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  111.  
  112.  
  113.                 DriverControlled = true;
  114.             }
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