FtcRobotController/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PIDSecondSlideTest.java at master · tanoom/FtcRobotController · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
package org.firstinspires.ftc.teamcode;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

@TeleOp(name = "PIDSecondSlideTest")
@Config
public class PIDSecondSlideTest extends LinearOpMode {
  private final MultipleTelemetry mTelemetry =
      new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
  private static double slideHeightTicks = 0;
  private DcMotorEx mIntakeLeft;
  private DcMotorEx mInakeRight;

  @Override
  public void runOpMode() throws InterruptedException {
    mIntakeLeft = hardwareMap.get(DcMotorEx.class, "intakeLeft");
    mInakeRight = hardwareMap.get(DcMotorEx.class, "intakeRight");

    mIntakeLeft.setDirection(DcMotorSimple.Direction.FORWARD);
    mInakeRight.setDirection(DcMotorSimple.Direction.REVERSE);

    mIntakeLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    mInakeRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);


    //        mFrontSlide.resetEncoder();
    //        mBackSlide.resetEncoder();
    //
    //        mFrontSlide.setPositionCoefficient(0.05);
    //        mBackSlide.setPositionCoefficient(0.05);
    //
    //        mFrontSlide.setPositionTolerance(10);
    //        mBackSlide.setPositionTolerance(10);
    //
    //        mFrontSlide.setRunMode(Motor.RunMode.PositionControl);
    //        mBackSlide.setRunMode(Motor.RunMode.PositionControl);

    waitForStart();
    while (opModeIsActive()) {

      if(gamepad1.a) {
        mIntakeLeft.setPower(1);
        mInakeRight.setPower(1);
      } else if (gamepad1.b) {
        mIntakeLeft.setPower(-1);
        mInakeRight.setPower(-1);
      }
      else {
        mIntakeLeft.setPower(0);
        mInakeRight.setPower(0);
      }
        //            mFrontSlide.setTargetDistance(slideHeightTicks);
        //            mBackSlide.setTargetDistance(slideHeightTicks);
        //
        //            mFrontSlide.set(0);
        //            mBackSlide.set(0);
        //
        //            if(!mFrontSlide.atTargetPosition()) {
        //                mFrontSlide.set(0.75);
        //            }
        //
        //            if(!mBackSlide.atTargetPosition()) {
        //                mBackSlide.set(0.75);
        //            }

        mTelemetry.update();
    }
  }
}