Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP

Comparing changes

Choose two branches to see what's changed or to start a new pull request. If you need to, you can also compare across forks.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also compare across forks.
  • 2 commits
  • 3 files changed
  • 0 commit comments
  • 1 contributor
View
75 src/java/penoplatinum/simulator/LineFollowerNavigator.java
@@ -0,0 +1,75 @@
+package penoplatinum.simulator;
+
+import lejos.robotics.Color;
+
+/*
+ * LineFollowerNavigator
+ *
+ * This navigator uses the lightsensor to follow lines accross the track
+ *
+ * author: Team Platinum
+ */
+public class LineFollowerNavigator implements Navigator {
+
+ private Boolean idle = true;
+ private Model model;
+ private int angle = -120;
+ private int[] rotates = new int[]{-5, 10, -20, 30, -35, 40, -50, 60, -70, 80, -90, 100, -110, 120, -130, 140, -150, 160, -170, 180, -190, 200, -210, 220, -230, 240, -250, 260, -270, 280, -290, 300};
+ private int timesturned = 0;
+
+ @Override
+ //no goal just drive
+ public Boolean reachedGoal() {
+ return false;
+ }
+
+ @Override
+ public int nextAction() {
+ //start
+ if (this.idle) {
+ this.idle = false;
+ return Navigator.MOVE;
+ }
+ //get the color we are on
+ int lightValue = model.getSensorValue(Model.S4);
+ //
+ if (lightValue == 100 || lightValue == 0) {
+ timesturned = 0;
+ return Navigator.MOVE;
+ } else {
+// try {
+// angle = (int) rotates[timesturned];
+// } catch (IndexOutOfBoundsException e) {
+// timesturned = 0;
+// angle = (int) rotates[timesturned];
+// }
+ timesturned++;
+ if(timesturned == angle/3){
+ angle = angle*-2;
+ timesturned = 0;
+ }
+ return Navigator.TURN;
+ }
+
+
+
+ }
+
+ @Override
+ public double getDistance() {
+ return 0.1;
+
+ }
+
+ @Override
+ public double getAngle() {
+ return angle;
+ }
+
+ @Override
+ // the model supplies the lightsensorValues so we can decide to drive or turn
+ public LineFollowerNavigator setModel(Model model) {
+ this.model = model;
+ return this;
+ }
+}
View
4 src/java/penoplatinum/simulator/SimulationRunner.java
@@ -17,7 +17,7 @@
public static void main(String[] args) {
// setup the robot
- Navigator navigator = new SonarNavigator();
+ Navigator navigator = new LineFollowerNavigator();
Robot robot = new NavigatorRobot(navigator);
// construct a course
@@ -37,7 +37,7 @@ public static void main(String[] args) {
simulator.useMap (map);
// put the robot at position 150 cm from top, 150 cm from left, in an
// angle of 33 degrees, with 0 degrees pointing north
- simulator.putRobotAt (robot, 150, 150, 33);
+ simulator.putRobotAt (robot, 20, 60, 0);
// give robot instructions through the communication layer
// simulator.send( "start" );
View
135 src/java/penoplatinum/simulator/Simulator.java
@@ -12,10 +12,13 @@
* Author: Team Platinum
*/
+import java.awt.Color;
import java.lang.System;
import java.awt.Point;
import java.util.List;
import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.Set;
class Simulator {
private static int tileSize = 80; // our tiles are 80cm
@@ -51,6 +54,8 @@
private int lastChangeM1 = 0; // the last change on Motor 1
private int lastChangeM2 = 0; // the last change on Motor 2
private int lastChangeM3 = 0; // the last change on Motor 3
+
+ private final int LIGHTSENSOR_DISTANCE =10;//radius to the lightsensorposition
// main constructor, no arguments, Simulator is selfcontained
public Simulator() {
@@ -265,12 +270,131 @@ private void updateMotors() {
this.sensorValues[Model.M2] = this.lastChangeM2;
}
- private void updateLightSensor() {
- // TODO: check lines
- // TODO: check barcodes
- // probably == a getColor() method
+private void updateLightSensor() {
+
+// determine tile coordinates we're on( TODO also used in getFreeFrontDistance Refactoring...)
+ int left = (int) Math.floor(this.positionX / Simulator.tileSize) + 1;
+ int top = (int) Math.floor(this.positionY / Simulator.tileSize) + 1;
+
+// determine position within tile
+ int x = (int) (this.positionX % Simulator.tileSize)*2;
+ int y = (int) (this.positionY % Simulator.tileSize)*2;
+
+
+// determine Light Sensor Position
+ Tile currentTile = map.get(left, top);
+// return currentTile.getLightValue(lightSensorPoint)
+ // TODO: check lines
+ int yposition = (int) (y+LIGHTSENSOR_DISTANCE*-Math.cos(direction));
+ int xposition = (int) (x+LIGHTSENSOR_DISTANCE*Math.sin(direction));
+
+ System.out.println(""+ xposition +" " + yposition + "");
+
+ Color currentColor = getLineColor(xposition, yposition, currentTile);
+
+ if(getRobotOnBarcode(xposition, yposition, currentTile)){
+ currentColor = getBarcodeColor(xposition,yposition,currentTile);
+ }
+ // TODO: check barcodes
- }
+ if(currentColor == Color.WHITE){
+ this.sensorValues[Model.S4]= 100;
+ }
+ else if(currentColor== Color.BLACK){
+ this.sensorValues[Model.S4]=0;
+ }
+ else
+ this.sensorValues[Model.S4]=70;
+ // update waarden in sensors
+
+
+ }
+ //return the Color from the Barcode
+ public Color getBarcodeColor(int x, int y, Tile currentTile){
+ // normalisatie.
+ int relativePosition= 0;
+ switch(currentTile.getBarcodeLocation()){
+ case Baring.N: relativePosition = y/Board.BARCODE_PIXEL_WIDTH;break;
+ case Baring.S: relativePosition = (Board.TILE_WIDTH_AND_LENGTH-y)/Board.BARCODE_PIXEL_WIDTH;break;
+ case Baring.E: relativePosition = (Board.TILE_WIDTH_AND_LENGTH -x)/Board.BARCODE_PIXEL_WIDTH;break;
+ case Baring.W: relativePosition = x/Board.BARCODE_PIXEL_WIDTH;break;
+ }
+ return ((currentTile.getBarcode() & (1<<relativePosition) )!= 0 ? Color.BLACK : Color.WHITE );
+ }
+
+
+ // check if the robot is on a barcode
+ public boolean getRobotOnBarcode(int x, int y,Tile currentTile ) {
+
+ switch(currentTile.getBarcodeLocation()){
+ case Baring.N: return y<28 ? true : false;
+ case Baring.E: return x>132 ? true : false;
+ case Baring.S: return y>132 ? true : false;
+ case Baring.W: return x<28 ? true : false;
+ }
+ return false;
+ }
+
+ public Color getLineColor(int x, int y, Tile currentTile) {
+
+ //check WEST and EAST
+ Color currentColor = new Color(205, 165, 100);
+ Set firstLineValues = new HashSet<Integer>();
+ Set secondLineValues = new HashSet<Integer>();
+ for (int i = 0; i < (Board.LINE_PIXEL_WIDTH - 1); i++) {
+ firstLineValues.add(Board.LINE_ORIGIN + i);
+ }
+
+ for (int i = 0; i < (Board.LINE_PIXEL_WIDTH - 1); i++) {
+ secondLineValues.add(Board.LINE_ORIGIN + (Board.TILE_WIDTH_AND_LENGTH - Board.LINE_ORIGIN * 2) + i);
+ }
+
+ if (firstLineValues.contains(x)) {
+
+ if (currentTile.hasLine(Baring.W)) {
+ currentColor = currentTile.hasLine(Baring.W, Tile.WHITE) ? Color.WHITE : Color.BLACK;
+ currentColor = resetColorForTurnY(currentTile, y, currentColor);
+ }
+ }
+ if (secondLineValues.contains(x)) {
+ if (currentTile.hasLine(Baring.E)) {
+ currentColor = currentTile.hasLine(Baring.E, Tile.WHITE) ? Color.WHITE : Color.BLACK;
+ currentColor = resetColorForTurnY(currentTile, y, currentColor);
+ }
+ }
+ if (firstLineValues.contains(y)) {
+ if (currentTile.hasLine(Baring.N)) {
+ currentColor = currentTile.hasLine(Baring.N, Tile.WHITE) ? Color.WHITE : Color.BLACK;
+ currentColor = resetColorForTurnX(currentTile, x, currentColor);
+ }
+ }
+ if (secondLineValues.contains(y)) {
+ if (currentTile.hasLine(Baring.S)) {
+ currentColor = currentTile.hasLine(Baring.S, Tile.WHITE) ? Color.WHITE : Color.BLACK;
+ currentColor = resetColorForTurnX(currentTile, x, currentColor);
+ }
+ }
+ return currentColor;
+ }
+
+ private Color resetColorForTurnX(Tile currentTile, int x, Color currentColor) {
+ if (currentTile.hasLine(Baring.W) && x < 40) {
+ currentColor = new Color(205, 165, 100);
+ }
+ if (currentTile.hasLine(Baring.E) && x > 120) {
+ currentColor = new Color(205, 165, 100);
+ }
+ return currentColor;
+ }
+ private Color resetColorForTurnY(Tile currentTile, int y, Color currentColor) {
+ if (currentTile.hasLine(Baring.N) && y < 40) {
+ currentColor = new Color(205, 165, 100);
+ }
+ if (currentTile.hasLine(Baring.S) && y > 120) {
+ currentColor = new Color(205, 165, 100);
+ }
+ return currentColor;
+ }
private void calculateBumperSensor(int angle, int lengthRobot, int sensorPort) {
angle = ( this.getAngle() + angle ) % 360;
@@ -382,6 +506,7 @@ public Simulator run() {
this.startTime = System.currentTimeMillis();
this.robotAgent.run();
while( ! this.robot.reachedGoal() && ! this.reachedGoal() ) {
+ System.out.println("" + this.sensorValues[Model.S4]+"");
this.robot.step();
this.step();
}

No commit comments for this range

Something went wrong with that request. Please try again.