Esempio n. 1
0
  // run method (required for Thread)
  public void run() {
    long updateStart, updateEnd;
    double distL, distR, deltaD, deltaT, dX, dY;

    while (true) {
      updateStart = System.currentTimeMillis();
      // put (some of) your odometer code here

      nowTachoL = leftMotor.getTachoCount(); // get tacho counts
      nowTachoR = rightMotor.getTachoCount();
      distL = Math.PI * WR * (nowTachoL - lastTachoL) / 180; // compute
      // wheel
      distR = Math.PI * WR * (nowTachoR - lastTachoR) / 180; // displacements
      lastTachoL = nowTachoL; // save tacho counts for next iteration
      lastTachoR = nowTachoR;
      deltaD = 0.5 * (distL + distR); // compute vehicle displacement
      deltaT = (distL - distR) / WB; // compute change in heading

      synchronized (lock) {
        // don't use the variables x, y, or theta anywhere but here!

        theta += deltaT;
        if (theta >= Math.PI * 2) {
          theta = theta - Math.PI * 2;
        }
        dX = deltaD * Math.sin(theta); // compute X component of
        // displacement
        dY = deltaD * Math.cos(theta); // compute Y component of
        // displacement
        x = x + dX;
        y = y + dY;
        thetaDeg = theta * 180 / Math.PI;
      }

      // this ensures that the odometer only runs once every period
      updateEnd = System.currentTimeMillis();
      if (updateEnd - updateStart < ODOMETER_PERIOD) {
        try {
          Thread.sleep(ODOMETER_PERIOD - (updateEnd - updateStart));
        } catch (InterruptedException e) {
          // there is nothing to be done here because it is not
          // expected that the odometer will be interrupted by
          // another thread
        }
      }
    }
  }
Esempio n. 2
0
 public static void frente(int distancia) {
   motorA.synchronizeWith(new RegulatedMotor[] {motorB});
   motorA.startSynchronization();
   motorA.rotate(odometria(distancia), true);
   motorB.rotate(odometria(distancia), true);
   motorC.stop();
   motorA.endSynchronization();
   motorA.waitComplete();
   motorB.waitComplete();
 }
Esempio n. 3
0
  // run method (required for Thread)
  public void run() {
    long updateStart, updateEnd;

    // reset the tacho count, and set the initial tacho counts to the last_tacho_X variables

    leftMotor.resetTachoCount();
    rightMotor.resetTachoCount();

    last_tacho_L = leftMotor.getTachoCount();
    last_tacho_R = rightMotor.getTachoCount();

    while (true) {
      updateStart = System.currentTimeMillis();

      int current_tacho_L = leftMotor.getTachoCount();
      int current_tacho_R = rightMotor.getTachoCount();

      // calculating the distance each wheel has travelled
      double distanceL = Math.PI * wheelRadius * (current_tacho_L - last_tacho_L) / 180;
      double distanceR = Math.PI * wheelRadius * (current_tacho_R - last_tacho_R) / 180;
      // updating the last tacho counts
      last_tacho_L = current_tacho_L;
      last_tacho_R = current_tacho_R;
      // calculating the change in angle and change in absolute displacement
      double deltaD = 0.5 * (distanceL + distanceR);
      double deltaT = (distanceR - distanceL) / axleLength;

      synchronized (lock) {
        // updating the locations of the variables
        theta += deltaT;

        // maintain the bounds of theta
        if (theta < 0) theta += 2 * Math.PI;
        if (theta > 2 * Math.PI) theta -= 2 * Math.PI;

        setDistanceTravelled(getDistanceTravelled() + deltaD);

        setX(x + deltaD * Math.cos(theta));
        setY(y + deltaD * Math.sin(theta));
      }

      // this ensures that the odometer only runs once every period
      updateEnd = System.currentTimeMillis();
      if (updateEnd - updateStart < ODOMETER_PERIOD) {
        try {
          Thread.sleep(ODOMETER_PERIOD - (updateEnd - updateStart));
        } catch (InterruptedException e) {
          // there is nothing to be done here because it is not
          // expected that the odometer will be interrupted by
          // another thread
        }
      }
    }
  }
Esempio n. 4
0
  public static void main(String[] args) {
    motorA = new EV3LargeRegulatedMotor(MotorPort.A);
    motorB = new EV3LargeRegulatedMotor(MotorPort.B);
    motorC = new EV3LargeRegulatedMotor(MotorPort.C);

    // motorA.setSpeed(200);//UMB
    // motorB.setSpeed(200.16f);//UMB
    motorA.setAcceleration(50);
    motorB.setAcceleration(50);
    motorA.setSpeed(200);
    motorB.setSpeed(201.5f);

    frente(100);
    parar();
    resolucao_primeira();
    levantar_garra();

    motorA.setAcceleration(20);
    motorB.setAcceleration(20);
    frente(20);
    baixar_garra();
    frente(20);

    levantar_garra();

    frente(24);

    motorA.setAcceleration(50);
    motorB.setAcceleration(50);
    frente(-10);
    baixar_garra();

    frente(-54);

    /*
    primeira_Prova();
    parar();
    resolucao_primeira();

    segunda_Prova();
    */
  }
  public void avoidObstacle(double pX, double pY) {

    rightMotor.stop();
    leftMotor.stop();

    leftUltraSoundMotor.rotate(50, true);
    leftMotor.rotate(NavigatorUtility.convertAngle(wheelRadius, axleLength, 90), true);
    rightMotor.rotate(-NavigatorUtility.convertAngle(wheelRadius, axleLength, 90), false);

    double currentX;
    double currentY;

    do {
      currentX = odometer.getX();
      currentY = odometer.getY();
      wallFollowerController.processUSData(ultraSonicPoller.getLeftUltraSoundSensorDistance());
    } while (Math.abs(
            NavigatorUtility.calculateAngleError(pX - currentX, pY - currentY, odometer.getTheta())
                * 180
                / Math.PI)
        > wallFollowingAngleError);

    leftUltraSoundMotor.rotate(-50, false);
  }
Esempio n. 6
0
  public void run() {
    leftMotor.setAcceleration(2000);
    rightMotor.setAcceleration(2000);

    distMid = getFilteredData();
    sensorMotor.rotate(45);
    Delay.msDelay(500);
    distLeft = getFilteredData();
    sensorMotor.rotate(-90);
    Delay.msDelay(500);
    distRight = getFilteredData();
    sensorMotor.rotate(45);

    if (distLeft > distMid && distLeft > distRight) {
      select = 0;
    } else if (distRight >= distMid && distRight >= distLeft) {
      select = 2; // turn right
    }

    if (select == 0) {

      leftMotor.setSpeed(ROTATE_SPEED);
      rightMotor.setSpeed(ROTATE_SPEED);
      sensorMotor.setAcceleration(1000);
      angleSave = odometer.getAng();
      leftMotor.rotate(-convertAngle(2.2, 15, 89.8), true);
      rightMotor.rotate(convertAngle(2.2, 15, 89.8), false);
      sensorMotor.rotate(-85);

      while (safe == false || collide() == false) {
        if (getFilteredData() > 100 && filterControl < FILTER_OUT) {
          // bad value, do not set the distance var, however do increment the filter value
          filterControl++;
        } else if (getFilteredData() > 100) {
          // set distance to this.distance
          distance = 100;
        } else {
          // distance went below 100, therefore reset everything.
          filterControl = 0;
          distance = getFilteredData();
        }

        perror = distance - bandCenter;
        amperror = perror * proportion;
        if (distance > 60) {
          // if distance >80, the robot is about to do a U-turn
          leftMotor.setSpeed(maxSpeed);
          rightMotor.setSpeed(motorStraight);
          // right wheel will speed up in order to turn left
          leftMotor.forward();
          rightMotor.forward();
        }
        /*	else if(distance<8){
        	// if distance<8, the robot is going to hit the wall
        	leftMotor.setSpeed(motorStraight+50);
        	rightMotor.setSpeed(motorStraight+50);
        	leftMotor.forward();
        	rightMotor.backward();
        	// right motor rolls backwards to respond.
        }
        */
        else {
          // general proportion error correction
          rightMotor.setSpeed(motorStraight);
          if ((motorStraight + amperror) > maxSpeed) {
            leftMotor.setSpeed(maxSpeed);
          }
          // left wheel remains constant speed
          else {
            leftMotor.setSpeed((int) (motorStraight + amperror));
          }
          // right wheel will continuously correcting its speed according to errors.
          leftMotor.forward();
          rightMotor.forward();
        }

        if (CloseTo(rightDegreeHelper(angleSave, odometer.getAng()), -90)) {
          sensorMotor.rotate(85);
          safe = true;
        }
      }
    }

    if (select == 2) {
      leftMotor.setSpeed(ROTATE_SPEED);
      rightMotor.setSpeed(ROTATE_SPEED);
      sensorMotor.setAcceleration(1000);
      angleSave = odometer.getAng();
      leftMotor.rotate(convertAngle(2.2, 15, 89.8), true);
      rightMotor.rotate(-convertAngle(2.2, 15, 89.8), false);
      sensorMotor.rotate(85);

      while (safe == false || collide() == false) {
        if (getFilteredData() > 100 && filterControl < FILTER_OUT) {
          // bad value, do not set the distance var, however do increment the filter value
          filterControl++;
        } else if (getFilteredData() > 100) {
          // set distance to this.distance
          distance = 100;
        } else {
          // distance went below 100, therefore reset everything.
          filterControl = 0;
          distance = getFilteredData();
        }

        perror = distance - bandCenter;
        amperror = perror * proportion;
        if (distance > 60) {
          // if distance >80, the robot is about to do a U-turn
          leftMotor.setSpeed(motorStraight);
          rightMotor.setSpeed(maxSpeed);
          // right wheel will speed up in order to turn left
          leftMotor.forward();
          rightMotor.forward();
        }
        /*	else if(distance<10){
        	// if distance<8, the robot is going to hit the wall
        	rightMotor.setSpeed(motorStraight+50);
        	rightMotor.setSpeed(motorStraight+50);
        	rightMotor.forward();
        	leftMotor.backward();
        	// right motor rolls backwards to respond.
        }
        */
        else {
          // general proportion error correction
          leftMotor.setSpeed(motorStraight);
          if ((motorStraight + amperror) > maxSpeed) {
            rightMotor.setSpeed(maxSpeed);
          }
          // left wheel remains constant speed
          else {
            rightMotor.setSpeed((int) (motorStraight + amperror));
          }
          // right wheel will continuously correcting its speed according to errors.
          leftMotor.forward();
          rightMotor.forward();
        }

        if (CloseTo(leftDegreeHelper(angleSave, odometer.getAng()), 90)) {
          sensorMotor.rotate(-85);
          safe = true;
        }
      }
    }
    if (collide() == true) {
      leftMotor.stop(true);
      rightMotor.stop(false);
      leftMotor.rotate(convertDistance(2.2, -10), true);
      rightMotor.rotate(convertDistance(2.2, -10), false);
    }
  }
Esempio n. 7
0
 public static void levantar_garra() {
   motorC.setSpeed(50);
   motorC.rotate(50, false);
   motorC.stop();
 }
Esempio n. 8
0
 public static void baixar_garra() {
   motorC.setSpeed(50);
   motorC.rotate(-50, false);
   motorC.stop();
 }
Esempio n. 9
0
 public static void resolucao_primeira() {
   motorC.setSpeed(30);
   // motorC.rotate(-15, false);
   motorC.rotate(-30, false);
   motorC.stop();
 }
Esempio n. 10
0
 public static void parar() {
   motorA.stop(true);
   motorB.stop(true);
 }