Esempio n. 1
0
 @Override
 public void loop() {
   if (this.time < 6.5d) {
     goForward(-1.0);
   } else if (this.time < 8.0d) {
     turnRight(1.0);
   } else if (this.time < 11d) {
     goForward(-1.0);
   } else if (this.time < 14d) {
     turnRight(1.0);
     armUp = true;
     motorArm.setPower(0.5);
     if (this.time > 11.2d) {
       motorArm.setPower(0.1);
     }
   } else if (this.time < 16) {
     goForward(1.0);
     motorSlide.setPower(1.0);
   } else if (this.time < 18) {
     stopMotors();
     motorSlide.setPower(0.0);
     forwardBack.setPosition(0.75);
   } else if (this.time < 22) {
     motorSlide.setPower(-1.0);
     forwardBack.setPosition(0.5);
   }
   telemetry.addData("Text", "*** Robot Data***");
   telemetry.addData("..", "time: " + String.format("%.2f", this.time));
 }
Esempio n. 2
0
  @Override
  public void init() {
    /* Assigns DcMotor objects to physical motors
    using Hardware Mapping */
    lbMotor = hardwareMap.dcMotor.get("lbMotor");
    lfMotor = hardwareMap.dcMotor.get("lfMotor");
    rfMotor = hardwareMap.dcMotor.get("rfMotor");
    rbMotor = hardwareMap.dcMotor.get("rbMotor");
    hangRotateMotor = hardwareMap.dcMotor.get("hangRotateMotor");
    hangSlideMotor = hardwareMap.dcMotor.get("hangSlideMotor");
    armMotor = hardwareMap.dcMotor.get("armMotor");

    /* Assigns Servo objects to physical servos
    using Hardware Mapping */
    climber = hardwareMap.servo.get("climber");
    rSwitch = hardwareMap.servo.get("rSwitch");
    lSwitch = hardwareMap.servo.get("lSwitch");

    /* resets Servo positions */
    climber.setPosition(CLIMBER_DOWN_POSITION);
    rSwitch.setPosition(RSWITCH_CLOSED_POSITION);
    lSwitch.setPosition(LSWITCH_CLOSED_POSITION);

    /* runs backward motors in opposite direction */
    lfMotor.setDirection(DcMotor.Direction.REVERSE);
    lbMotor.setDirection(DcMotor.Direction.REVERSE);

    telemetry.addData("init complete", "");
  }
  @Override
  public void init() {
    // Treads.
    rightTread = hardwareMap.dcMotor.get("rightTread");
    leftTread = hardwareMap.dcMotor.get("leftTread");

    leftTread.setDirection(DcMotor.Direction.REVERSE);

    rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
    leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);

    // Servos.
    rightPusher = hardwareMap.servo.get("rightPusher");
    leftPusher = hardwareMap.servo.get("leftPusher");
    rightBumper = hardwareMap.servo.get("rightBumper");
    leftBumper = hardwareMap.servo.get("leftBumper");
    climber = hardwareMap.servo.get("climber");

    rightPusher.setPosition(NeverlandServoConstants.RIGHT_PUSHER_STOWED);
    leftPusher.setPosition(NeverlandServoConstants.LEFT_PUSHER_STOWED);
    rightBumper.setPosition(NeverlandServoConstants.RIGHT_BUMPER_DOWN);
    leftBumper.setPosition(NeverlandServoConstants.LEFT_BUMPER_DOWN);
    climber.setPosition(NeverlandServoConstants.CLIMBER_STORE);

    // Beacon arms.
    BeaconArms beacon = new BeaconArms(rightPusher, leftPusher, true);
  }
 public void smoothDump(ElapsedTime timer) {
   moveStraight(8.5, false, .3);
   double pos = Keys.CLIMBER_INITIAL_STATE;
   // .85 to .31 so you want to decrement
   timer.reset();
   telemetry.addData("place", "before while");
   while (pos > Keys.CLIMBER_DUMP) {
     timer.reset();
     while (timer.time() * 1000 < 200) {
       // do nothing
       try {
         sleep(2);
       } catch (InterruptedException e) {
         e.printStackTrace();
       }
       telemetry.addData("waiting", timer.time() * 1000);
     }
     pos -= .05;
     climber.setPosition(pos);
     /*telemetry.addData("place","during while");
     telemetry.addData("timer",timer.time());
     telemetry.addData("timer math",((int)(timer.time()*1000)));
     telemetry.addData("timer math2",((int)(timer.time()*1000))%200);
     telemetry.addData("climber",climber.getPosition());
     telemetry.addData("constant","INIT"+Keys.CLIMBER_INITIAL_STATE+" DUMP"+Keys.CLIMBER_DUMP);*/
   }
   // once it is here, it finished dumping.
   // retract - the sudden should be ok cuz hopefully by that time it will have already dumped
   climber.setPosition(Keys.CLIMBER_INITIAL_STATE);
   moveStraight(8.5, true, .3);
   telemetry.addData("place", "after while");
 }
  @Override
  public void loop() {

    // Gamepad 1 - Drive Controller

    float rightDriveMotorSpeed = -gamepad1.right_stick_y;
    float leftDriveMotorSpeed = -gamepad1.left_stick_y;

    rightDriveMotorSpeed = Range.clip(rightDriveMotorSpeed, -1, 1);
    leftDriveMotorSpeed = Range.clip(leftDriveMotorSpeed, -1, 1);

    rightDriveMotorSpeed = (float) scaleInput(rightDriveMotorSpeed);
    leftDriveMotorSpeed = (float) scaleInput(leftDriveMotorSpeed);
    System.out.println(rightDriveMotorSpeed);

    driveRight.setPower(rightDriveMotorSpeed);
    driveLeft.setPower(leftDriveMotorSpeed);

    if (gamepad1.a & hookServoClick == 0) {
      hookServoPosition -= hookServoDelta;
      System.out.println(hookServoPosition);
      hookServoClick = 1;
      hookServo.setPosition(hookServoPosition);
    }

    if (gamepad1.a & hookServoClick == 1) {
      hookServoPosition += hookServoDelta;
      System.out.println(hookServoPosition);
      hookServoClick = 0;
      hookServo.setPosition(hookServoPosition);
    }

    if (gamepad1.x & triggerServoClick == 0) {
      triggerServoPosition -= triggerServoDelta;
      System.out.println(triggerServoPosition);
      triggerServoClick = 1;
      triggerServo.setPosition(triggerServoPosition);
    }

    if (gamepad1.x & triggerServoClick == 1) {
      triggerServoPosition += triggerServoDelta;
      System.out.println(triggerServoPosition);
      triggerServoClick = 0;
      triggerServo.setPosition(triggerServoPosition);
    }

    // Gamepad 2

    float armStage1MotorSpeed = -gamepad2.right_stick_y;
    float armStage2MotorSpeed = -gamepad2.left_stick_y;

    armStage1MotorSpeed = Range.clip(armStage1MotorSpeed, -1, 1);
    armStage2MotorSpeed = Range.clip(armStage2MotorSpeed, -1, 1);

    armStage1MotorSpeed = (float) scaleInput(armStage1MotorSpeed);
    armStage2MotorSpeed = (float) scaleInput(armStage2MotorSpeed);

    armStage1.setPower(armStage1MotorSpeed);
    armStage2.setPower(armStage2MotorSpeed);
  }
Esempio n. 6
0
  public void init(HardwareMap hardwareMap) {
    leftClimberServo = hardwareMap.servo.get("leftClimberServo");
    leftClimberServo.setPosition(leftInitialPosition);

    rightClimberServo = hardwareMap.servo.get("rightClimberServo");
    rightClimberServo.setPosition(rightInitialPosition);
  }
Esempio n. 7
0
  private void armDrive(double armPosition, long sleepAmount) throws InterruptedException {
    climbers.setPosition(armPosition); // set the arm power according to the double armPower

    sleep(sleepAmount); // sleep for a certain amount of milliseconds

    climbers.setPosition(1.0); // stop the motors
  }
  @Override
  public void init() {

    // Drive Motors
    driveRight = hardwareMap.dcMotor.get("driveRight");
    driveLeft = hardwareMap.dcMotor.get("driveLeft");

    // Arm Motors
    armStage1 = hardwareMap.dcMotor.get("armStage1");
    armStage2 = hardwareMap.dcMotor.get("armStage2");

    // Servos
    hookServo = hardwareMap.servo.get("hookServo");
    triggerServo = hardwareMap.servo.get("triggerServo");

    // Controllers
    driveController = hardwareMap.dcMotorController.get("driveController");
    armController = hardwareMap.dcMotorController.get("armController");
    servoController = hardwareMap.servoController.get("servoController");

    // sets servo Pos
    hookServoPosition = 0.2;
    triggerServoPosition = 0.2;

    hookServo.setPosition(hookServoPosition);
    triggerServo.setPosition(triggerServoPosition);
  }
  @Override
  public void loop() {
    double rightDrive;
    double leftDrive;

    rightDrive = -gamepad1.right_stick_y;
    leftDrive = -gamepad1.left_stick_y;
    if (gamepad1.dpad_up) {
      rightDrive = 0.3;
      leftDrive = 0.3;
    } else if (gamepad1.dpad_down) {
      rightDrive = -0.3;
      leftDrive = -0.3;
    } else if (gamepad1.dpad_right) {
      rightDrive = -0.3;
      leftDrive = 0.3;
    } else if (gamepad1.dpad_left) {
      rightDrive = 0.3;
      leftDrive = -0.3;
    }
    motorRight.setPower(rightDrive);
    motorLeft.setPower(leftDrive);

    if (gamepad1.a) {
      servoClimberDumper.setPosition(1.0);
    } else {
      servoClimberDumper.setPosition(0.0);
    }

    telemetry.addData("trackLifter", Integer.toString(trackLifter.getCurrentPosition()));
    telemetry.addData("liftCheck", liftCheck.isPressed());
    telemetry.addData("ENCLeft", Integer.toString(motorLeft.getCurrentPosition()));
    telemetry.addData("ENCRight", Integer.toString(motorRight.getCurrentPosition()));
    telemetry.addData("trigger", gamepad1.right_trigger);
  }
  /* Initialize standard Hardware interfaces */
  public void init(HardwareMap ahwMap) {
    // Save reference to Hardware map
    hwMap = ahwMap;

    // Define and Initialize Motors
    leftMotor = hwMap.dcMotor.get("left_drive");
    rightMotor = hwMap.dcMotor.get("right_drive");
    armMotor = hwMap.dcMotor.get("left_arm");
    leftMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
    rightMotor.setDirection(DcMotor.Direction.REVERSE); // Set to FORWARD if using AndyMark motors

    // Set all motors to zero power
    leftMotor.setPower(0);
    rightMotor.setPower(0);
    armMotor.setPower(0);

    // Set all motors to run without encoders.
    // May want to use RUN_USING_ENCODERS if encoders are installed.
    leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

    // Define and initialize ALL installed servos.
    leftClaw = hwMap.servo.get("left_hand");
    rightClaw = hwMap.servo.get("right_hand");
    leftClaw.setPosition(MID_SERVO);
    rightClaw.setPosition(MID_SERVO);
  }
Esempio n. 11
0
  /*
   * This method will be called repeatedly in a loop
   *
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
   */
  @Override
  public void loop() {
    double reflection = 0.0;
    double left, right = 0.0;

    // keep manipulator out of the way.
    arm.setPosition(armPosition);
    claw.setPosition(clawPosition);

    /*
     * Use the 'time' variable of this op mode to determine
     * how to adjust the motor power.
     */
    if (this.time <= 1) {
      // from 0 to 1 seconds, run the motors for five seconds.
      left = 0.15;
      right = 0.15;
    } else if (this.time > 5 && this.time <= 8.5) {
      // between 5 and 8.5 seconds, point turn right.
      left = 0.15;
      right = -0.15;
    } else if (this.time > 8.5 && this.time <= 15) {
      // between 8 and 15 seconds, idle.
      left = 0.0;
      right = 0.0;
    } else if (this.time > 15d && this.time <= 20.75d) {
      // between 15 and 20.75 seconds, point turn left.
      left = -0.15;
      right = 0.15;
    } else {
      // after 20.75 seconds, stop.
      left = 0.0;
      right = 0.0;
    }

    /*
     * set the motor power
     */
    motorRight.setPower(left);
    motorLeft.setPower(right);

    /*
     * read the light sensor.
     */
    // reflection = reflectedLight.getLightLevel();

    /*
     * Send telemetry data back to driver station. Note that if we are using
     * a legacy NXT-compatible motor controller, then the getPower() method
     * will return a null value. The legacy NXT-compatible motor controllers
     * are currently write only.
     */

    telemetry.addData("Text", "*** Robot Data***");
    telemetry.addData("time", "elapsed time: " + Double.toString(this.time));
    telemetry.addData("reflection", "reflection:  " + Double.toString(reflection));
    telemetry.addData("left tgt pwr", "left  pwr: " + Double.toString(left));
    telemetry.addData("right tgt pwr", "right pwr: " + Double.toString(right));
  }
Esempio n. 12
0
 public void operate(Servo ser, double position, long wait) {
   ser.setPosition(position);
   try {
     Thread.sleep(wait);
   } catch (Exception e) {
     System.err.println(e);
     System.exit(1);
   }
   ser.setPosition(0);
 }
Esempio n. 13
0
 // initialize servos
 public void init_servos() {
   //
   redNoodleServo = hardwareMap.servo.get("redNoodle");
   blueNoodleServo = hardwareMap.servo.get("blueNoodle");
   shoulderRatchet = hardwareMap.servo.get("shoulderRatchet");
   tapeRatchet = hardwareMap.servo.get("tapeRatchet");
   redNoodleServo.setPosition(redNoodleDown);
   blueNoodleServo.setPosition(blueNoodleDown);
   tapeRatchet.setPosition(tapeRatchetOpen);
   shoulderRatchet.setPosition(shoulderRatchetOpen);
 }
Esempio n. 14
0
  public void loop() {
    resetServos();
    moveSwiv();

    swivLeft.setPosition(leftSwivPos);
    swivRight.setPosition((rightSwivPos));
    updateGamepadTelemetry();
    float direction = gamepad1.right_stick_y;
    direction = Range.clip(direction, -1, 1);
    arm.setPosition(scaleContinuous(-gamepad1.right_stick_y));
  }
Esempio n. 15
0
  /*
   * Function to automatically move the hanging arm.
   * Should be called in a loop.
   * @return returns true when the arm is in the process of moving.
   */
  public boolean hangingAutomation() {
    double hang1Position = Math.max(srvoHang_1.getPosition() - HangServoDelta, HANG1_MIN_RANGE);

    srvoHang_1.setPosition(hang1Position);
    // when big servo is midway through it's travel from min to max...
    if (srvoHang_1.getPosition() < (HANG1_MAX_RANGE + HANG1_MIN_RANGE) / 2) {
      // tell the big servo to go to it's max range (as fast as possible)
      srvoHang_1.setPosition(HANG1_MAX_RANGE);
      // start the small servo to create a lunging effect
      srvoHang_2.setPosition(HANG2_MAX_RANGE);
      return false;
    }
    return true;
  }
Esempio n. 16
0
 /**
  * Moves right climber servo based on {@code enum rightClimberDirection} value set in {@link
  * DriverStation#getNextClimberCmd()}
  *
  * @param drvrcmd {@link DriverCommand} object with values.
  */
 public void applyDSCmd(DriverCommand drvrcmd) {
   switch (drvrcmd.rightClimberCmd.rightClimberDirection) {
     case DOWN:
       rightClimber.setPosition(0.5);
       break;
     case UP:
       rightClimber.setPosition(0);
       break;
     case NONE:
       break;
     default:
       break;
   }
 }
Esempio n. 17
0
  public void setPosition(Direction direction) {

    if (verticalPostiiton == Direction.MID) {
      switch (direction) {
        case UP:
          servoLeft.setPosition(leftUP);
          servoRight.setPosition(rightUP);
          break;
        case MID:
          servoLeft.setPosition(leftMID);
          servoRight.setPosition(rightMID);
          break;
        case DOWN:
          servoLeft.setPosition(leftDOWN);
          servoRight.setPosition(rightDOWN);
          break;
        default:
          throw new IllegalArgumentException("Direction must be UP, DOWN, MID!");
      }
      horizontalPosition = direction;
    } else {
      servoLeft.setPosition(leftMID);
      servoRight.setPosition(rightMID);
      horizontalPosition = Direction.MID;
    }
  }
 /*
  * handleServos
  *
  * Oscillate the servos.
  */
 protected void handleServos() {
   if ((firstServos) || (servoOscTimer.time() > SERVO_OSC_FREQ)) {
     if (servoPosition == 0.0) {
       servoPosition = 1.0;
     } else {
       servoPosition = 0.0;
     }
     servo1.setPosition(servoPosition);
     servo2.setPosition(servoPosition);
     servo3.setPosition(servoPosition);
     servo4.setPosition(servoPosition);
     servoOscTimer.reset();
     firstServos = false;
   }
 }
 @Override
 public void init() {
   motorHangL = hardwareMap.dcMotor.get("motorHangL");
   motorHangR = hardwareMap.dcMotor.get("motorHangR");
   motorSpinner = hardwareMap.dcMotor.get("manipulator");
   ramp = hardwareMap.servo.get("ramp");
   drop = hardwareMap.servo.get("drop");
   claw = hardwareMap.servo.get("claw");
   motorHangL.setPower(0);
   motorHangR.setPower(0);
   motorSpinner.setPower(0);
   boxBelt.setPosition(.5);
   ramp.setPosition(0);
   drop.setPosition(0);
 }
Esempio n. 20
0
 public Climber_Savers(LinearOpMode opMode) throws InterruptedException {
   this.opMode = opMode;
   opMode.telemetry.addData("ClimberSavers", "constructor");
   // get hardware mappings
   climberSaverServo = opMode.hardwareMap.servo.get("ClimberSaverServo");
   climberSaverServo.setPosition(STARTPOSITION);
 }
  /*
   * This method will be called repeatedly in a loop
   *
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
   */
  @Override
  public void loop() {

    /*
     * Gamepad 1
     *
     * Gamepad 1 controls the motors via the left stick, and it controls the
     * wrist/claw via the a,b, x, y buttons
     */

    // throttle: left_stick_y ranges from -1 to 1, where -1 is full up, and
    // 1 is full down
    // direction: left_stick_x ranges from -1 to 1, where -1 is full left
    // and 1 is full right
    float left_throttle = -gamepad1.left_stick_y;
    float right_throttle = -gamepad1.right_stick_y;

    // scale the joystick value to make it easier to control
    // the robot more precisely at slower speeds.
    left_throttle = (float) scaleInput(left_throttle);
    right_throttle = (float) scaleInput(right_throttle);

    // write the values to the motors
    front_right.setPower(right_throttle);
    back_right.setPower(right_throttle);
    front_left.setPower(left_throttle);
    back_left.setPower(left_throttle);

    // update the position of the arm.
    if (gamepad1.y) {
      // if the A button is pushed on gamepad1, increment the position of
      // the arm servo.
      spinnerPosition += spinnerDelta;
    }

    if (gamepad1.a) {
      // if the Y button is pushed on gamepad1, decrease the position of
      // the arm servo.
      spinnerPosition -= spinnerDelta;
    }

    // clip the position values so that they never exceed their allowed range.
    spinnerPosition = Range.clip(spinnerPosition, 0.0, 1.0);

    // write position values to the wrist and claw servo
    spinner.setPosition(spinnerPosition);

    /*
     * Send telemetry data back to driver station. Note that if we are using
     * a legacy NXT-compatible motor controller, then the getPower() method
     * will return a null value. The legacy NXT-compatible motor controllers
     * are currently write only.
     */
    telemetry.addData("Text", "*** Robot Data***");
    telemetry.addData("arm", "arm:  " + String.format("%.2f", armPosition));
    telemetry.addData("claw", "claw:  " + String.format("%.2f", clawPosition));
    telemetry.addData("left tgt pwr", "left  pwr: " + String.format("%.2f", left));
    telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
  }
Esempio n. 22
0
 public void moveVertical(Direction direction) {
   switch (direction) {
     case UP:
       setPosition(Direction.MID);
       servoVertical.setPosition(verticalUP);
       break;
     case MID:
       servoVertical.setPosition(verticalMID);
       break;
     case DOWN:
       setPosition(Direction.MID);
       servoVertical.setPosition(verticalDOWN);
       break;
     default:
       throw new IllegalArgumentException("Direction must be UP, DOWN, MID!");
   }
   verticalPostiiton = direction;
 }
Esempio n. 23
0
  @Override
  public void runOpMode() throws InterruptedException {
    initMotors();
    waitForStart();
    // Autonomous starts here

    // Wait for motors to calibrate
    Thread.sleep(1000);

    // Drive to floorgoal, backwards
    MotorRunner.run(
        this,
        new DcMotor[] {motorLeft, motorRight},
        -Power.FULL_SPEED,
        new TimeUnit(Values.DRIVE_FLOORGOAL));
    // Turn to align with floor goal
    MotorRunner.run(this, motorRight, Power.FULL_SPEED, new TimeUnit(Values.TURN_FLUSH));
    // Attempt to Deliver Climbers
    dump.setPosition(Values.DUMP_DOWN);
    Thread.sleep(1000);
    dump.setPosition(Values.DUMP_UP);

    // Turn to align with mountain
    motorLeft.setPower(-Power.FULL_SPEED);
    MotorRunner.run(this, motorRight, Power.FULL_SPEED, new TimeUnit(Values.TURN_MOUNTAIN));
    motorRight.setPower(0);

    // Drive up mountain
    MotorRunner.run(
        this,
        new DcMotor[] {motorLeft, motorRight},
        Power.FULL_SPEED,
        new TimeUnit(Values.DRIVE_MOUNTAIN));

    // Try to grab a churro
    MotorRunner.run(this, tape1, Power.SLOW_SPEED, new TimeUnit(Values.SEND_TAPE));
    motorLeft.setPower(Power.FULL_SPEED);
    motorRight.setPower(Power.FULL_SPEED);
    for (int i = 0; i < Values.TAPE_TIMES; i++) {
      MotorRunner.run(this, tape1, -Power.NORMAL_SPEED, new TimeUnit(Values.RETRACT_TAPE));
      Thread.sleep(250);
    }
    stopMotors();
  }
Esempio n. 24
0
 /**
  * Moves right climber servo based on {@code enum rightClimberDirection} value set in {@link
  * DriverStation#getNextClimberCmd()}
  *
  * @param drvrcmd {@link DriverCommand} object with values.
  */
 public void applyDSCmd(DriverCommand drvrcmd) {
   if (!rightClimberAvailable) {
     return;
   }
   double rightClimberPosition = rightClimber.getPosition();
   DbgLog.msg(String.format("RightClimber Position = %f", rightClimberPosition));
   switch (drvrcmd.rightClimberCmd.rightClimberStatus) {
     case -1:
       rightClimber.setPosition(Range.clip(0, 0, 1));
       break;
     case 1:
       rightClimber.setPosition(Range.clip(1, 0, 1));
       break;
     case 0:
       break;
     default:
       break;
   }
 }
Esempio n. 25
0
 /*
  * Code to run when the op mode is first enabled goes here
  *
  * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
  */
 @Override
 public void init() {
   // Setup motor and reverse direction of left motor
   motorRight = hardwareMap.dcMotor.get("motor_3");
   motorLeft = hardwareMap.dcMotor.get("motor_4");
   motorLeft.setDirection(DcMotor.Direction.REVERSE);
   motorArm = hardwareMap.dcMotor.get("motor_1");
   motorSlide = hardwareMap.dcMotor.get("motor_2");
   rightLeft = hardwareMap.servo.get("servo_3");
   forwardBack = hardwareMap.servo.get("servo_4");
   clawGrab = hardwareMap.servo.get("servo_5");
   grabPos = clawGrab.getPosition();
   while (grabPos < 0.9) {
     grabPos += 0.1;
     clawGrab.setPosition(grabPos);
   }
   rightLeft.setPosition(0.2);
   forwardBack.setPosition(0.20);
   new ArmStopper().start();
 }
Esempio n. 26
0
  @Override
  public void loop() {
    telemetry.addData("!WARNING! ", "!DANGEROUS SERVO TESTING! - " + mode);
    changeMode();
    theDumper.setPosition(Range.clip(lDrivePower, 0.0, 1.0));
    if (!mode) {
      lDrivePower -= 0.01;
    }

    if (mode) {
      lDrivePower += 0.01;
    }
  }
Esempio n. 27
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. 28
0
  public void loop() {
    arm.setPower(1);
    arm.setTargetPosition(3500);
    if (getRuntime() > 5) {
      revMotor.setPower(0 - 1 * (1.3 / 3));
      if (getRuntime() > 10) launcher.setPosition(0);
    }
    if (getRuntime() > 11) {
      launcher.setPosition(1);
      revMotor.setPower(0);
    }
    if ((getRuntime() > 12) && (getRuntime() < 14)) {
      arm.setTargetPosition(0);
      arm.setPower(-1);
      motorBackLeft.setPower(-1);
      motorFrontLeft.setPower(0.6);
      motorBackRight.setPower(1);
      motorBackRight.setPower(-0.6);
      telemetry.addData("I'm doing it", arm.isBusy());
    }

    telemetry.addData("amIBusy()", arm.isBusy());
    telemetry.update();
  }
  @Override
  public void init() {
    // Ints hardware
    motorRight = hardwareMap.dcMotor.get("motorR");
    motorLeft = hardwareMap.dcMotor.get("motorL");
    trackLifter = hardwareMap.dcMotor.get("trkLftr");
    armExtend = hardwareMap.dcMotor.get("armExtend");

    // servos
    // servoBeaconPusher = hardwareMap.servo.get("beacon_pusher");
    servoClimberDumper = hardwareMap.servo.get("climber_dumper");
    //        climberDoor = hardwareMap.servo.get("climberDoor");
    servoDist = hardwareMap.servo.get("servoDist");
    climberTriggerLeft = hardwareMap.servo.get("trigLeft");
    climberTriggerRight = hardwareMap.servo.get("trigRight");
    cowCatcher = hardwareMap.servo.get("cowCatcher");
    armLock = hardwareMap.servo.get("armLock");
    trackLock = hardwareMap.servo.get("trackLock");
    // zipTieSweeper = hardwareMap.servo.get("zipTieSweeper");
    ColorSense = hardwareMap.colorSensor.get("color");
    nearMountainSwitch = hardwareMap.digitalChannel.get("nearMtnSw");
    redBlueSwitch = hardwareMap.digitalChannel.get("rbSw");
    ultraSense = hardwareMap.ultrasonicSensor.get("ultraSense");
    gyroSense = hardwareMap.gyroSensor.get("gyro");
    liftCheck = hardwareMap.touchSensor.get("liftCheck");
    servoClimberDumper.setPosition(0.0);
    // WHATEVER WE ARE GOING TO CALL THIS THING =
    // hardwareMap.opticalDistanceSensor.get("STUFFANDTHINGS");
    nearMountainFlag = nearMountainSwitch.getState();
    // set up motors
    motorLeft.setDirection(DcMotor.Direction.FORWARD);
    motorRight.setDirection(DcMotor.Direction.REVERSE);
    trackLifter.setDirection(DcMotor.Direction.REVERSE);
    armExtend.setDirection(DcMotor.Direction.REVERSE);
    motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
    motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
    trackLifter.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    armExtend.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
    cowCatcher.setPosition(0.1);
    climberTriggerLeft.setPosition(0.8);
    climberTriggerRight.setPosition(0.0);
    armLock.setPosition(0.5);
    servoDist.setPosition(0.5);
    trackLock.setPosition(0.45);
  }
  /**
   * Initializes hardware devices and ties them to the hardware map. Also sets starting directions
   * and initialization positions for servos and motors. Initializes a new Simulacrum object.
   *
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode
   */
  @SuppressLint("SimpleDateFormat")
  public void init() {

    leftWheels = hardwareMap.dcMotor.get("motor_2");
    rightWheels = hardwareMap.dcMotor.get("motor_1");
    armExtender2 = hardwareMap.dcMotor.get("motor_6");
    hangMotor = hardwareMap.dcMotor.get("motor_4");
    liftMotor = hardwareMap.dcMotor.get("motor_3");
    armExtender = hardwareMap.dcMotor.get("motor_5");

    leftBucket = hardwareMap.servo.get("servo_1");
    rightBucket = hardwareMap.servo.get("servo_2");
    collectorDrive = hardwareMap.servo.get("servo_3");
    collectorDrive2 = hardwareMap.servo.get("servo_4");
    climberCarrierServo = hardwareMap.servo.get("servo_5");
    leftClimberServo = hardwareMap.servo.get("servo_6");
    rightClimberServo = hardwareMap.servo.get("servo_7");

    leftWheels.setDirection(DcMotor.Direction.REVERSE);
    armExtender.setDirection(DcMotor.Direction.REVERSE);

    leftBucket.setDirection(Servo.Direction.REVERSE);
    rightBucket.setDirection(Servo.Direction.FORWARD);

    leftClimberServo.setDirection(Servo.Direction.REVERSE);
    rightClimberServo.setDirection(Servo.Direction.FORWARD);

    leftBucket.setPosition(0.5);
    rightBucket.setPosition(0.5);
    collectorDrive.setPosition(0.5);
    collectorDrive2.setPosition(0.5);

    rightClimberServo.setPosition(0.05);
    leftClimberServo.setPosition(0.05);

    climberCarrierServo.setPosition(1.0);

    try {
      readWriteObj.writeInit();
    } catch (IOException e) {
      e.printStackTrace();
      telemetry.addData("Fatal Error", "IOException @ Init");
    }
  }