SHARE
TWEET

Untitled

a guest Jan 25th, 2020 74 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.hardware.bosch.BNO055IMU;
  4. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  5. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  6. import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  7. import com.qualcomm.robotcore.hardware.DcMotor;
  8. import com.qualcomm.robotcore.hardware.DigitalChannel;
  9. import com.qualcomm.robotcore.hardware.DistanceSensor;
  10. import com.qualcomm.robotcore.hardware.Servo;
  11. import com.qualcomm.robotcore.util.ElapsedTime;
  12. import com.qualcomm.robotcore.util.Range;
  13.  
  14. import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
  15. import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
  16. import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
  17. import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
  18. import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
  19.  
  20. @TeleOp
  21. public class station_test extends LinearOpMode {
  22.  
  23.     private DcMotor backLeftWheel, backRightWheel, frontLeftWheel, frontRightWheel, linearLift, linearLift2;
  24.     private Servo CLAW, trayServoL, trayServoR;
  25.     BNO055IMU imu;
  26.     Orientation angles;
  27.     DistanceSensor Distance;
  28.     DigitalChannel Touch;
  29.     private ElapsedTime runtime = new ElapsedTime();
  30.     double startPosition;
  31.     int mockGoalLiftheight = 0;
  32.     boolean upButton;
  33.     boolean downButton;
  34.     double liftMultiplier = 1;
  35.     int goalLiftHeight = 0;
  36.     int prevMockGoalLiftHeight;
  37.     boolean goingUp = false;
  38.     boolean goingDown = false;
  39.  
  40.     @Override
  41.     public void runOpMode() throws InterruptedException {
  42.  
  43.         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
  44.         parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
  45.         parameters.calibrationDataFile = "BNO055IMUCalibration.json";
  46.  
  47.         imu = hardwareMap.get(BNO055IMU.class, "imu");
  48.         backLeftWheel = hardwareMap.get(DcMotor.class, "Back_left_wheel");
  49.         backRightWheel = hardwareMap.get(DcMotor.class, "Back_right_wheel");
  50.         frontLeftWheel = hardwareMap.get(DcMotor.class, "Front_left_wheel");
  51.         frontRightWheel = hardwareMap.get(DcMotor.class, "Front_right_wheel");
  52.         CLAW = hardwareMap.servo.get("CLAW");
  53.         linearLift = hardwareMap.get(DcMotor.class, "linearLift");
  54.         linearLift2 = hardwareMap.get(DcMotor.class, "linearLift2");
  55.         trayServoL = hardwareMap.get(Servo.class, "trayServoL");
  56.         trayServoR = hardwareMap.get(Servo.class, "trayServoR");
  57.         Distance = hardwareMap.get(DistanceSensor.class, "Distance");
  58.         Touch = hardwareMap.get(DigitalChannel.class, "Touch");
  59.         imu.initialize(parameters);
  60.  
  61.         backRightWheel.setDirection(DcMotor.Direction.REVERSE);
  62.         frontRightWheel.setDirection(DcMotor.Direction.REVERSE);
  63.         backLeftWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  64.         backRightWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  65.         frontLeftWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  66.         frontRightWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  67.         linearLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  68.         linearLift2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
  69.         //linearLift2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  70.         //linearLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  71.         Touch.setMode(DigitalChannel.Mode.INPUT);
  72.  
  73.         trayServoR.setPosition(1);
  74.         trayServoL.setPosition(0);
  75.  
  76.         CLAW.setPosition(0);
  77.  
  78.         while ((!imu.isGyroCalibrated()) && !isStopRequested()) {
  79.  
  80.         }
  81.  
  82.         backLeftWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  83.         backRightWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  84.         frontLeftWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  85.         frontRightWheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
  86.  
  87.         linearLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  88.         linearLift2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
  89.  
  90.         //  linearLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  91.         //linearLift2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  92.  
  93.         telemetry.addData("Status:", "initialized");
  94.         telemetry.update();
  95.  
  96.         waitForStart();
  97.  
  98.         while (opModeIsActive()) {
  99.             if (gamepad2.dpad_up) {
  100.                 if (!upButton) {
  101.                     //startPosition = linearLift.getCurrentPosition();
  102.                     mockGoalLiftheight++;
  103.                     upButton = true;
  104.  
  105.                 } else {
  106.                 }
  107.  
  108.             } else {
  109.                 upButton = false;
  110.             }
  111.  
  112.             if (gamepad2.dpad_down) {
  113.                 if (!downButton) {
  114.                     // startPosition = linearLift.getCurrentPosition();
  115.                     mockGoalLiftheight -= 1;
  116.                     downButton = true;
  117.  
  118.                 } else {
  119.                 }
  120.  
  121.             } else {
  122.                 downButton = false;
  123.             }
  124.  
  125.             if (mockGoalLiftheight > 4) {
  126.                 mockGoalLiftheight = 4;
  127.             } else if (mockGoalLiftheight < 0) {
  128.                 mockGoalLiftheight = 0;
  129.             } else{
  130.                 mockGoalLiftheight = mockGoalLiftheight;
  131.             }
  132.  
  133.             if (mockGoalLiftheight == 0) {
  134.                 goalLiftHeight = 0;
  135.  
  136.             } else if (mockGoalLiftheight == 1) {
  137.                 goalLiftHeight = -560;
  138.  
  139.             } else if (mockGoalLiftheight == 2) {
  140.                 goalLiftHeight = -1120;
  141.  
  142.             } else if (mockGoalLiftheight == 3) {
  143.                 goalLiftHeight = -1680;
  144.  
  145.             } else if (mockGoalLiftheight == 4) {
  146.                 goalLiftHeight = -2240;
  147.  
  148.             }
  149.  
  150.             if (prevMockGoalLiftHeight > mockGoalLiftheight){
  151.                 goingUp = true;
  152.                 goingDown = false;
  153.             } else if (prevMockGoalLiftHeight < mockGoalLiftheight){
  154.                 goingDown = true;
  155.                 goingUp = false;
  156.             }
  157.  
  158.             if (goingUp){
  159.                 while (linearLift.getCurrentPosition() > goalLiftHeight){
  160.                     linearLift.setPower(-.7);
  161.                     linearLift2.setPower(-.7);
  162.                     telemetry.addLine("lifting");
  163.                 }
  164.  
  165.  
  166.             } else if (goingDown){
  167.  
  168.                 while (linearLift.getCurrentPosition() < goalLiftHeight){
  169.                     linearLift.setPower(.7);
  170.                     linearLift2.setPower(.7);
  171.                 }
  172.  
  173.             } else {
  174.                 linearLift.setPower(0);
  175.                 linearLift2.setPower(0);
  176.             }
  177.  
  178.             /**while (linearLift.getCurrentPosition() != goalLiftHeight){
  179.  
  180.                 linearLift.setTargetPosition(goalLiftHeight);
  181.                 linearLift2.setTargetPosition(goalLiftHeight);
  182.                 linearLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  183.                 linearLift2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  184.                 linearLift.setPower(.1);
  185.                 linearLift.setPower(.1);
  186.  
  187.                 telemetry.addLine("unbroken");
  188.  
  189.                 if (linearLift.getCurrentPosition() == goalLiftHeight){
  190.                     break;
  191.                 }
  192.  
  193.             }
  194.             linearLift.setPower(0);
  195.             linearLift2.setPower(0);
  196.  
  197.             telemetry.addLine("broken");
  198.  
  199.  
  200.  
  201.             /**linearLift.setTargetPosition(-1120);
  202.              linearLift2.setTargetPosition(-1120);
  203.              linearLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  204.              linearLift2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  205.              linearLift.setPower(-.7);
  206.              linearLift2.setPower(-.7);
  207.              while (linearLift.isBusy()){
  208.  
  209.              }
  210.              linearLift.setPower(0);
  211.              linearLift2.setPower(0);
  212.              */
  213.  
  214.                 telemetry.addData("linear lift height:", linearLift2.getCurrentPosition());
  215.                 telemetry.addData("linear lift height:", goalLiftHeight);
  216.                 telemetry.addData("mockGoalLiftHeight:", mockGoalLiftheight);
  217.                 telemetry.update();
  218.  
  219.                 prevMockGoalLiftHeight = mockGoalLiftheight;
  220.         }
  221.     }
  222.  
  223. }
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