예제 #1
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);
    }
  }
예제 #2
0
파일: Lab5.java 프로젝트: sebastien177/DPM
  public static void main(String[] args) {
    int buttonChoice;

    // setup the odometer and display
    Odometer odo = new Odometer(leftMotor, rightMotor, 30, true);
    final TextLCD t = LocalEV3.get().getTextLCD();

    do {
      // clear the display
      t.clear();

      // ask the user whether he wants to detect blocks or search blocks
      t.drawString("< Left  |Right >", 0, 0);
      t.drawString("        |       ", 0, 1);
      t.drawString("Detect|Search ", 0, 2);

      buttonChoice = Button.waitForAnyPress();

      while (buttonChoice != Button.ID_LEFT
          && buttonChoice != Button.ID_RIGHT
          && buttonChoice != Button.ID_ESCAPE) {
        /*
         * These two if statements is to make the motor attached to the USsensor rotate
         * 90 degrees before the main methods are launched
         */
        if (buttonChoice == Button.ID_UP) {
          Scan scan = new Scan(usMotor);
          scan.usMotorSpeed(50);
          scan.turnSensor(90);
          buttonChoice = Button.waitForAnyPress();
        }

        if (buttonChoice == Button.ID_DOWN) {
          Scan scan = new Scan(usMotor);
          scan.usMotorSpeed(50);
          scan.turnSensor(-90);
          buttonChoice = Button.waitForAnyPress();
        }
      }
    } while (buttonChoice != Button.ID_LEFT
        && buttonChoice != Button.ID_RIGHT
        && buttonChoice != Button.ID_ESCAPE);

    if (buttonChoice == Button.ID_ESCAPE) {
      System.exit(0);
    }

    SensorModes usSensor = new EV3UltrasonicSensor(usPort);
    SampleProvider usValue =
        usSensor.getMode("Distance"); // colorValue provides samples from this instance
    float[] usData =
        new float[usValue.sampleSize()]; // colorData is the buffer in which data are returned

    SensorModes colorSensor = new EV3ColorSensor(colorPort);
    SampleProvider colorValue =
        colorSensor.getMode("ColorID"); // colorValue provides samples from this instance
    float[] colorData =
        new float[colorValue.sampleSize()]; // colorData is the buffer in which data are returned

    // The following start the PartA of the Lab when the right button is pressed, afterwards press
    // escape to exit program
    while (buttonChoice != Button.ID_RIGHT && buttonChoice != Button.ID_ESCAPE) {
      if (buttonChoice == Button.ID_LEFT) {
        ObjectDetection od = new ObjectDetection(colorValue, colorData, usValue, usData);
        od.run();
        LCD.drawString("< Left  |Right >", 0, 0);
        LCD.drawString("        |       ", 0, 1);
        LCD.drawString("Detect|Search ", 0, 2);
      }
      buttonChoice = Button.waitForAnyPress();
    }

    if (buttonChoice == Button.ID_ESCAPE) {
      System.exit(0);
    }
    // If the left button is pressed, the robot will start partB of the lab which is localize, and
    // start scanning the field

    odo.start();
    final USLocalizer usl =
        new USLocalizer(
            odo,
            usSensor,
            usData,
            USLocalizer.LocalizationType.FALLING_EDGE,
            leftMotor,
            rightMotor,
            WHEEL_RADIUS,
            TRACK);
    final Scan scan =
        new Scan(usValue, usData, colorValue, colorData, odo, leftMotor, rightMotor, usMotor);
    br = new BlockRecognition(odo, usSensor, usData, colorValue, colorData, rightMotor, leftMotor);

    new LCDInfo(odo, usSensor, usData, colorSensor, colorData);

    // begin the threads (we launch a thread to be able to exit it whenever we want using escape)
    (new Thread() {
          public void run() {
            br.start();
            scan.start();
            usl.doLocalization();
            scan.startRun();
          }
        })
        .start();
    while (Button.waitForAnyPress() != Button.ID_ESCAPE) ;
    System.exit(0);
  }