Пример #1
0
 /**
  * Rotates the robot a specified number of degrees left or right.
  *
  * @param turnDegrees the degree of the turn.
  * @param turnLeft true to turn left, false to turn right.
  */
 public void rotation(int turnDegrees, boolean turnLeft) {
   if (turnLeft == true) {
     pilot.rotate(turnDegrees);
   } else {
     pilot.rotate(-turnDegrees);
   }
 }
Пример #2
0
  public int[] getAngledDistances(DifferentialPilot pilot, int angle, int[] prevVals) {

    // const
    int sleepTime = 350;

    // action info
    LCD.clear(0);
    LCD.drawString("Finding direction", 0, 0);

    // get central dist
    prevVals[1] += getFastMeasurement();
    LCD.drawInt(prevVals[1], 5, 1);

    // rotate, ping, wait a bit
    pilot.rotate(angle);

    // read left distance
    prevVals[0] += getFastMeasurement();
    LCD.drawInt(prevVals[0], 0, 1);

    // rotate, ping, wait a bit
    pilot.rotate(-2 * angle);

    // read right distance
    prevVals[2] += getFastMeasurement();
    LCD.drawInt(prevVals[2], 10, 1);

    pilot.rotate(angle);

    return prevVals;
  }
 public void checkIfSharpCorner() {
   leftMotor.stop();
   rightMotor.stop();
   isSharpCorner = false;
   int performedRotation = 0;
   DifferentialPilot pilot = new DifferentialPilot(2, 10, leftMotor, rightMotor);
   if (getRealTimeValue() >= 0.1) {
     pilot.rotate(-4);
     isSharpCorner = true;
     tellLineBehaviorIsSharpCorner();
   } else {
     for (int i = 0; i <= 40; i++) {
       pilot.rotate(1);
       performedRotation += 1;
       if (getRealTimeValue() >= 0.1) {
         isSharpCorner = true;
         tellLineBehaviorIsSharpCorner();
         break;
       }
       if (Button.readButtons() != 0) {
         this.interrupt();
       }
     }
   }
   if (!isSharpCorner) {
     align(pilot, performedRotation);
     line.setIsNoSharpCorner();
   }
 }
Пример #4
0
 public void drawSquare(float length) {
   byte direction_l = 1;
   if (length < 0) {
     direction_l = -1;
     length = -length;
   }
   for (int i = 0; i < 4; i++) {
     p.travel(length);
     p.rotate(direction_l * 90);
   }
 }
Пример #5
0
  /** Default constructor, initializes the motors and sensors */
  public MazeSolver() {
    // Set up motors and sensors
    myPilot = new DifferentialPilot(wheelDiam, axleLength, Motor.B, Motor.A, true);
    myFrontSensor = new LightSensor(SensorPort.S1);
    myRightSensor = new LightSensor(SensorPort.S2);
    myCompass = new CompassSensor(SensorPort.S3);

    // Set rotate speed
    myPilot.setRotateSpeed(rotateSpeed);
    myPilot.setAcceleration(70);

    // Calibrate
    doCalibration();
  }
Пример #6
0
 /**
  * If an abyss is detected, the robot will rotate 10 degrees, until the light sensor sees normal
  * ground again.
  */
 @Override
 public void action() {
   System.out.println("Abys");
   suppressed = false;
   while (lightSensor.getLightValue() < threshold && !suppressed) {
     pilot.rotate(10, true);
     if (!pilot.isMoving()) {
       Settings.whipAndBridgeCounter++;
     }
     Delay.msDelay(10);
   }
   while (pilot.isMoving() && !suppressed) {
     Thread.yield();
   }
   pilot.stop();
 }
Пример #7
0
 private boolean moveStraight(boolean forward, int distance) {
   if (forward) {
     if (distance == 0) {
       pilot.forward();
       return true;
     } else {
       pilot.travel(distance);
       return true;
     }
   } else {
     if (distance == 0) {
       pilot.backward();
       return true;
     } else {
       pilot.travel(-distance);
       return true;
     }
   }
 }
Пример #8
0
 private boolean turn(boolean right, int radius) {
   if (right) {
     if (radius == 0) {
       pilot.rotateRight();
       return true;
     } else {
       pilot.rotate(radius);
       return true;
     }
   } else {
     if (radius == 0) {
       pilot.rotateLeft();
       return true;
     } else {
       pilot.rotate(-radius);
       return true;
     }
   }
 }
Пример #9
0
  /**
   * Stops the robot when the ultrasonic sensor detects a certain distance.
   *
   * @param stopDistance as long as the current distance is less than this stop distance, the robot
   *     will keep moving. Once the current distance is larger than the stop distance, the function
   *     returns.
   * @return the current distance upon stopping.
   */
  private boolean stopBeforeEdge() {
    int currentDistance = 0;
    int lightLevelReading;
    int distanceToObject;
    boolean edgeIsSafe = false;

    do {
      lightLevelReading = lightSensorDown.readValue();

      Motor.A.rotate(centerToEdgeDistance, true);
      Motor.C.rotate(centerToEdgeDistance, true);
    } while ((lightLevelReading > BLACK_WHITE_THRESHOLD)
        && (Motor.A.isMoving() && Motor.C.isMoving()));

    pilot.stop();

    lightLevelReading = lightSensorDown.readValue();

    /*
    distanceToObject = ultrasonicSensor.getDistance();

    if(distanceToObject < 20)
    {
    	edgeIsSafe = true;
    	Motor.A.setSpeed(1000);
    	Motor.C.setSpeed(1000);
    	pilot.setRotateSpeed(1000);
    	pilot.travel(280);
    	Delay.msDelay(10000);
    	return(edgeIsSafe);
    }

    if(lightLevelReading < BLACK_WHITE_THRESHOLD)
    {
    	System.out.println("Unsafe edge");
    	edgeIsSafe = false;
    	return(edgeIsSafe);
    }
    else if (lightLevelReading > BLACK_WHITE_THRESHOLD)
    {
    	System.out.println("Safe edge");
    	edgeIsSafe = true;
    	safeEdgeCount++;
    	return(edgeIsSafe);
    }

    */

    return (edgeIsSafe);
  }
Пример #10
0
 /** The robot moves along the Wall and turns to the right, if the wall is too far away. */
 @Override
 public void action() {
   suppressed = false;
   Settings.motorAAngle = Settings.SENSOR_RIGHT;
   while (!suppressed
       && !((Settings.TOUCH_L.isPressed() || Settings.TOUCH_R.isPressed()))
       && Settings.LIGHT.getLightValue() < 80) {
     if (sonic.getDistance() > (distanceToWall + 10)) {
       pilot.arc(-15, -90, true);
     } else if (sonic.getDistance() <= distanceToWall) {
       pilot.arc(40, 20, true);
     } else if (sonic.getDistance() > distanceToWall) {
       pilot.arc(-60, -20, true);
     }
   }
   if (Settings.LIGHT.getLightValue() >= 80) {
     pilot.stop();
     System.out.println("LINE Detected");
     Settings.afterSlider = true;
     Settings.motorAAngle = Settings.SENSOR_FRONT;
     Settings.PILOT.setTravelSpeed(Settings.PILOT.getMaxTravelSpeed() * 0.15);
   }
 }
Пример #11
0
  /** Turns the robot left using compass */
  private void turnLeft() {
    System.out.println("L");
    /*
     * // Stop momentarily myPilot.stop();
     *
     * // Get current bearing float x = myCompass.getDegrees(); float y = (x
     * + 90f) % 360;
     *
     * // Get us within a threshold of the degree that we want while (x < y
     * - 3) { myPilot.rotate(-5); x = myCompass.getDegrees();
     * System.out.println("Bearing: " + x); }
     */

    // Turn Left
    goBack();
    // goBack();
    myPilot.rotate(76);
    goForward(2, travelDist);
  }
Пример #12
0
  private boolean moveArc(boolean forward, boolean right, int distance, int radius) {

    if (forward) {
      if (right) {
        if (distance == 0 && radius == 0) {
          pilot.arcForward(-DEFAULT_RADIUS);
          return true;
        } else {
          pilot.arc(-radius, FORWARD_ANGLE);
          return true;
        }
      } else {
        if (distance == 0 && radius == 0) {
          pilot.arcForward(DEFAULT_RADIUS);
          return true;
        } else {
          pilot.arc(radius, FORWARD_ANGLE);
          return true;
        }
      }
    } else {
      if (right) {
        if (distance == 0 && radius == 0) {
          pilot.arcBackward(-DEFAULT_RADIUS);
          return true;
        } else {
          pilot.arc(-radius, BACKWARD_ANGLE);
          return true;
        }
      } else {
        if (distance == 0 && radius == 0) {
          pilot.arcBackward(DEFAULT_RADIUS);
          return true;
        } else {
          pilot.arc(radius, BACKWARD_ANGLE);
          return true;
        }
      }
    }
  }
Пример #13
0
  private int waitForReading() {
    int lightLevelReading;
    int distanceToObject;

    do {
      try {
        Thread.sleep(100);
      } catch (InterruptedException ex) {
      }

      lightLevelReading = lightSensorDown.readValue();
    } while (lightLevelReading > BLACK_WHITE_THRESHOLD);

    distanceToObject = ultrasonicSensor.getDistance();

    if (distanceToObject < 20) {
      Motor.A.setSpeed(1000);
      Motor.C.setSpeed(1000);
      pilot.travel(300);
      Delay.msDelay(10000);
    }

    return (lightLevelReading);
  }
Пример #14
0
  // circle method: traces a semi-circle and a quarter circle
  // and takes two DifferentialPilot objects as method input
  public static void circle(DifferentialPilot Pilot1, DifferentialPilot Pilot2) {
    // Semi-Circle
    Pilot1.setTravelSpeed(5); // first drawing the arc for the semi-circle
    Pilot1.arc(36, 180); // robot arcs left for 180 degrees, guided by a 36 inch radius
    Pilot1.stop();
    Delay.msDelay(300);
    Pilot1.setRotateSpeed(30);
    Pilot1.rotate(90); // robot makes a 90 degree left turn
    Pilot1.stop();
    Delay.msDelay(300);
    Pilot1.setTravelSpeed(5);
    Pilot1.travel(
        72); // here, we're closing up the semi-circle (that is, tracing diameter of the circle)

    Delay.msDelay(1300);

    // Quarter-Circle
    Pilot2.setTravelSpeed(5);
    Pilot2.arc(12, 90); // robot arcs right for 90 degree (so forms 1/4 of circle)
    Pilot2.stop();
    Delay.msDelay(300);

    // loop to close up the quarter circle
    for (int l = 1; l <= 2; l++) {
      Pilot2.setRotateSpeed(30);
      Pilot2.rotate(90); // robot makes a 90 degree right turn
      Pilot2.stop();
      Delay.msDelay(300);
      Pilot2.setTravelSpeed(5);
      Pilot2.travel(12); // robot goes forward for 12 in (the radius of circle)
      Pilot2.stop();
      Delay.msDelay(300);
    }
  }
Пример #15
0
  // polygon method: traces a triangle and a pentagon, and takes a DifferentialPilot object as
  // method input
  public static void polygon(DifferentialPilot Pilot1) {
    // Triangle: having three sides, so for-loop goes from 1 to 3
    for (int i = 1; i <= 3; i++) {
      Pilot1.setTravelSpeed(5); // setting the travel and rotation speed for the robot
      Pilot1.setRotateSpeed(30);
      Pilot1.travel(24); // robot travels 2 feet forward
      Pilot1.stop();
      Delay.msDelay(300);
      Pilot1.rotate(120); // robot rotates left to form a 60 degree interior angle
      Pilot1.stop();
      Delay.msDelay(300);
    }
    Delay.msDelay(1300);

    // Pentagon: having five sides, so for-loop goes from 1 to 5
    for (int i = 1; i <= 5; i++) {
      Pilot1.setTravelSpeed(5); // setting the travel and rotation speed for the robot
      Pilot1.setRotateSpeed(30);
      Pilot1.travel(24); // robot travels 2 feet forward
      Pilot1.stop();
      Delay.msDelay(300);
      Pilot1.rotate(72); // robot rotates left to form 108 interior angle of pentagon
      Pilot1.stop();
      Delay.msDelay(300);
    }
    Delay.msDelay(1300);
  }
Пример #16
0
 /** Set the velocity of the robot. Wheel diameter units per second */
 @Override
 public void setVelocity(double speed) {
   pilot.setTravelSpeed(speed);
 }
Пример #17
0
 private void goBack() {
   System.out.println("B");
   // Move some more, and return right away
   myPilot.travel(travelDist * -2.1);
 }
Пример #18
0
 /**
  * Returns a boolean based on the robot moving.
  *
  * @return boolean, True if robot is moving
  */
 @Override
 public boolean isMoving() {
   return pilot.isMoving();
 }
Пример #19
0
 private boolean stop() {
   pilot.stop();
   return true;
 }
Пример #20
0
  public static void main(String[] args) throws Exception {

    DifferentialPilot pilot = new DifferentialPilot(56f, 126f, Motor.D, Motor.A);
    pilot.setTravelSpeed(150);

    Brick brick = BrickFinder.getDefault();
    Port s1 = brick.getPort("S1"); //
    Port s4 = brick.getPort("S4"); //

    TextLCD lcd = brick.getTextLCD();

    NXTLightSensor lysSensor = new NXTLightSensor(s1); // NXT LYS
    EV3ColorSensor fargeSensor = new EV3ColorSensor(s4); // EV3 LYS

    // COLOR-----------------------------------------------------------------
    SampleProvider fargeSample = fargeSensor.getColorIDMode();
    float[] colorVerdi = new float[fargeSample.sampleSize()];

    // ----------------------------------------------------------------------

    // LYS-------------------------------------------------------------------
    SampleProvider lysSample = lysSensor;
    float[] lysVerdi = new float[lysSample.sampleSize()];

    // ----------------------------------------------------------------------

    Boolean move = true; // Sett til false hvis roboten ikke skal kjøre videre
    // System.out.println("start lokke");

    while (!Button.ESCAPE.isDown()) { // Kjører så lenge vi ikke trykker på exit-knappen
      // Kjør framover hvis vi ikke allerede gjør det
      if (!pilot.isMoving() && move) {
        pilot.forward();
      }

      lysSensor.fetchSample(lysVerdi, 0); // LYS
      float lys = lysVerdi[0];

      fargeSample.fetchSample(colorVerdi, 0); // COLOR
      // float color = colorVerdi[0];
      int color = fargeSensor.getColorID(); // Fungerer dette?

      lcd.clear();
      lcd.drawString("NXT " + lys, 0, 0);
      lcd.drawString("EV3 " + color, 0, 1);

      if (color != Color.BLACK) {

        pilot.rotate(-10); // VENSTRE
      } else if (lys < 48) {

        // pilot.arcForward(-150);
        pilot.rotate(10);
      }
      // Elsen under trengs ikke, da forward kjøres på starten uansett
      else {
        // pilot.forward();
        if (!pilot.isMoving()) {
          pilot.forward();
        }
      }
    } // while
    System.out.println("AVSLUTTET");
  } // main
Пример #21
0
 /** Go forward method */
 private void goForward(double numTimes, double dist) {
   System.out.println("F");
   // Move some more, and return right away
   myPilot.travel(dist * numTimes);
 }
 private void align(DifferentialPilot pilot, int performedRotation) {
   pilot.rotate(-performedRotation);
   pilot.rotate(-45);
 }
Пример #23
0
 // Called once after isFinished returns true
 protected void end() {
   p.stop();
   drivetrain.tankDrive(0, 0);
   drivetrain.disableControl();
 }
Пример #24
0
 /**
  * Returns the velocity of the robot.
  *
  * @return double, the velocity of the robot in wheel diameter units per second
  */
 @Override
 public double getVelocity() {
   return pilot.getTravelSpeed();
 }