@Override
  public void loop() {

    ElapsedTime timer = new ElapsedTime();
    double timeLimit = 3;

    /*		if (gamepad1.dpad_up) {
    	robot.barGrabberServo.goHome();
    }

    if (gamepad1.dpad_down) {
    	robot.barGrabberServo.goGrabBar();
    }

          if (gamepad1.dpad_up) {
              // go at servo direct
          }

          if (gamepad1.dpad_down) {
              // go at servo direct
          }*/
    for (int i = 0; i < 11; i++) {
      if (timer.time() > timeLimit) {
        timer.reset();
        robot.leftZipLineServo.setPosition(i / 10);
      }
    }

    telemetry.addData(
        "servo", "positiom " + String.format("%.2f", robot.leftZipLineServo.getPosition()));
  }
Beispiel #2
0
  public void moveForwardToWall(double pow, int timeout) throws InterruptedException {
    double angle = Math.abs(sensor.getGyroYaw());
    double startAngle = angle;

    double power = pow;

    ElapsedTime time = new ElapsedTime();
    time.startTime();
    while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
      angle = Math.abs(sensor.getGyroYaw());
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      if (angle < startAngle - 1) {
        startMotors((power * .75), power);
      } else if (angle > startAngle + 1) {
        startMotors(power, (power * .75));
      } else {
        startMotors(power, power);
      }

      opMode.idle();
    }

    stopMotors();

    angleError = sensor.getGyroYaw();
    opMode.telemetry.update();
  }
 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");
 }
Beispiel #4
0
  public void moveFowardToLine(double ri, double le, int timeout) throws InterruptedException {
    double angle;
    double startAngle = Math.abs(sensor.getGyroYaw());
    opMode.telemetry.update();

    setNullValue();
    ElapsedTime time = new ElapsedTime();
    time.startTime();

    while (!sensor.isLeftLine() && time.milliseconds() < timeout) {
      angle = Math.abs(sensor.getGyroYaw());

      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      //            if(angle < startAngle - 2) {
      //                startMotors((power * .75), power);
      //            } else if(angle > startAngle + 2) {
      //                startMotors(power, (power * .75));
      //            } else {
      //                startMotors(power, power);
      //            }

      startMotors(ri, le);
      opMode.idle();
    }
    stopMotors();
    opMode.telemetry.update();
    angleError = sensor.getGyroYaw();
  }
  /*
   * Code to run when the op mode is first enabled goes here
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
   */
  @Override
  public void init_loop() {
    startDate = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(new Date());
    runtime.reset();
    telemetry.addData("Null Op Init Loop", runtime.toString());

    range = hardwareMap.analogInput.get("range");
  }
 /*
  * handleBattery
  *
  * The Matrix controller has a separate battery whose voltage can be read.
  */
 protected void handleBattery() {
   if ((firstBattery) || (spamPrevention.time() > SPAM_PREVENTION_FREQ)) {
     battery = mc.getBattery();
     spamPrevention.reset();
     firstBattery = false;
   }
   telemetry.addData("Battery: ", ((float) battery / 1000));
 }
  /**
   * *
   *
   * <p>waitForTick implements a periodic delay. However, this acts like a metronome with a regular
   * periodic tick. This is used to compensate for varying processing times for each cycle. The
   * function looks at the elapsed cycle time, and sleeps for the remaining time interval.
   *
   * @param periodMs Length of wait cycle in mSec.
   * @throws InterruptedException
   */
  public void waitForTick(long periodMs) throws InterruptedException {

    long remaining = periodMs - (long) period.milliseconds();

    // sleep for the remaining portion of the regular cycle period.
    if (remaining > 0) Thread.sleep(remaining);

    // Reset the cycle clock for the next pass.
    period.reset();
  }
 /*
  * 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;
   }
 }
  /*
   * Code to run ONCE when the driver hits PLAY
   */
  @Override
  public void start() {
    runtime.reset();

    // @todo add all one time 'get ready to run' here

  }
  @Override
  public boolean timeslice() {
    if (timer.time() >= timeout) {
      new SingleShotTimerEvent(this, EventKind.EXPIRED).postEvent();

      return true; // Task is done, will be deleted.
    } else return false; // Task needs to keep running.
  }
Beispiel #11
0
  public void moveForwardUntilZero(double pow, double timeout) throws InterruptedException {

    double angle = sensor.getGyroYaw();

    ElapsedTime time = new ElapsedTime();
    time.startTime();
    while (Math.abs(angle) > 2 && time.milliseconds() < timeout) {
      startMotors(-.3, -.5);

      opMode.telemetry.addData("current", "zeroout");
      opMode.telemetry.update();

      opMode.idle();
    }

    stopMotors();
  }
Beispiel #12
0
  public void moveForwardToWallEnc(double pow, int timeout) throws InterruptedException {
    double angle = Math.abs(sensor.getGyroYaw());
    double startAngle = angle;

    double power = pow;

    int tick = 1;

    resetEncoders();

    int startEncoder = Math.abs(motorFL.getCurrentPosition());

    int endEncoder;

    ElapsedTime time = new ElapsedTime();
    time.startTime();
    while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
      angle = Math.abs(sensor.getGyroYaw());
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      if (time.milliseconds() > 250 * tick) {
        endEncoder = Math.abs(motorFL.getCurrentPosition());
        if (startEncoder + 250 < endEncoder) {
          break;
        }
      }

      if (angle < startAngle - 1) {
        startMotors((power * .75), power);
      } else if (angle > startAngle + 1) {
        startMotors(power, (power * .75));
      } else {
        startMotors(power, power);
      }

      opMode.idle();
    }

    stopMotors();

    angleError = sensor.getGyroYaw();
    opMode.telemetry.update();
  }
  /*
   * handleMotors
   *
   * Oscillate the motors.
   */
  protected void handleMotors() {
    if ((firstMotors) || (motorOscTimer.time() > MOTOR_OSC_FREQ)) {
      motorPower = -motorPower;

      /*
       * The MatrixDcMotorController's setMotorPower() method may take
       * a collection of motors.  If this is chosen, then the controller will
       * set a pending bit.  The pending bit tells the controller to
       * defer turning on, or changing the current set point, for a motor
       * until the pending bit is cleared.
       *
       * When the pending bit is cleared all motor power values are applied
       * simultaneously.  setMotorPower() handles the pending bit for you.
       */
      mc.setMotorPower(motorSet, motorPower);
      motorOscTimer.reset();
      firstMotors = false;
    }
  }
  /*
   * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
   */
  @Override
  public void loop() {

    if (debugmode) telemetry.addData("Status", "Running: " + runtime.toString());

    handleControls(); // function to read all input controls and set globals here
    handleDrivetrain(); //  function to handle drivetrain changes here
    handleFeatures(); //  function to handle auxillary hardware features here
    telemetry.update(); //  needed to ensure that driver station is updated, but udpate
    //  liomited by SDK to only four times per second
  }
  /**
   * Update the angles using the raw readings.
   *
   * @param rawX raw rotation along x axis
   * @param rawY raw rotation along y axis
   * @param rawZ raw rotation along z axis
   */
  public void update(int rawX, int rawY, int rawZ) {
    if (stopWatch == null) {
      // first time?  initialize the stop watch
      stopWatch = new ElapsedTime();
    } else {
      // integrate values
      double elapsed = stopWatch.time();

      pitch += ((rawX + prevX) / 2.0) * elapsed;
      roll += ((rawY + prevY) / 2.0) * elapsed;
      heading += ((rawZ + prevZ) / 2.0) * elapsed;

      stopWatch.reset();
    }

    // remember these readings
    prevX = rawX;
    prevY = rawY;
    prevZ = rawZ;
  }
    public void run() {
      RobotLog.v("EventLoopRunnable has started");

      try {
        ElapsedTime elapsedEventLoop = new ElapsedTime();
        double sMinLoopInterval = 0.001D;
        long msLoopIntervalStep = 5L;

        while (!Thread.interrupted()) {
          while (elapsedEventLoop.time() < sMinLoopInterval) {
            Thread.sleep(msLoopIntervalStep);
          }

          elapsedEventLoop.reset();
          if (RobotLog.hasGlobalErrorMsg()) {
            EventLoopManager.this.buildAndSendTelemetry(
                "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
          }

          if (EventLoopManager.this.elapsedSinceHeartbeatReceived.startTime() == 0.0D) {
            Thread.sleep(500L);
          } else if (EventLoopManager.this.elapsedSinceHeartbeatReceived.time() > 2.0D) {
            EventLoopManager.this.handleDroppedConnection();
            EventLoopManager.this.currentPeerAddressAndPort = null;
            EventLoopManager.this.elapsedSinceHeartbeatReceived = new ElapsedTime(0L);
          }

          Iterator syncdDeviceIterator = EventLoopManager.this.syncdDevices.iterator();
          SyncdDevice syncdDevice;
          while (syncdDeviceIterator.hasNext()) {
            syncdDevice = (SyncdDevice) syncdDeviceIterator.next();
            syncdDevice.blockUntilReady();
          }

          boolean unblockOnException = false;

          try {
            unblockOnException = true;
            EventLoopManager.this.eventLoop.loop();
            unblockOnException = false;
          } catch (Exception e) {
            RobotLog.e("Event loop threw an exception");
            RobotLog.logStacktrace(e);
            String exceptionMessage =
                e.getClass().getSimpleName()
                    + (e.getMessage() != null ? " - " + e.getMessage() : "");
            RobotLog.setGlobalErrorMsg(
                "User code threw an uncaught exception: " + exceptionMessage);
            EventLoopManager.this.buildAndSendTelemetry(
                "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
            throw new RobotCoreException("EventLoop Exception in loop()");
          } finally {
            if (unblockOnException) {
              Iterator var9 = EventLoopManager.this.syncdDevices.iterator();

              while (var9.hasNext()) {
                SyncdDevice var10 = (SyncdDevice) var9.next();
                var10.startBlockingWork();
              }
            }
          }

          syncdDeviceIterator = EventLoopManager.this.syncdDevices.iterator();

          while (syncdDeviceIterator.hasNext()) {
            syncdDevice = (SyncdDevice) syncdDeviceIterator.next();
            syncdDevice.startBlockingWork();
          }
        }
      } catch (InterruptedException var20) {
        RobotLog.v("EventLoopRunnable interrupted");
        EventLoopManager.this.reportRobotStatus(RobotState.STOPPED);
      } catch (RobotCoreException var21) {
        RobotLog.v("RobotCoreException in EventLoopManager: " + var21.getMessage());
        EventLoopManager.this.reportRobotStatus(RobotState.EMERGENCY_STOP);
        EventLoopManager.this.buildAndSendTelemetry(
            "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
      }

      try {
        EventLoopManager.this.eventLoop.teardown();
      } catch (Exception var17) {
        RobotLog.w("Caught exception during looper teardown: " + var17.toString());
        RobotLog.logStacktrace(var17);
        if (RobotLog.hasGlobalErrorMsg()) {
          EventLoopManager.this.buildAndSendTelemetry(
              "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
        }
      }

      RobotLog.v("EventLoopRunnable has exited");
    }
 /*
  * Code to run when the op mode is first enabled goes here
  * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
  */
 @Override
 public void init_loop() {
   telemetry.addData("navX Op Init Loop", runtime.toString());
 }
  @Override
  public void runOpMode() throws InterruptedException {

    lwa = hardwareMap.dcMotor.get("leftwheelA");
    lwb = hardwareMap.dcMotor.get("leftwheelB");
    rwa = hardwareMap.dcMotor.get("rightwheelA");
    rwb = hardwareMap.dcMotor.get("rightwheelB");
    rwa.setDirection(DcMotor.Direction.REVERSE);
    rwb.setDirection(DcMotor.Direction.REVERSE);
    liftL = hardwareMap.dcMotor.get("liftL");
    liftR = hardwareMap.dcMotor.get("liftR");
    // liftR.setDirection(DcMotor.Direction.REVERSE);
    // liftL.setDirection(DcMotor.Direction.REVERSE);
    collector = hardwareMap.dcMotor.get("collector");
    // rightCR = hardwareMap.servo.get("rightCR");
    // leftCR = hardwareMap.servo.get("leftCR");
    swivel = hardwareMap.servo.get("swivel");
    dump = hardwareMap.servo.get("dump");
    trigL = hardwareMap.servo.get("trigL");
    trigR = hardwareMap.servo.get("trigR");
    dds = hardwareMap.servo.get("dds");
    holdL = hardwareMap.servo.get("holdL");
    holdR = hardwareMap.servo.get("holdR");
    holdC = hardwareMap.servo.get("holdC");
    // leftPivot = hardwareMap.servo.get("leftPivot");
    // rightPivot = hardwareMap.servo.get("rightPivot");
    lineSensor = hardwareMap.opticalDistanceSensor.get("dist1");
    touch = hardwareMap.touchSensor.get("touch");
    Gyro = hardwareMap.gyroSensor.get("Gyro");

    //  leftPivot.setPosition(1);
    // rightPivot.setPosition(0);
    dump.setPosition(0);
    trigL.setPosition(0.8);
    trigR.setPosition(0.05);
    //   leftCR.setPosition(0.5);
    //     rightCR.setPosition(0.5);
    dds.setPosition(1);
    holdL.setPosition(0.75);
    holdR.setPosition(0.05);
    holdC.setPosition(1);
    double lineSensorValue;

    double heading = 0;
    //    double integratedHeading = Gyro.getIntegratedZValue();
    double headingError;
    double targetHeading;
    double drivesteering;
    double driveGain = 0.005;
    double leftPower;
    double rightPower;
    double midPower = 0;
    double minPower = 0.2;

    Gyro.calibrate();

    // wait for the start button to be pressed
    waitForStart();

    timer = new ElapsedTime();

    collector.setPower(0);

    while (rwa.getCurrentPosition() > -7700 && timer.time() < 15) {
      rwa.setPower(-0.75);
      rwb.setPower(-0.75);
      lwa.setPower(-0.75);
      lwb.setPower(-0.75);
    }

    rwa.setPower(0);
    rwb.setPower(0);
    lwa.setPower(0);
    lwb.setPower(0);

    sleep(100);

    heading = Gyro.getHeading();
    telemetry.addData("heading", heading);

    heading = 359;
    sleep(100);

    while (heading > 225) {
      heading = Gyro.getHeading();
      //          integratedHeading = Gyro.getIntegratedZValue();

      telemetry.addData("heading", heading);
      //        telemetry.addData("Integrated", integratedHeading);

      targetHeading = 225;

      headingError = targetHeading - heading;

      drivesteering = driveGain * headingError;

      if (drivesteering > 1) {
        drivesteering = 1;
        telemetry.addData("Caught illegal value", "reset drivesteering to 1");
      }

      leftPower = midPower + drivesteering;
      rightPower = midPower + drivesteering;

      if (leftPower > minPower) {
        leftPower = minPower;
      }
      if (rightPower < minPower) {
        rightPower = minPower;
      }

      lwa.setPower(leftPower);
      lwb.setPower(leftPower);
      rwa.setPower(rightPower);
      rwb.setPower(rightPower);
    }

    while (heading < 225) {
      heading = Gyro.getHeading();
      //          integratedHeading = Gyro.getIntegratedZValue();

      telemetry.addData("heading", heading);
      //        telemetry.addData("Integrated", integratedHeading);

      targetHeading = 225;

      headingError = targetHeading - heading;

      drivesteering = driveGain * headingError;

      if (drivesteering > 1) {
        drivesteering = 1;
        telemetry.addData("Caught illegal value", "reset drivesteering to 1");
      }

      rightPower = midPower - drivesteering;
      leftPower = midPower + drivesteering;

      if (rightPower > minPower) {
        rightPower = minPower;
      }
      if (leftPower < minPower) {
        leftPower = minPower;
      }

      lwa.setPower(leftPower);
      lwb.setPower(leftPower);
      rwa.setPower(rightPower);
      rwb.setPower(rightPower);
    }

    lwa.setPower(1);
    lwb.setPower(1);
    rwa.setPower(1);
    rwb.setPower(1);
    sleep(400);

    lwa.setPower(0);
    lwb.setPower(0);
    rwa.setPower(0);
    rwb.setPower(0);
    telemetry.addData("Event", "Done");
    sleep(100);

    while (!touch.isPressed() && timer.time() < 20) {

      lineSensorValue = lineSensor.getLightDetectedRaw();

      if (lineSensorValue < 10) {

        lwa.setPower(0.5);
        lwb.setPower(0.5);
        rwa.setPower(0.0);
        rwb.setPower(0.0);
      } else {
        lwa.setPower(0.0);
        lwb.setPower(0.0);
        rwa.setPower(0.5);
        rwb.setPower(0.5);
      }
    }

    lwa.setPower(0.0);
    lwb.setPower(0.0);
    rwa.setPower(0.0);
    rwb.setPower(0.0);
    sleep(100);

    collector.setPower(0.0);
    sleep(100);

    if (timer.time() < 20) {
      lwa.setPower(-0.35);
      lwb.setPower(-0.35);
      rwa.setPower(-0.35);
      rwb.setPower(-0.35);

      sleep(225);

      lwa.setPower(0.0);
      lwb.setPower(0.0);
      rwa.setPower(0.0);
      rwb.setPower(0.0);

      sleep(100);
      dds.setPosition(0);

      sleep(900);
    }

    lwa.setPower(-1);
    lwb.setPower(-1);
    rwa.setPower(-1);
    rwb.setPower(-1);

    sleep(500);

    lwa.setPower(0.0);
    lwb.setPower(0.0);
    rwa.setPower(0.0);
    rwb.setPower(0.0);

    while (heading > 130) {
      heading = Gyro.getHeading();
      telemetry.addData("heading", heading);

      rwa.setPower(0.5);
      rwb.setPower(0.5);
      lwa.setPower(-0.5);
      lwb.setPower(-0.5);
    }

    lwa.setPower(-0.5);
    lwb.setPower(-0.5);
    rwa.setPower(-0.5);
    rwb.setPower(-0.5);
    sleep(1000);
    dds.setPosition(1);

    lwa.setPower(0.0);
    lwb.setPower(0.0);
    rwa.setPower(0.0);
    rwb.setPower(0.0);

    sleep(500);
  }
Beispiel #19
0
  public void moveBackward(double pow, int encoderVal, int timeout) throws InterruptedException {
    //        sensor.resetGyro();
    double angle;
    double startAngle = Math.abs(sensor.getGyroYaw());
    opMode.telemetry.update();

    double error;
    double power;

    motorBL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();

    motorBL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();

    setNullValue();

    nullValue = 0;

    ElapsedTime time = new ElapsedTime();
    time.startTime();

    int currentEncoder = getEncoderAvg() - nullValue;
    while (encoderVal > currentEncoder && time.milliseconds() < timeout) {
      opMode.telemetry.update();
      angle = Math.abs(sensor.getGyroYaw());

      currentEncoder = getEncoderAvg() - nullValue;

      error = (double) (encoderVal - currentEncoder) / encoderVal;

      power = (pow * error) - .25;

      Range.clip(power, -1, 1);

      opMode.telemetry.addData("Power", power);
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      if (angle < startAngle - 2) {
        startMotors((power), (power * .75));
      } else if (angle > startAngle + 2) {
        startMotors((power * .75), (power));
      } else {
        startMotors(power, power);
      }

      startMotors(power, power);
      opMode.idle();
    }
    stopMotors();
    opMode.telemetry.update();
    angleError = sensor.getGyroYaw();
  }
  @Override
  public void runOpMode() throws InterruptedException {
    climber = hardwareMap.servo.get(Keys.climber);
    swivel = hardwareMap.servo.get(Keys.swivel);
    hang = hardwareMap.servo.get(Keys.hang);
    clampLeft = hardwareMap.servo.get(Keys.clampLeft);
    clampRight = hardwareMap.servo.get(Keys.clampRight);
    dump = hardwareMap.servo.get(Keys.dump);
    fr = hardwareMap.dcMotor.get(Keys.frontRight);
    fl = hardwareMap.dcMotor.get(Keys.frontLeft);
    bl = hardwareMap.dcMotor.get(Keys.backLeft);
    br = hardwareMap.dcMotor.get(Keys.backRight);
    collector = hardwareMap.dcMotor.get(Keys.collector);
    fl.setDirection(DcMotor.Direction.REVERSE);
    bl.setDirection(DcMotor.Direction.REVERSE);
    dump.setPosition(Keys.DUMP_INIT);
    swivel.setPosition(Keys.SWIVEL_CENTER);
    hang.setPosition(Keys.HANG_INIT);
    clampLeft.setPosition(Keys.CLAMP_LEFT_INIT);
    clampRight.setPosition(Keys.CLAMP_RIGHT_INIT);
    climber.setPosition(Keys.CLIMBER_INITIAL_STATE);
    collector.setDirection(DcMotor.Direction.REVERSE);
    sonarAbovePhone = hardwareMap.analogInput.get(Keys.SONAR_ABOVE_PHONE);
    sonarFoot = hardwareMap.analogInput.get(Keys.SONAR_FOOT);
    navx_device =
        AHRS.getInstance(
            hardwareMap.deviceInterfaceModule.get(Keys.advancedSensorModule),
            Keys.NAVX_DIM_I2C_PORT,
            AHRS.DeviceDataType.kProcessedData,
            Keys.NAVX_DEVICE_UPDATE_RATE_HZ);
    while (!calibration_complete) {
      calibration_complete = !navx_device.isCalibrating();
      if (!calibration_complete) {
        telemetry.addData("Calibration Complete?", "No");
      }
    }
    telemetry.addData("Calibration Complete?", "Yes");
    // telemetry.addData("Start Autonomous?", "Yes");
    waitForStart();
    smoothMoveVol2(48, false);
    telemetry.addData("starting", "smoothDump starting");
    timer = new ElapsedTime();
    timer.reset();
    smoothDump(timer);
    mCamera = ((FtcRobotControllerActivity) hardwareMap.appContext).mCamera;
    // i need to init the camera and also get the instance of the camera        //on pic take
    // protocol
    telemetry.addData("camera", "initingcameraPreview");
    ((FtcRobotControllerActivity) hardwareMap.appContext).initCameraPreview(mCamera, this);

    // wait, because I have handler wait three seconds b4 it'll take a picture, in initCamera
    sleep(Vision.RETRIEVE_FILE_TIME);
    // now we are going to retreive the image and convert it to bitmap
    SharedPreferences prefs =
        hardwareMap
            .appContext
            .getApplicationContext()
            .getSharedPreferences("com.quan.companion", Context.MODE_PRIVATE);
    String path = prefs.getString(Keys.pictureImagePathSharedPrefsKeys, "No path found");
    Log.e("path", path);
    telemetry.addData("image", path);
    // debug stuff - telemetry.addData("camera", "path: " + path);
    File imgFile = new File(path);
    image = BitmapFactory.decodeFile(imgFile.getAbsolutePath());
    Log.e("image", image.toString());
    // cool now u have the image file u just took the picture of
    VisionProcess mVP = new VisionProcess(image);
    Log.e("starting output", "start");
    telemetry.addData("starting output", "doing smart computer stuff now");
    Beacon beacon = mVP.output(hardwareMap.appContext);
    Log.e("beacon", beacon.toString());
    telemetry.addData("beacon", beacon);
  }
 @Override
 public void start() {
   motorOscTimer.reset();
   servoOscTimer.reset();
   spamPrevention.reset();
 }
  @Override
  public void runOpMode() {
    elapsedTime = new ElapsedTime();
    dim = hardwareMap.deviceInterfaceModule.get("dim");
    dim.setDigitalChannelMode(GYROSCOPE_SPOT, DigitalChannelController.Mode.OUTPUT);
    gyro = hardwareMap.gyroSensor.get("gyro");
    dim.setDigitalChannelState(GYROSCOPE_SPOT, true);
    motorBL = hardwareMap.dcMotor.get("motorBL");
    motorBR = hardwareMap.dcMotor.get("motorBR");
    motorFL = hardwareMap.dcMotor.get("motorFL");
    elapsedTime.startTime();
    motorFR = hardwareMap.dcMotor.get("motorFR");
    //        color = hardwareMap.colorSensor.get("color");
    int distance1 = 5550;
    int distance3 = 9700;
    double currentAngle = 0.0;
    while (motorBL.getCurrentPosition() < distance1) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
    }
    while (currentAngle < 90.0) {
      startMotors(-1, -1, -1, -1);
      getEncoderValues();
      currentAngle = gyro.getRotation();
    }
    while (motorBL.getCurrentPosition() < distance3) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
    }
    getTime();
    elapsedTime.reset();
    double currentTime = 0.0;
    while (currentTime < 5.0) {
      getEncoderValues();
      getTime();
      currentTime = elapsedTime.time();
    }
    while (motorBL.getCurrentPosition() > 9000) { // 9034
      startMotors(1, -1, -1, 1);
      getEncoderValues();
    }
    while (currentAngle > 45.0) {
      startMotors(-1, -1, -1, -1);
      getEncoderValues();
      currentAngle = gyro.getRotation();
    }

    elapsedTime.reset();
    currentTime = 0.0;
    while (currentTime < 5.0) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
      getTime();
      currentTime = elapsedTime.time();
    }
    stopMotors();
    motorBL.close();
    motorFL.close();
    motorBR.close();
    motorFR.close();
  }
 public void getTime() {
   telemetry.addData("time", elapsedTime.time());
 }
Beispiel #24
0
 /*
  * This method will be called repeatedly in a loop
  * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
  */
 @Override
 public void loop() {
   telemetry.addData("1 Start", "NullOp started at " + startDate);
   telemetry.addData("2 Status", "running for " + runtime.toString());
 }