API tools faq
paste
Advertisement
Golden_Dragoon

Hephaestus

Golden_Dragoon
Feb 20th, 2019
102
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 14.20 KB | None | 0 0
raw download clone embed print report
  1. /* Copyright (c) 2018 The Light Brigade, Team 13507. All rights reserved.
  2.  *
  3.  * Redistribution and use in source and binary forms, with or without modification,
  4.  * are permitted (subject to the limitations in the disclaimer below) provided that
  5.  * the following conditions are met:
  6.  *
  7.  * Redistributions of source code must retain the above copyright notice, this list
  8.  * of conditions and the following disclaimer.
  9.  *
  10.  * Redistributions in binary form must reproduce the above copyright notice, this
  11.  * list of conditions and the following disclaimer in the documentation and/or
  12.  * other materials provided with the distribution.
  13.  *
  14.  * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
  15.  * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  16.  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  17.  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18.  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
  19.  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  20.  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  21.  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  22.  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  23.  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  24.  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  25.  */
  26.  
  27. package org.firstinspires.ftc.teamcode.TeleOp;
  28.  
  29. import com.qualcomm.hardware.bosch.BNO055IMU;
  30. import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
  31. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  32. import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  33. import com.qualcomm.robotcore.hardware.DcMotor;
  34. import com.qualcomm.robotcore.hardware.Servo;
  35.  
  36. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  37. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  38. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  39. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  40.  
  41. //import com.qualcomm.robotcore.hardware.CRServo;
  42.  
  43.  
  44. @TeleOp(name = "Hephaestus", group = "Chronos")
  45. public class Hephaestus extends LinearOpMode {
  46.  
  47.     protected boolean DriverControlled = true;
  48.     DcMotor ForwardRight, ForwardLeft, BackLeft, BackRight;
  49.     DcMotor EncoderY, EncoderX;
  50.     DcMotor LeftLift, RightLift;
  51.     DcMotor CableSpool, Intake;
  52.     Servo Flap;
  53.     BNO055IMU imu;
  54.     double positionY, positionX;
  55.     int RightLeaderPos;
  56.     Orientation angles;
  57.     private double previousHeading = 0;
  58.     private double integratedHeading = 0;
  59.  
  60.     //ExpansionHubEx expansionHub;
  61.  
  62.  
  63.     //ColorSensor color_sensor;
  64.     public void runOpMode() {
  65.  
  66.         telemetry.addData("Status", "Initialized");
  67.         telemetry.update();
  68.  
  69.  
  70.         double ForwardLeftPower;
  71.         double ForwardRightPower;
  72.         double BackLeftPower;
  73.         double BackRightPower;
  74.         double G1LY;
  75.         double G1LX;
  76.         double G1RX;
  77.         double G1RY;
  78.         double G2LY;
  79.         double G2RY;
  80.         double SpeedMod = .6;
  81.         double LastHeading = 0;
  82.         double CurrentCompassHeading = 0;
  83.  
  84.         int ArmPos = 1, CableSpoolPos = 1, ArmPosStop = 2, CableSpoolPosStop = 2;
  85.         //String LiftMovingDown = "false";
  86.  
  87.         Flap = hardwareMap.get(Servo.class, "Flap");
  88.  
  89.         ForwardLeft = hardwareMap.get(DcMotor.class, "ForwardLeft");
  90.         /*ForwardLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);*/
  91.         ForwardLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  92.         ForwardLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  93.  
  94.         ForwardRight = hardwareMap.get(DcMotor.class, "ForwardRight");
  95.         /*ForwardRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);*/
  96.         ForwardRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  97.         ForwardRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  98.  
  99.         BackLeft = hardwareMap.get(DcMotor.class, "BackLeft");
  100.         /*BackLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);*/
  101.         BackLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  102.         BackLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  103.  
  104.         BackRight = hardwareMap.get(DcMotor.class, "BackRight");
  105.         /*BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);*/
  106.         BackRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  107.         BackRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  108.  
  109.         /*ArmLifter = hardwareMap.get(DcMotor.class, "ArmExtend");
  110.         ArmLifter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);*/
  111.  
  112.         EncoderY = hardwareMap.get(DcMotor.class, "EncoderY");
  113.         EncoderY.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  114.         EncoderY.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  115.  
  116.         EncoderX = hardwareMap.get(DcMotor.class, "EncoderX");
  117.         EncoderX.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  118.         EncoderX.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  119.  
  120.         LeftLift = hardwareMap.get(DcMotor.class, "EncoderY");
  121.         //BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  122.         //LeftLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  123.         LeftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  124.         LeftLift.setDirection(DcMotor.Direction.REVERSE);
  125.         LeftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  126.  
  127.         RightLift = hardwareMap.get(DcMotor.class, "RightLift");
  128.         RightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  129.         RightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  130.         RightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  131.  
  132.         CableSpool = hardwareMap.get(DcMotor.class, "CableSpool");
  133.         //BackRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  134.         CableSpool.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  135.         CableSpool.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  136.         CableSpool.setDirection(DcMotor.Direction.REVERSE);
  137.         CableSpool.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  138.  
  139.         Intake = hardwareMap.get(DcMotor.class, "EncoderX");
  140.         Intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  141.         Intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  142.  
  143.         ForwardLeft.setDirection(DcMotor.Direction.FORWARD);
  144.         ForwardRight.setDirection(DcMotor.Direction.REVERSE);
  145.         BackLeft.setDirection(DcMotor.Direction.FORWARD);
  146.         BackRight.setDirection(DcMotor.Direction.REVERSE);
  147.  
  148.         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  149.         parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
  150.         parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
  151.         parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
  152.         parameters.loggingEnabled = true;
  153.         parameters.loggingTag = "IMU";
  154.         parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
  155.  
  156.         imu = hardwareMap.get(BNO055IMU.class, "imu");
  157.         imu.initialize(parameters);
  158.         boolean isG2APressed = false, isG2YPressed = false, HasPassed180 = false;
  159.         // Wait for the game to start (driver presses PLAY)
  160.         waitForStart();
  161.  
  162.         RightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  163.         // run until the end of the match (driver presses STOP)
  164.         while (opModeIsActive()) {
  165.             float zAxis = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  166.             telemetry.addData("RightLiftActualPos", RightLift.getCurrentPosition());
  167.             telemetry.addData("CableSpoolActualPos", CableSpool.getCurrentPosition());
  168.             /**
  169.              * Main Init
  170.              */
  171.             G1LY = gamepad1.left_stick_y;
  172.             G1LX = gamepad1.left_stick_x;
  173.             G1RX = gamepad1.right_stick_x;
  174.             G1RY = gamepad1.right_stick_y;
  175.             G2LY = gamepad2.left_stick_y;
  176.             G2RY = -gamepad2.right_stick_y;
  177.             telemetry.addData("Status", "Running");
  178.             telemetry.update();
  179.  
  180.             /**
  181.              * Driver Controls
  182.              */
  183.             //if(DriverControlled = true) {
  184.  
  185.             ForwardLeftPower = G1LY + -G1RX;
  186.             ForwardRightPower = G1LY + G1RX;
  187.             BackRightPower = G1LY + -G1RX;
  188.             BackLeftPower = G1LY + G1RX;
  189.  
  190.             if (gamepad1.a) {
  191.                 SpeedMod = .4;
  192.             }
  193.             if (gamepad1.b) {
  194.                 SpeedMod = .6;
  195.             }
  196.             if (gamepad1.y) {
  197.                 SpeedMod = .8;
  198.             }
  199.             if (gamepad1.x) {
  200.                 SpeedMod = 1;
  201.             }
  202.  
  203.             while (gamepad1.dpad_left) {
  204.                 ForwardLeft.setPower(1 * SpeedMod);
  205.                 ForwardRight.setPower(-1 * SpeedMod);
  206.                 BackLeft.setPower(1 * SpeedMod);
  207.                 BackRight.setPower(-1 * SpeedMod);
  208.                 ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  209.                 ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  210.                 BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  211.                 BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  212.             }
  213.  
  214.             while (gamepad1.dpad_right) {
  215.                 ForwardLeft.setPower(-1 * SpeedMod);
  216.                 ForwardRight.setPower(1 * SpeedMod);
  217.                 BackLeft.setPower(-1 * SpeedMod);
  218.                 BackRight.setPower(1 * SpeedMod);
  219.                 ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  220.                 ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  221.                 BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  222.                 BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  223.             }
  224.  
  225.             if (gamepad1.atRest()) {
  226.                 ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  227.                 ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  228.                 BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  229.                 BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  230.             } else {
  231.                 ForwardLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  232.                 ForwardRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  233.                 BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  234.                 BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
  235.             }
  236.  
  237.             ForwardLeft.setPower(ForwardLeftPower * SpeedMod);
  238.             ForwardRight.setPower(ForwardRightPower * SpeedMod);
  239.             BackLeft.setPower(BackLeftPower * SpeedMod);
  240.             BackRight.setPower(BackRightPower * SpeedMod);
  241.  
  242.             /**
  243.              * Operator controls
  244.              * */
  245.  
  246.             if (gamepad2.x) {
  247.                 Flap.setPosition(.37);
  248.             } else {
  249.                 Flap.setPosition(.1);
  250.             }
  251.  
  252.             if (gamepad2.dpad_right) {
  253.                 Intake.setPower(.3);
  254.             }
  255.             if (gamepad2.dpad_left) {
  256.                 Intake.setPower(-1);
  257.             }
  258.             if (gamepad2.dpad_down) {
  259.                 Intake.setPower(0);
  260.             }
  261.  
  262.             if (RightLift.getCurrentPosition() >= -30) {
  263.                 ArmPos = 1;
  264.             } else if (RightLift.getCurrentPosition() <= -6000) {
  265.                 ArmPos = 3;
  266.             } else {
  267.                 ArmPos = 2;
  268.             }
  269.  
  270.             if (ArmPos != 3 && G2RY <= 0) {
  271.                 RightLift.setPower(G2RY);
  272.                 LeftLift.setPower(G2RY);
  273.             } else if (ArmPos != 1 && G2RY >= 0) {
  274.                 RightLift.setPower(G2RY);
  275.                 LeftLift.setPower(G2RY);
  276.             } else if (CableSpoolPos != 3 && G2RY >= 0) {
  277.                 RightLift.setPower(0);
  278.                 LeftLift.setPower(0);
  279.             } else if (CableSpoolPos != 1 && G2LY <= 0) {
  280.                 RightLift.setPower(0);
  281.                 LeftLift.setPower(0);
  282.             } else {
  283.                 RightLift.setPower(G2RY);
  284.                 LeftLift.setPower(G2RY);
  285.             }
  286.  
  287.  
  288.             if (CableSpool.getCurrentPosition() >= -5 && CableSpoolPos != 13) {
  289.                 CableSpoolPos = 1;
  290.             } else if (CableSpool.getCurrentPosition() <= -3300 && CableSpoolPos != 13) {
  291.                 CableSpoolPos = 3;
  292.             } else {
  293.                 CableSpoolPos = 2;
  294.             }
  295.  
  296.             if (CableSpoolPos != 3 && G2LY <= 0) {
  297.                 CableSpool.setPower(G2LY);
  298.             } else if (CableSpoolPos != 1 && G2LY >= 0) {
  299.                 CableSpool.setPower(G2LY);
  300.             } else if (CableSpoolPos != 3 && G2LY >= 0) {
  301.                 CableSpool.setPower(0);
  302.             } else if (CableSpoolPos != 1 && G2LY <= 0) {
  303.                 CableSpool.setPower(0);
  304.             } else {
  305.                 CableSpool.setPower(G2LY);
  306.             }
  307.  
  308.             while (gamepad2.right_bumper && !isStopRequested()) {
  309.                 RightLift.setPower(1);
  310.                 LeftLift.setPower(1);
  311.                 RightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  312.                 RightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  313.  
  314.             }
  315.  
  316.             if (gamepad2.a) {
  317.                 PythReturn(EncoderY.getCurrentPosition(), EncoderX.getCurrentPosition());
  318.             }
  319.  
  320.  
  321.         }
  322.     }
  323.  
  324.     public void PythReturn(int yCoord, int xCoord) {
  325.         double hypoLength = Math.sqrt((yCoord ^ 2) + (xCoord ^ 2));
  326.         double hypoAngle = Math.acos(xCoord / hypoLength);
  327.  
  328.     }
  329.  
  330.     private double getIntegratedHeading() {
  331.         double currentHeading = imu.getAngularOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
  332.         double deltaHeading = currentHeading - previousHeading;
  333.  
  334.         if (deltaHeading < -180) {
  335.             deltaHeading += 360;
  336.         } else if (deltaHeading >= 180) {
  337.             deltaHeading -= 360;
  338.         }
  339.  
  340.         integratedHeading += deltaHeading;
  341.         previousHeading = currentHeading;
  342.  
  343.         return integratedHeading;
  344.     }
  345. }
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!