@Override
  public void loop() {
    telemetry.addData("encoders", motor1.getCurrentPosition());
    telemetry.addData("x", x);
    switch (x) {
      case 0:
        motor1.setPower(0.2);
        motor1.setTargetPosition(1120);
        motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

        if (motor1.getCurrentPosition() < 1125 && motor1.getCurrentPosition() > 1115) {
          motor1.setPower(0.0);
          x = 1;
          motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
        }
        break;
      case 1:
        motor1.setPower(0.1);
        motor1.setTargetPosition(2400);
        motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

        if (motor1.getCurrentPosition() < 2405 && motor1.getCurrentPosition() > 2395) {
          motor1.setPower(0.0);
          x = 2;
          motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
        }
    }
  }
  public void runOpMode() throws InterruptedException {

    leftDrive = hardwareMap.dcMotor.get("leftDrive");
    rightDrive = hardwareMap.dcMotor.get("rightDrive");
    leftDriveB = hardwareMap.dcMotor.get("leftDriveB");
    rightDriveB = hardwareMap.dcMotor.get("rightDriveB");
    climberServo = hardwareMap.servo.get("climberServo");

    rightDrive.setDirection(DcMotor.Direction.REVERSE);
    rightDriveB.setDirection(DcMotor.Direction.REVERSE);

    leftDrive.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);
    rightDrive.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);
    leftDriveB.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);
    rightDriveB.setChannelMode(DcMotor.RunModeController.RUN_TO_POSITION);

    waitForStart();

    drive(-0.5, -0.5, 2.4);
    // drive(0.5, -0.5, 0.7);
    // drive(-0.5, -0.5, 3);
    turn(RIGHT, 45);
    // drive(-0.5, -0.5, 1);
    // climberServo.setPosition(0.7);
    // Thread.sleep((long)(0.5));
    // climberServo.setPosition(0.5);

  }
  @Override
  public void init() {
    leftMotor = hardwareMap.dcMotor.get("left_drive");
    rightMotor = hardwareMap.dcMotor.get("right_drive");

    navx_device =
        AHRS.getInstance(
            hardwareMap.deviceInterfaceModule.get("dim"),
            NAVX_DIM_I2C_PORT,
            AHRS.DeviceDataType.kProcessedData,
            NAVX_DEVICE_UPDATE_RATE_HZ);

    rightMotor.setDirection(DcMotor.Direction.REVERSE);

    /* If possible, use encoders when driving, as it results in more */
    /* predictable drive system response.                           */
    leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);

    /* Create a PID Controller which uses the Yaw Angle as input. */
    yawPIDController =
        new navXPIDController(navx_device, navXPIDController.navXTimestampedDataSource.YAW);

    /* Configure the PID controller */
    yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
    yawPIDController.setContinuous(true);
    yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
    yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
    yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
    yawPIDController.enable(true);

    first_iteration = true;
  }
  @Override
  public void init_loop() {
    // Reverses the Right Wheel
    driveRight.setDirection(DcMotor.Direction.REVERSE);

    // Sets motors to run without incoders
    driveLeft.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
    driveRight.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
  }
Esempio n. 5
0
  /*
     Constructor method that initializes most of the variables and objects
     @param  left            the left side motors
     @param  right           the right side motors
     @param  WheelDiameter   the diameter of the wheel
     @param  GearRatio       the gear ratio
  */
  public MovementAuto(DcMotor left, DcMotor right, int WheelDiameter, double GearRatio) {
    leftMotor = left;
    rightMotor = right;
    wheelDiameter = WheelDiameter;
    gearRatio = GearRatio;

    rightMotor.setDirection(DcMotor.Direction.REVERSE);

    leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
    rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
  }
  /** if val is true, enables encoders, else disables them */
  public void enableEncoders(boolean val) {
    if (val) {
      leftMotorf.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
      rightMotorf.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
      encodersEnabled = true;
    } else {
      leftMotorf.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
      rightMotorf.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);

      encodersEnabled = false;
    }
  }
Esempio n. 7
0
  @Override
  public void start() {

    motorRight.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
    motorLeft.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
    //  currentState = 1;

    telemetry.addData("Text", "***Starting***");

    motorRight.setTargetPosition(5000);
    motorRight.setPower(.75);
    //   motorLeft.setTargetPosition(2500);
    motorLeft.setPower(.75);
  }
    private void a() {
      Iterator var1 = this.hardwareMap.servoController.iterator();

      while (var1.hasNext()) {
        ServoController var2 = (ServoController) var1.next();
        var2.pwmDisable();
      }

      var1 = this.hardwareMap.dcMotorController.iterator();

      while (var1.hasNext()) {
        DcMotorController var3 = (DcMotorController) var1.next();
        var3.setMotorControllerDeviceMode(DeviceMode.WRITE_ONLY);
      }

      var1 = this.hardwareMap.dcMotor.iterator();

      while (var1.hasNext()) {
        DcMotor var4 = (DcMotor) var1.next();
        var4.setPower(0.0D);
        var4.setChannelMode(RunMode.RUN_WITHOUT_ENCODERS);
      }

      var1 = this.hardwareMap.lightSensor.iterator();

      while (var1.hasNext()) {
        LightSensor var5 = (LightSensor) var1.next();
        var5.enableLed(false);
      }
    }
Esempio n. 9
0
 public void resetEncoders() {
   motor_1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   motor_2.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   motor_3.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   motor_4.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   motor_1.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
   motor_2.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
   motor_3.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
   motor_4.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
 }
Esempio n. 10
0
  @Override
  public void init() {
    // Get hardware references
    leftBackMotor = hardwareMap.dcMotor.get("back_left_drive");
    leftFrontMotor = hardwareMap.dcMotor.get("front_left_drive");
    rightBackMotor = hardwareMap.dcMotor.get("back_right_motor");
    rightFrontMotor = hardwareMap.dcMotor.get("front_right_motor");
    leftController = hardwareMap.dcMotorController.get("right_controller");
    rightController = hardwareMap.dcMotorController.get("left_controller");

    // Reverses right motors
    rightBackMotor.setDirection(DcMotor.Direction.REVERSE);
    leftFrontMotor.setDirection(DcMotor.Direction.REVERSE);
    leftBackMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    leftFrontMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    rightBackMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    rightFrontMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
  }
Esempio n. 11
0
  /*
     reverseDriveAuto sets the motors to drive in reverse to the target location
     @param distance     distance to travel in inches
  */
  public void reverseDriveAuto(double distance) {
    double counts = ENCODER_CPR * (distance / (Math.PI * wheelDiameter)) * gearRatio;

    leftMotor.setTargetPosition((int) counts);
    rightMotor.setTargetPosition((int) counts);

    if (leftMotor.getCurrentPosition() != leftMotor.getTargetPosition()) {

      leftMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
      rightMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

      reverseDrive(leftMotor, rightMotor, POWER);

    } else {
      leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
      rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
    }
  }
Esempio n. 12
0
  /*
     forwardDriveAuto method that calculates the distance from inches to encoder counts and then
     sets the motors to drive to the target location
     @param distance     distance to travel in inches
  */
  public void forwardDriveAuto(double distance) {

    double counts = ENCODER_CPR * (distance / (Math.PI * wheelDiameter)) * gearRatio;

    leftMotor.setTargetPosition((int) counts);
    rightMotor.setTargetPosition((int) counts);

    if (leftMotor.getCurrentPosition()
        != leftMotor
            .getTargetPosition()) { // Resets the encoders after the position has been reached

      leftMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
      rightMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

      forwardDrive(leftMotor, rightMotor, POWER);

    } else {
      leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
      rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
    }
  }
Esempio n. 13
0
  @Override
  public void init() {

    /*
     * Use the hardwareMap to get the dc motors and servos by name. Note
     * that the names of the devices must match the names used when you
     * configured your robot and created the configuration file.
     */

    motor_1 = hardwareMap.dcMotor.get("motor_1"); // right side
    motor_2 = hardwareMap.dcMotor.get("motor_2"); // right side
    motor_3 = hardwareMap.dcMotor.get("motor_3"); // left side
    motor_4 = hardwareMap.dcMotor.get("motor_4"); // left side
    motor_harvest = hardwareMap.dcMotor.get("motor_harvest");
    motor_harvest2 = hardwareMap.dcMotor.get("motor_harvest2");
    servo_1 = hardwareMap.servo.get("servo_1");
    servo_2 = hardwareMap.servo.get("servo_2");

    motor_1.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    motor_2.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    motor_3.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    motor_4.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    motor_harvest.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    motor_harvest2.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
  }
Esempio n. 14
0
  @Override
  public void init() {
    // currentState = 1;
    knockerRight = hardwareMap.servo.get("knockerRight"); // servo_3
    knockerLeft = hardwareMap.servo.get("knockerLeft"); // servo_2
    backClimber = hardwareMap.servo.get("backClimber"); // servo_4

    // sweeper = hardwareMap.dcMotor.get("motor_5");
    hook = hardwareMap.dcMotor.get("hookMotor"); // motor_3
    winch = hardwareMap.dcMotor.get("winchMotor"); // motor_4
    motorLeft = hardwareMap.dcMotor.get("leftMotor"); // motor_2
    motorRight = hardwareMap.dcMotor.get("rightMotor"); // motor_1

    // physical motors are mounted 180 from each other to both face out to wheels.
    // So, to have robot go forward, one motor needs to be reversed.
    motorLeft.setDirection(DcMotor.Direction.REVERSE);

    // 1/21/16 - adding (motor mode) Reset Encoders before start loop.
    motorRight.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
    motorLeft.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);

    telemetry.addData("Text", "***RESETTING***");

    //  motorRight.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
    //  motorLeft.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

    motorRight.setTargetPosition(0);
    motorRight.setPower(0.0);
    motorLeft.setTargetPosition(0);
    motorLeft.setPower(0.0);

    knockerLeft.setPosition(.52);
    knockerRight.setPosition(.52);
    backClimber.setPosition(.52);
    // REMEMBER: 0.0 to .5 is forward, while .5 to 1.0 is backwards.

  }
Esempio n. 15
0
 public void resetEncoders() {
   rf.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   rb.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   lf.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   lb.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
 }
Esempio n. 16
0
  @Override
  public void runOpMode() {

    /*
     * Use the hardwareMap to get the dc motors and servos by name. Note
     * that the names of the devices must match the names used when you
     * configured your robot and created the configuration file.
     */

    rf = hardwareMap.dcMotor.get("rf"); // right
    rb = hardwareMap.dcMotor.get("rb");
    lf = hardwareMap.dcMotor.get("lf"); // left
    lb = hardwareMap.dcMotor.get("lb");
    lift1 = hardwareMap.dcMotor.get("lift"); // RIGHT lift
    lift2 = hardwareMap.dcMotor.get("lift2"); // LEFT lift
    harvester = hardwareMap.dcMotor.get("harvester");
    retractor = hardwareMap.dcMotor.get("retractor");
    harvester.setChannelMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
    lift2.setDirection(DcMotor.Direction.REVERSE);
    basket = hardwareMap.servo.get("basket"); // basket servo
    door = hardwareMap.servo.get("door");

    // waitForStart();

    // Straight 4 tiles in order to line up with the climber box
    rb.setPower(1.0);
    rf.setPower(1.0);
    lf.setPower(-1.0);
    lb.setPower(-1.0);
    while (rf.getCurrentPosition() < 5040 && lf.getCurrentPosition() > -5040) {}
    rf.setPower(0.0);
    rb.setPower(0.0);
    lf.setPower(0.0);
    lb.setPower(0.0);
    resetEncoders();

    // turn left to face away from basket
    rb.setPower(1.0);
    rf.setPower(1.0);
    lf.setPower(1.0);
    lb.setPower(1.0);
    while (rf.getCurrentPosition() < 840 && lf.getCurrentPosition() < 840) {}
    rf.setPower(0.0);
    rb.setPower(0.0);
    lf.setPower(0.0);
    lb.setPower(0.0);
    resetEncoders();

    // Straight 3 tiles in order to go to climber box
    rf.setPower(-1.0);
    rb.setPower(-1.0);
    lf.setPower(1.0);
    lb.setPower(1.0);
    while (rf.getCurrentPosition() < 3360 && lf.getCurrentPosition() > -3360) {}
    rf.setPower(0.0);
    rb.setPower(0.0);
    lf.setPower(0.0);
    lb.setPower(0.0);
    resetEncoders();

    // lift basket towards box
    lift1.setPower(0.7);
    lift2.setPower(0.7);
    retractor.setPower(0.7);
    door.setPosition(0.8);
    while (lift1.getCurrentPosition() < 2800 && lift2.getCurrentPosition() < 2800) {}
    lift1.setPower(0.0);
    lift2.setPower(0.0);
    retractor.setPower(0.0);
    // turn basket to score
    basket.setPosition(0.8);
    door.setPosition(0.1);
    while (basket.getPosition() != 90) {}
    try {
      wait(3000L);
    } catch (InterruptedException e) {
    }
    basket.setPosition(0);
    while (basket.getPosition() != 0) {}

    // lower slides
    lift1.setPower(-0.5);
    lift2.setPower(-0.5);
    retractor.setPower(-0.5);
    door.setPosition(0.3);
    while (lift1.getCurrentPosition() > 0 && lift2.getCurrentPosition() > 0) {}
    lift1.setPower(0.0);
    lift2.setPower(0.0);
    retractor.setPower(0.0);

    // head straight 3 to the opposite ramp
    rf.setPower(1.0);
    rb.setPower(1.0);
    lf.setPower(-1.0);
    lb.setPower(-1.0);
    while (rf.getCurrentPosition() < 2940 && lf.getCurrentPosition() > -2940) {}
    rf.setPower(0.0);
    rb.setPower(0.0);
    lf.setPower(0.0);
    lb.setPower(0.0);
    resetEncoders();

    // turn left to face away from ramp
    rf.setPower(1.0);
    rb.setPower(1.0);
    lf.setPower(1.0);
    lb.setPower(1.0);
    while (rf.getCurrentPosition() < 1260 && lf.getCurrentPosition() < 1260) {}
    rf.setPower(0.0);
    rb.setPower(0.0);
    lf.setPower(0.0);
    lb.setPower(0.0);
    resetEncoders();

    // GO ONTO THE RAMP
    rf.setPower(-1.0);
    rb.setPower(-1.0);
    lf.setPower(1.0);
    lb.setPower(1.0);
    while (rf.getCurrentPosition() > -2940 && lb.getCurrentPosition() < 2940) {}
    rf.setPower(0.0);
    rb.setPower(0.0);
    lf.setPower(0.0);
    lb.setPower(0.0);
    resetEncoders();

    try {
      wait(30000L);
    } catch (InterruptedException e) {
    }
  }
Esempio n. 17
0
 public void resetEncoders() {
   motorBR.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   motorBL.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   motorFR.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
   motorFL.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
 }
 @Override
 public void init() {
   motor1 = hardwareMap.dcMotor.get("motor_2");
   motor1.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
 }