From: Natanael Adityasatria Date: Wed, 4 Nov 2015 17:13:30 +0000 (+0100) Subject: Implemented AvoidObject behaviour and TextLCD X-Git-Url: https://git.martlubbers.net/?a=commitdiff_plain;h=8af02b9d128a7671b7332db44b1ec7366ce32cf5;p=des2015.git Implemented AvoidObject behaviour and TextLCD --- diff --git a/mart/ev3/ex2/nl/ru/des/ColorMemory.java b/mart/ev3/ex2/nl/ru/des/ColorMemory.java deleted file mode 100644 index 7e92f12..0000000 --- a/mart/ev3/ex2/nl/ru/des/ColorMemory.java +++ /dev/null @@ -1,25 +0,0 @@ -package nl.ru.des; - -import java.util.Arrays; -import java.util.HashSet; -import java.util.List; -import java.util.Set; - -public class ColorMemory implements MessageHandler { - private List COLORSTOFIND = Arrays.asList(new String[] { "Blue", "Red", "Yellow" }); - private Set colors; - - public ColorMemory(){ - colors = new HashSet(); - } - - @Override - public void handleMessage(String message) { - colors.add(message); - LCDPrinter.print(message + " found"); - } - - public boolean finished() { - return colors.equals(new HashSet(COLORSTOFIND)); - } -} diff --git a/mart/ev3/ex2/nl/ru/des/ColorSensor.java b/mart/ev3/ex2/nl/ru/des/ColorSensor.java deleted file mode 100644 index 616b275..0000000 --- a/mart/ev3/ex2/nl/ru/des/ColorSensor.java +++ /dev/null @@ -1,78 +0,0 @@ -package nl.ru.des; - -import java.util.Arrays; -import java.util.HashMap; -import java.util.Map; - -import lejos.hardware.Button; -import lejos.hardware.sensor.EV3ColorSensor; -import lejos.robotics.SampleProvider; - -public class ColorSensor { - public static final String[] COLORS = new String[]{ - "Black", - "White", - "Yellow", - "Red", - "Blue" - }; - public static final long SAMPLETIME = 50; - - private float[] samples; - private SampleProvider sampleProvider; - private Map colors; - private long lastSampleTaken; - - private class Color{ - private float[] rgb; - - public Color(float r, float g, float b){ - this.rgb = new float[]{r, g, b}; - } - - public double distanceTo(Color c){ - return Math.sqrt( - Math.pow(this.rgb[0]-c.rgb[0], 2)+ - Math.pow(this.rgb[1]-c.rgb[1], 2)+ - Math.pow(this.rgb[2]-c.rgb[2], 2) - ); - } - } - - public ColorSensor(EV3ColorSensor colorSensor){ - this.sampleProvider = colorSensor.getRGBMode(); - this.colors = new HashMap(); - this.samples = new float[sampleProvider.sampleSize()]; - calibrate(); - this.lastSampleTaken = System.currentTimeMillis(); - } - - public void calibrate(){ - LCDPrinter.print("Color calibration initialized..."); - for(String c : COLORS){ - LCDPrinter.print("Put the sensor on " + c); - Button.waitForAnyPress(); - sampleProvider.fetchSample(samples, 0); - colors.put(c, new Color(samples[0], samples[1], samples[2])); - LCDPrinter.print("Value:" + Arrays.toString(samples)); - } - } - - public String getCurrentColor(){ - if(System.currentTimeMillis()-lastSampleTaken > SAMPLETIME){ - lastSampleTaken = System.currentTimeMillis(); - sampleProvider.fetchSample(samples, 0); - } - Color current = new Color(samples[0], samples[1], samples[2]); - String min = null; - double dist = Double.MAX_VALUE; - for(String c : colors.keySet()){ - double newdist = current.distanceTo(colors.get(c)); - if(newdist < dist){ - min = c; - dist = newdist; - } - } - return min; - } -} diff --git a/mart/ev3/ex2/nl/ru/des/CommBehaviour.java b/mart/ev3/ex2/nl/ru/des/CommBehaviour.java deleted file mode 100644 index c52e15c..0000000 --- a/mart/ev3/ex2/nl/ru/des/CommBehaviour.java +++ /dev/null @@ -1,30 +0,0 @@ -package nl.ru.des; - -import lejos.hardware.Sound; -import lejos.hardware.motor.EV3LargeRegulatedMotor; -import lejos.robotics.subsumption.Behavior; - -public class CommBehaviour implements Behavior { - - private String message; - - @Override - public boolean takeControl() { - // TODO Auto-generated method stub - return false; - } - - @Override - public void action() { - // TODO Auto-generated method stub - LCDPrinter.print(message); - } - - @Override - public void suppress() { - // TODO Auto-generated method stub - - } - - -} diff --git a/mart/ev3/ex2/nl/ru/des/LCDPrinter.java b/mart/ev3/ex2/nl/ru/des/LCDPrinter.java index 104521a..8807ce9 100644 --- a/mart/ev3/ex2/nl/ru/des/LCDPrinter.java +++ b/mart/ev3/ex2/nl/ru/des/LCDPrinter.java @@ -1,56 +1,58 @@ package nl.ru.des; +import java.io.IOException; +import java.io.OutputStream; +import java.io.PrintStream; import java.util.Deque; -import java.util.Iterator; import java.util.LinkedList; -import lejos.hardware.lcd.Font; -import lejos.hardware.lcd.GraphicsLCD; +import lejos.hardware.lcd.TextLCD; import lejos.utility.Delay; -public class LCDPrinter extends Thread { +public class LCDPrinter{ public static final int PRINTDELAY = 50; private static Deque buffer = new LinkedList(); - private GraphicsLCD glcd; - private Font font; - private Deque lcdbuffer; - private int charwidth; - - public LCDPrinter(GraphicsLCD glcd, Font font) { - this.glcd = glcd; - this.font = font; - glcd.setFont(font); - lcdbuffer = new LinkedList(); - charwidth = glcd.getWidth()/font.width; - for(int i = 0; i glcd.getTextWidth()){ + buffer.addFirst(c.substring(glcd.getTextWidth(), c.length())); + c = c.substring(0, glcd.getTextWidth()); + } + glcd.scroll(); + glcd.drawString(c, 0, glcd.getTextHeight()-1); + } + Delay.msDelay(PRINTDELAY); + } + } + }).start(); + LCDPrinter.print("LCDThread started"); } public static void print(String s){ buffer.addLast(s); } - - public void run() { - Iterator it; - while (true) { - if (!buffer.isEmpty()) { - String c = buffer.remove(); - lcdbuffer.removeLast(); - if(c.length() > charwidth){ - buffer.addFirst(c.substring(charwidth, c.length())); - c = c.substring(0, charwidth); - } - lcdbuffer.addFirst(c); - glcd.clear(); - it = lcdbuffer.descendingIterator(); - for(int i = 0; it.hasNext(); i++){ - glcd.drawString(it.next(), 0, i*font.height, font.getBaselinePosition()); + + public static PrintStream getPrefixedPrintstream(final String prefix, final TextLCD glcd){ + return new PrintStream(new OutputStream(){ + private char[] line = new char[glcd.getTextWidth()]; + private int index = 0; + + @Override + public void write(int b) throws IOException { + if(index == line.length-prefix.length() || b == '\n'){ + LCDPrinter.print(prefix + String.valueOf(line, 0, index)); + index = 0; + } else { + line[index++] = (char)b; } } - Delay.msDelay(PRINTDELAY); - } + }); } } diff --git a/mart/ev3/ex2/nl/ru/des/Main.java b/mart/ev3/ex2/nl/ru/des/Main.java index 3a7e256..1be815b 100644 --- a/mart/ev3/ex2/nl/ru/des/Main.java +++ b/mart/ev3/ex2/nl/ru/des/Main.java @@ -1,38 +1,50 @@ package nl.ru.des; +import lejos.hardware.Button; import lejos.hardware.ev3.EV3; import lejos.hardware.ev3.LocalEV3; import lejos.hardware.lcd.Font; +import lejos.hardware.lcd.TextLCD; import lejos.hardware.motor.EV3LargeRegulatedMotor; import lejos.hardware.port.MotorPort; import lejos.hardware.sensor.EV3ColorSensor; +import lejos.hardware.sensor.EV3TouchSensor; +import lejos.hardware.sensor.EV3UltrasonicSensor; import lejos.robotics.subsumption.Arbitrator; import lejos.robotics.subsumption.Behavior; +import nl.ru.des.behaviours.AvoidHighObjectBehaviour; +import nl.ru.des.behaviours.AvoidLowObjectBehaviour; +import nl.ru.des.behaviours.ShutdownBehaviour; +import nl.ru.des.behaviours.StayInFieldBehaviour; +import nl.ru.des.behaviours.WandererBehaviour; +import nl.ru.des.bluetooth.BTController; +import nl.ru.des.bluetooth.ColorMemory; +import nl.ru.des.sensors.ColorSensor; +import nl.ru.des.sensors.TouchSensor; +import nl.ru.des.sensors.UltraSoneSensor; public class Main { public static Arbitrator arbitrator; public static void main(String[] args) { EV3 brick = LocalEV3.get(); - LCDPrinter lcdprinter = new LCDPrinter(brick.getGraphicsLCD(), Font.getSmallFont()); - lcdprinter.start(); - /* - * Start the slave first. "Socket is 18" is always displayed, we can just ignore this - */ + TextLCD tlcd = brick.getTextLCD(Font.getSmallFont()); + LCDPrinter.startLCDPrinter(tlcd); + System.setOut(LCDPrinter.getPrefixedPrintstream("out: ", tlcd)); + System.setErr(LCDPrinter.getPrefixedPrintstream("err: ", tlcd)); + ColorMemory mh = new ColorMemory(); - //BTController btController = BTController.getBTMaster("Rover2", mh); // Master - BTController btController = BTController.getBTSlave(mh); // Slave + LCDPrinter.print("Loading keylistener..."); + brick.getKey("Escape").addKeyListener(new ButtonListener()); + + BTController btController; + LCDPrinter.print("Trying to pair"); + btController = BTController.getBTMaster("Rover1", mh); // Master + //btController = BTController.getBTSlave(mh); // Slave btController.start(); - BTController.SendMessage("Hi"); LCDPrinter.print("Robots are connected"); - - LCDPrinter.print("Starting up systems"); - - - LCDPrinter.print("Loading keylistener..."); - brick.getKey("Escape").addKeyListener(new ButtonListener()); LCDPrinter.print("Loading motors..."); EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.D); @@ -42,28 +54,29 @@ public class Main { rightMotor.setAcceleration(1000); leftMotor.setAcceleration(1000); -// LCDPrinter.print("Loading touch sensors..."); -// EV3TouchSensor leftTouch = new EV3TouchSensor(brick.getPort("S1")); -// EV3TouchSensor rightTouch = new EV3TouchSensor(brick.getPort("S4")); + LCDPrinter.print("Loading touch sensors..."); + TouchSensor leftTouchSensor = new TouchSensor(new EV3TouchSensor(brick.getPort("S1"))); + TouchSensor rightTouchSensor = new TouchSensor(new EV3TouchSensor(brick.getPort("S4"))); LCDPrinter.print("Loading color sensor..."); - EV3ColorSensor colorSensor = new EV3ColorSensor(brick.getPort("S2")); - ColorSensor colorSensorObject = new ColorSensor(colorSensor); + ColorSensor colorSensorObject = new ColorSensor(new EV3ColorSensor(brick.getPort("S2"))); -// LCDPrinter.print("Loading ultrasone sensor..."); -// EV3UltrasonicSensor ultraSensor = new EV3UltrasonicSensor(brick.getPort("S3")); + LCDPrinter.print("Loading ultrasone sensor..."); + UltraSoneSensor ultraSoneSensor = new UltraSoneSensor(new EV3UltrasonicSensor(brick.getPort("S3"))); LCDPrinter.print("Initializing behaviours..."); Behavior[] behaviorList = new Behavior[] { new WandererBehaviour(colorSensorObject, leftMotor, rightMotor, mh), - new StayInFieldBehaviour(colorSensorObject, leftMotor, rightMotor) - //new CommBehaviour(leftMotor, rightMotor) + new StayInFieldBehaviour(colorSensorObject, leftMotor, rightMotor), + new AvoidLowObjectBehaviour(leftMotor, rightMotor, leftTouchSensor, rightTouchSensor), + new AvoidHighObjectBehaviour(leftMotor, rightMotor, ultraSoneSensor), + new ShutdownBehaviour(mh) }; LCDPrinter.print("Initializing arbitrator..."); Main.arbitrator = new Arbitrator(behaviorList); + LCDPrinter.print("Press any button for takeoff!"); + Button.waitForAnyPress(); Main.arbitrator.start(); - LCDPrinter.print("Takeoff!"); - colorSensor.close(); -// ultraSensor.close(); + LCDPrinter.print("Finish"); } } \ No newline at end of file diff --git a/mart/ev3/ex2/nl/ru/des/behaviours/AvoidHighObjectBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidHighObjectBehaviour.java new file mode 100644 index 0000000..67ebad5 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidHighObjectBehaviour.java @@ -0,0 +1,44 @@ +package nl.ru.des.behaviours; + +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.robotics.subsumption.Behavior; +import nl.ru.des.sensors.UltraSoneSensor; + +public class AvoidHighObjectBehaviour implements Behavior{ + private static final long TURNTIME = 250; + + private UltraSoneSensor ultraSone; + private EV3LargeRegulatedMotor leftMotor; + private EV3LargeRegulatedMotor rightMotor; + private boolean suppressed; + + public AvoidHighObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor, + UltraSoneSensor ultraSoneSensor) { + ultraSone = ultraSoneSensor; + this.leftMotor = leftMotor; + this.rightMotor = rightMotor; + } + + @Override + public boolean takeControl() { + return ultraSone.getCurrentStatus(); + } + + @Override + public void action() { + suppressed = false; + rightMotor.backward(); + leftMotor.forward(); + long time = System.currentTimeMillis(); + while (!suppressed && System.currentTimeMillis() - time > TURNTIME) { + Thread.yield(); + } + leftMotor.stop(true); + rightMotor.stop(true); + } + + @Override + public void suppress() { + suppressed = true; + } +} \ No newline at end of file diff --git a/mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java new file mode 100644 index 0000000..0ce1704 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java @@ -0,0 +1,71 @@ +package nl.ru.des.behaviours; + +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.robotics.subsumption.Behavior; +import nl.ru.des.sensors.TouchSensor; + +public class AvoidLowObjectBehaviour implements Behavior { + private static final long BACKWARDSTIME = 250; + private static final long TURNTIME = 250; + private TouchSensor rightTouch, leftTouch; + private boolean avoidDirection; + private boolean suppressed; + private EV3LargeRegulatedMotor leftMotor, rightMotor; + + public AvoidLowObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor, + TouchSensor leftTouchSensor, TouchSensor rightTouchSensor) { + rightTouch = rightTouchSensor; + leftTouch = leftTouchSensor; + this.leftMotor = leftMotor; + this.rightMotor = rightMotor; + } + + @Override + public boolean takeControl() { + if(leftTouch.getCurrentStatus()){ + avoidDirection = false; + return true; + } + if(rightTouch.getCurrentStatus()){ + avoidDirection = true; + return true; + } + return false; + } + + @Override + public void action() { + suppressed = false; + int leftacc = leftMotor.getAcceleration(); + int rightacc = rightMotor.getAcceleration(); + leftMotor.setAcceleration(10000); + rightMotor.setAcceleration(10000); + leftMotor.backward(); + rightMotor.backward(); + long time = System.currentTimeMillis(); + while (!suppressed && System.currentTimeMillis() - time > BACKWARDSTIME) { + Thread.yield(); + } + + if(avoidDirection){ + leftMotor.backward(); + rightMotor.forward(); + } else { + leftMotor.forward(); + rightMotor.backward(); + } + time = System.currentTimeMillis(); + while (!suppressed && System.currentTimeMillis() - time > TURNTIME) { + Thread.yield(); + } + leftMotor.stop(true); + leftMotor.stop(true); + leftMotor.setAcceleration(leftacc); + leftMotor.setAcceleration(rightacc); + } + + @Override + public void suppress() { + suppressed = true; + } +} \ No newline at end of file diff --git a/mart/ev3/ex2/nl/ru/des/behaviours/ShutdownBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/ShutdownBehaviour.java new file mode 100644 index 0000000..6fd5429 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/behaviours/ShutdownBehaviour.java @@ -0,0 +1,33 @@ +package nl.ru.des.behaviours; + +import lejos.robotics.subsumption.Behavior; +import nl.ru.des.LCDPrinter; +import nl.ru.des.bluetooth.ColorMemory; + +public class ShutdownBehaviour implements Behavior { + private ColorMemory cm; + private boolean suppressed; + + public ShutdownBehaviour(ColorMemory cm) { + this.cm = cm; + } + + @Override + public boolean takeControl() { + return cm.finished(); + } + + @Override + public void action() { + suppressed = false; + LCDPrinter.print("finish"); + while (!suppressed){ + Thread.yield(); + } + } + + @Override + public void suppress() { + suppressed = true; + } +} diff --git a/mart/ev3/ex2/nl/ru/des/StayInFieldBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/StayInFieldBehaviour.java similarity index 76% rename from mart/ev3/ex2/nl/ru/des/StayInFieldBehaviour.java rename to mart/ev3/ex2/nl/ru/des/behaviours/StayInFieldBehaviour.java index 7534d3c..3d9e7f6 100644 --- a/mart/ev3/ex2/nl/ru/des/StayInFieldBehaviour.java +++ b/mart/ev3/ex2/nl/ru/des/behaviours/StayInFieldBehaviour.java @@ -1,25 +1,28 @@ -package nl.ru.des; +package nl.ru.des.behaviours; import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.robotics.Color; import lejos.robotics.subsumption.Behavior; +import nl.ru.des.sensors.ColorSensor; public class StayInFieldBehaviour implements Behavior { public static final long BACKWARDSTIME = 250; public static final long TURNTIME = 250; - + private ColorSensor colorSensor; private EV3LargeRegulatedMotor leftMotor, rightMotor; private boolean suppressed; - - public StayInFieldBehaviour(ColorSensor colorSensor, EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor) { + + public StayInFieldBehaviour(ColorSensor colorSensor, EV3LargeRegulatedMotor leftMotor, + EV3LargeRegulatedMotor rightMotor) { this.colorSensor = colorSensor; this.leftMotor = leftMotor; this.rightMotor = rightMotor; } - + @Override public boolean takeControl() { - return colorSensor.getCurrentColor() == "Black"; + return colorSensor.getCurrentColor() == Color.BLACK; } @Override @@ -32,14 +35,14 @@ public class StayInFieldBehaviour implements Behavior { leftMotor.backward(); rightMotor.backward(); long time = System.currentTimeMillis(); - while(!suppressed && System.currentTimeMillis()-time > BACKWARDSTIME){ + while (!suppressed && System.currentTimeMillis() - time > BACKWARDSTIME) { Thread.yield(); } leftMotor.forward(); rightMotor.backward(); time = System.currentTimeMillis(); - while(!suppressed && System.currentTimeMillis()-time > TURNTIME){ + while (!suppressed && System.currentTimeMillis() - time > TURNTIME) { Thread.yield(); } leftMotor.stop(true); diff --git a/mart/ev3/ex2/nl/ru/des/WandererBehaviour.java b/mart/ev3/ex2/nl/ru/des/behaviours/WandererBehaviour.java similarity index 77% rename from mart/ev3/ex2/nl/ru/des/WandererBehaviour.java rename to mart/ev3/ex2/nl/ru/des/behaviours/WandererBehaviour.java index a0c8bcc..ec16941 100644 --- a/mart/ev3/ex2/nl/ru/des/WandererBehaviour.java +++ b/mart/ev3/ex2/nl/ru/des/behaviours/WandererBehaviour.java @@ -1,8 +1,11 @@ -package nl.ru.des; +package nl.ru.des.behaviours; import lejos.hardware.Sound; import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.robotics.Color; import lejos.robotics.subsumption.Behavior; +import nl.ru.des.bluetooth.ColorMemory; +import nl.ru.des.sensors.ColorSensor; public class WandererBehaviour implements Behavior { private EV3LargeRegulatedMotor leftMotor, rightMotor; @@ -31,10 +34,10 @@ public class WandererBehaviour implements Behavior { leftMotor.forward(); rightMotor.forward(); while (!suppressed) { - String current = colorSensor.getCurrentColor(); - if (current == "Yellow" || current == "Blue" || current == "Red") { + int current = colorSensor.getCurrentColor(); + if (current == Color.YELLOW || current == Color.BLUE || current == Color.RED) { Sound.beep(); - BTController.SendMessage(current); + cm.addColor(current); } Thread.yield(); } @@ -46,5 +49,4 @@ public class WandererBehaviour implements Behavior { public void suppress() { suppressed = true; } - } diff --git a/mart/ev3/ex2/nl/ru/des/BTMemory.java b/mart/ev3/ex2/nl/ru/des/bluetooth/BTController.java similarity index 93% rename from mart/ev3/ex2/nl/ru/des/BTMemory.java rename to mart/ev3/ex2/nl/ru/des/bluetooth/BTController.java index 75fc60a..0a9102b 100644 --- a/mart/ev3/ex2/nl/ru/des/BTMemory.java +++ b/mart/ev3/ex2/nl/ru/des/bluetooth/BTController.java @@ -1,4 +1,4 @@ -package nl.ru.des; +package nl.ru.des.bluetooth; import java.io.DataInputStream; import java.io.DataOutputStream; @@ -32,10 +32,11 @@ public class BTController extends Thread { while (true) { try { int c = dataInput.readUnsignedByte(); - sb.appendCodePoint(c); if (c == '\n') { sh.handleMessage(sb.toString()); sb = new StringBuilder(); + } else { + sb.appendCodePoint(c); } } catch (IOException e) { e.printStackTrace(); @@ -57,7 +58,7 @@ public class BTController extends Thread { } } - public static void SendMessage(String message) { + public static void sendMessage(String message) { messageBuffer.add(message + "\n"); } diff --git a/mart/ev3/ex2/nl/ru/des/bluetooth/ColorMemory.java b/mart/ev3/ex2/nl/ru/des/bluetooth/ColorMemory.java new file mode 100644 index 0000000..5e89606 --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/bluetooth/ColorMemory.java @@ -0,0 +1,36 @@ +package nl.ru.des.bluetooth; + +import java.util.Arrays; +import java.util.HashSet; +import java.util.List; +import java.util.Set; + +import lejos.robotics.Color; +import nl.ru.des.LCDPrinter; + +public class ColorMemory implements MessageHandler { + private List COLORSTOFIND = Arrays.asList(new Integer[] {Color.BLUE, Color.RED, Color.YELLOW}); + private Set colors; + + public ColorMemory() { + colors = new HashSet(); + } + + @Override + public void handleMessage(String message) { + colors.add(Integer.valueOf(message)); + LCDPrinter.print(message + " received"); + LCDPrinter.print(colors.toString()); + } + + public boolean finished() { + return colors.containsAll(COLORSTOFIND); + } + + public void addColor(int current) { + BTController.sendMessage(Integer.toString(current)); + colors.add(current); + LCDPrinter.print(current + " found"); + LCDPrinter.print(colors.toString()); + } +} diff --git a/mart/ev3/ex2/nl/ru/des/MessageHandler.java b/mart/ev3/ex2/nl/ru/des/bluetooth/MessageHandler.java similarity index 72% rename from mart/ev3/ex2/nl/ru/des/MessageHandler.java rename to mart/ev3/ex2/nl/ru/des/bluetooth/MessageHandler.java index 3f4abb6..757ac06 100644 --- a/mart/ev3/ex2/nl/ru/des/MessageHandler.java +++ b/mart/ev3/ex2/nl/ru/des/bluetooth/MessageHandler.java @@ -1,4 +1,4 @@ -package nl.ru.des; +package nl.ru.des.bluetooth; public interface MessageHandler { public void handleMessage(String message); diff --git a/mart/ev3/ex2/nl/ru/des/sensors/ColorSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/ColorSensor.java new file mode 100644 index 0000000..884e0ef --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/sensors/ColorSensor.java @@ -0,0 +1,14 @@ +package nl.ru.des.sensors; + +import lejos.hardware.sensor.EV3ColorSensor; + +public class ColorSensor extends SuperSensor{ + public ColorSensor(EV3ColorSensor colorSensor){ + super(colorSensor.getColorIDMode()); + } + + public int getCurrentColor(){ + fetchSample(); + return (int) samples[0]; + } +} diff --git a/mart/ev3/ex2/nl/ru/des/sensors/SuperSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/SuperSensor.java new file mode 100644 index 0000000..affddbe --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/sensors/SuperSensor.java @@ -0,0 +1,29 @@ +package nl.ru.des.sensors; + +import lejos.robotics.SampleProvider; + +public abstract class SuperSensor { + public static final long SAMPLETIME = 50; + + private long lastSampleTaken; + private SampleProvider sampleProvider; + + protected float[] samples; + + public SuperSensor(SampleProvider sampleProvider){ + this.sampleProvider = sampleProvider; + this.samples = new float[sampleProvider.sampleSize()]; + this.lastSampleTaken = System.currentTimeMillis()-SAMPLETIME; + } + + public void fetchSample(){ + fetchSample(false); + } + + public void fetchSample(boolean always){ + if(always || System.currentTimeMillis()-lastSampleTaken > SAMPLETIME){ + lastSampleTaken = System.currentTimeMillis(); + sampleProvider.fetchSample(samples, 0); + } + } +} diff --git a/mart/ev3/ex2/nl/ru/des/sensors/TouchSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/TouchSensor.java new file mode 100644 index 0000000..864635b --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/sensors/TouchSensor.java @@ -0,0 +1,15 @@ +package nl.ru.des.sensors; + +import lejos.hardware.sensor.EV3TouchSensor; + +public class TouchSensor extends SuperSensor { + + public TouchSensor(EV3TouchSensor leftTouch) { + super(leftTouch.getTouchMode()); + } + + public boolean getCurrentStatus(){ + fetchSample(); + return samples[0] == 1; + } +} diff --git a/mart/ev3/ex2/nl/ru/des/sensors/UltraSoneSensor.java b/mart/ev3/ex2/nl/ru/des/sensors/UltraSoneSensor.java new file mode 100644 index 0000000..0f0a10b --- /dev/null +++ b/mart/ev3/ex2/nl/ru/des/sensors/UltraSoneSensor.java @@ -0,0 +1,27 @@ +package nl.ru.des.sensors; + +import lejos.hardware.Button; +import lejos.hardware.sensor.EV3UltrasonicSensor; +import nl.ru.des.LCDPrinter; + +public class UltraSoneSensor extends SuperSensor { + private float limit; + + public UltraSoneSensor(EV3UltrasonicSensor ultraSone) { + super(ultraSone.getDistanceMode()); + calibrate(); + } + + private void calibrate(){ + LCDPrinter.print("Calibrate ultrasone, place object in the turn radius..."); + Button.waitForAnyPress(); + fetchSample(true); + limit = samples[0]; + LCDPrinter.print("Limit: " + limit); + } + + public boolean getCurrentStatus(){ + fetchSample(); + return samples[0]