-Name MartNatanael
Acceleration 1000
Speed 200
Behaviour Wander
left motor backward
wait 250 ms
Behaviour AvoidHighObjects
- take control: Distance < 50 cm
+ take control:
action:
right motor forward
wait 250 ms
--- /dev/null
+package nl.ru.des;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.LinkedList;
+import java.util.Queue;
+
+import lejos.remote.nxt.BTConnector;
+import lejos.remote.nxt.NXTConnection;
+
+public class BTController{
+ public static void startMaster(final String slave, final MessageHandler sh) {
+ BTConnector btconnector = new BTConnector();
+ NXTConnection conn = btconnector.connect(slave, NXTConnection.RAW);
+ final DataInputStream dataInput = conn.openDataInputStream();
+ new Thread() {
+ @Override
+ public void run() {
+ StringBuilder sb = new StringBuilder();
+ while (true) {
+ try {
+ int c = dataInput.readUnsignedByte();
+ if (c == '\n') {
+ sh.handleMessage(sb.toString());
+ sb = new StringBuilder();
+ } else {
+ sb.appendCodePoint(c);
+ }
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ }
+ }
+ }.start();
+ }
+
+ public static Queue<String> startSlave() {
+ BTConnector btconnector = new BTConnector();
+ NXTConnection conn = btconnector.waitForConnection(60000, NXTConnection.RAW);
+ final DataOutputStream dataOutput = conn.openDataOutputStream();
+ final Queue<String> buf = new LinkedList<String>();
+ new Thread(){
+ @Override public void run(){
+ while (true) {
+ if (!buf.isEmpty()) {
+ try {
+ dataOutput.write(buf.poll().getBytes());
+ dataOutput.flush();
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ }
+ Thread.yield();
+ }
+ }
+ }.start();
+ return buf;
+ }
+}
\ No newline at end of file
+++ /dev/null
-package nl.ru.des;
-
-import java.util.LinkedList;
-
-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.SampleProvider;
-import lejos.robotics.subsumption.Arbitrator;
-
-public class MarsRover {
- public static final float SAMPLERATE = 100;
- public static Arbitrator arb;
-
- @SuppressWarnings("resource")
- public static void main(String[] args) {
- EV3 brick = LocalEV3.get();
- TextLCD tlcd = brick.getTextLCD(Font.getSmallFont());
- LCDPrinter.startLCDPrinter(tlcd);
- System.setOut(LCDPrinter.getPrefixedPrintstream("out: ", tlcd));
- System.setErr(LCDPrinter.getPrefixedPrintstream("err: ", tlcd));
-
- LCDPrinter.print("Loading keylistener...");
- brick.getKey("Escape").addKeyListener(new ButtonListener());
-
- LCDPrinter.print("Loading motors...");
- EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.D);
- EV3LargeRegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.A);
- leftMotor.setSpeed(Constants.speed);
- rightMotor.setSpeed(Constants.speed);
- rightMotor.setAcceleration(Constants.acceleration);
- leftMotor.setAcceleration(Constants.acceleration);
-
- LCDPrinter.print("Loading touch sensors...");
- SampleProvider leftTouch = new EV3TouchSensor(brick.getPort("S1")).getTouchMode();
- SampleProvider rightTouch = new EV3TouchSensor(brick.getPort("S4")).getTouchMode();
-
- LCDPrinter.print("Loading color sensor...");
- SampleProvider color = new EV3ColorSensor(brick.getPort("S2")).getColorIDMode();
-
- LCDPrinter.print("Loading ultrasone sensor...");
- SampleProvider ultraSonic = new EV3UltrasonicSensor(brick.getPort("S3")).getDistanceMode();
-
- LCDPrinter.print("Initializing behaviours...");
- SensorCollector sensors = new SensorCollector(ultraSonic, color, leftTouch, rightTouch);
-
- LCDPrinter.print("Initializing color collector...");
- ColorMemory colorMemory = new ColorMemory(color);
-
- Arbitrator a;
- LinkedList<Mission> missions = Missions.getMissions(sensors, rightMotor, leftMotor, colorMemory);
- for(Mission m : missions){
- LCDPrinter.print("Start " + m.name + " mission...");
- a = new Arbitrator(m.behaviours);
- a.start();
- }
- }
-}
\ No newline at end of file
--- /dev/null
+package nl.ru.des;
+
+import java.util.Queue;
+
+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.EV3GyroSensor;
+import lejos.hardware.sensor.EV3TouchSensor;
+import lejos.hardware.sensor.EV3UltrasonicSensor;
+import lejos.hardware.sensor.NXTLightSensor;
+import lejos.robotics.SampleProvider;
+import lejos.robotics.subsumption.Arbitrator;
+
+public class Marster {
+ public static Arbitrator arb;
+
+ @SuppressWarnings("resource")
+ public static void main(String[] args) {
+ EV3 brick = LocalEV3.get();
+ TextLCD tlcd = brick.getTextLCD(Font.getSmallFont());
+ LCDPrinter.startLCDPrinter(tlcd);
+ System.setOut(LCDPrinter.getPrefixedPrintstream("out: ", tlcd));
+ System.setErr(LCDPrinter.getPrefixedPrintstream("err: ", tlcd));
+
+ LCDPrinter.print("Loading keylistener...");
+ brick.getKey("Escape").addKeyListener(new ButtonListener());
+
+ if(brick.getName().equalsIgnoreCase("Rover6") || brick.getName().equalsIgnoreCase("Rover8")){
+ LCDPrinter.print("Starting as a slave...");
+ LCDPrinter.print("Loading touch sensors...");
+ SampleProvider leftTouch = new EV3TouchSensor(brick.getPort("S1")).getTouchMode();
+ SampleProvider rightTouch = new EV3TouchSensor(brick.getPort("S2")).getTouchMode();
+
+ LCDPrinter.print("Loading ultrasone sensor...");
+ SampleProvider frontUltra = new EV3UltrasonicSensor(brick.getPort("S3")).getDistanceMode();
+
+ LCDPrinter.print("Loading color sensors...");
+ SampleProvider color = new EV3ColorSensor(brick.getPort("S4")).getColorIDMode();
+ RemoteSensors rs = new RemoteSensors(leftTouch, rightTouch, frontUltra, color);
+
+ LCDPrinter.print("Start BT...");
+ Queue<String> msgs = BTController.startSlave();
+ rs.start(msgs);
+ } else {
+ LCDPrinter.print("Starting as as master...");
+ LCDPrinter.print("Loading motors...");
+ EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.A);
+ EV3LargeRegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.B);
+ EV3LargeRegulatedMotor measMotor = new EV3LargeRegulatedMotor(MotorPort.C);
+// leftMotor.setSpeed(Constants.speed);
+// rightMotor.setSpeed(Constants.speed);
+// measMotor.setSpeed(100);
+// rightMotor.setAcceleration(Constants.acceleration);
+// leftMotor.setAcceleration(Constants.acceleration);
+// measMotor.setAcceleration(100);
+
+ LCDPrinter.print("Loading touch sensors...");
+ SampleProvider leftLight = new NXTLightSensor(brick.getPort("S1")).getRedMode();
+ SampleProvider rightLight = new NXTLightSensor(brick.getPort("S2")).getRedMode();
+
+ LCDPrinter.print("Loading ultrasone sensor...");
+ SampleProvider backUltra = new EV3UltrasonicSensor(brick.getPort("S3")).getDistanceMode();
+
+ LCDPrinter.print("Loading gyro sensor...");
+ SampleProvider gyro = new EV3GyroSensor(brick.getPort("S4")).getAngleAndRateMode();
+
+ LCDPrinter.print("Start BT...");
+ BTController.startMaster(brick.getName() == "Rover5" ? "Rover6" : "Rover8", new SensorCollector(backUltra, leftLight, rightLight, gyro));
+ LCDPrinter.print("Finished loading");
+ Button.waitForAnyPress();
+// Arbitrator a;
+// LinkedList<Mission> missions = Missions.getMissions(sensors, rightMotor, leftMotor, colorMemory);
+// for(Mission m : missions){
+// LCDPrinter.print("Start " + m.name + " mission...");
+// a = new Arbitrator(m.behaviours);
+// a.start();
+// }
+ }
+ }
+}
\ No newline at end of file
--- /dev/null
+package nl.ru.des;
+
+public interface MessageHandler {
+ public void handleMessage(String message);
+}
--- /dev/null
+package nl.ru.des;
+
+import java.util.Queue;
+
+import lejos.robotics.SampleProvider;
+
+public class RemoteSensors{
+ public static final int DELAY = 250;
+ private SampleProvider left, right, ultra, color;
+ private float[] leftSamples, rightSamples, ultraSamples, colorSamples;
+ private float leftLatest, rightLatest, ultraLatest, colorLatest;
+
+ public enum RemoteSensorEnum{
+ LEFT, RIGHT, ULTRA, COLOR;
+ }
+
+ public RemoteSensors(SampleProvider left, SampleProvider right, SampleProvider ultra, SampleProvider color){
+ this.left = left;
+ this.right = right;
+ this.ultra = ultra;
+ this.color = color;
+ leftSamples = new float[left.sampleSize()];
+ rightSamples = new float[right.sampleSize()];
+ ultraSamples = new float[ultra.sampleSize()];
+ colorSamples = new float[color.sampleSize()];
+ }
+
+ public void start(Queue<String> q){
+ long last = System.currentTimeMillis();
+ LCDPrinter.print("Start sending values...");
+ while(true){
+ if(System.currentTimeMillis()-last > DELAY && q.size()<5){
+ last = System.currentTimeMillis();
+ left.fetchSample(leftSamples, 0);
+ if(leftSamples[0] != leftLatest){
+ leftLatest = leftSamples[0];
+ q.add(Integer.toString(RemoteSensorEnum.LEFT.ordinal())+Integer.toString((int)leftLatest)+"\n");
+ }
+ right.fetchSample(rightSamples, 0);
+ if(rightSamples[0] != rightLatest){
+ rightLatest = rightSamples[0];
+ q.add(Integer.toString(RemoteSensorEnum.RIGHT.ordinal())+Integer.toString((int)rightLatest)+"\n");
+ }
+
+ ultra.fetchSample(ultraSamples, 0);
+ if(ultraSamples[0] != ultraLatest){
+ ultraLatest = ultraSamples[0];
+ q.add(Integer.toString(RemoteSensorEnum.ULTRA.ordinal())+Float.toString(ultraLatest)+"\n");
+ }
+
+ color.fetchSample(colorSamples, 0);
+ if(colorSamples[0] != colorLatest){
+ colorLatest = colorSamples[0];
+ q.add(Integer.toString(RemoteSensorEnum.COLOR.ordinal())+Integer.toString((int)colorLatest)+"\n");
+ }
+ }
+ Thread.yield();
+ }
+ }
+}
package nl.ru.des;
-import java.util.Arrays;
-import java.util.HashSet;
-import java.util.Set;
-
import lejos.robotics.SampleProvider;
-public class SensorCollector{
+public class SensorCollector implements MessageHandler{
public static final int DELAY = 300;
- private SampleProvider ultrasone, leftTouch, rightTouch;
- private float[] ultrasoneSamples, colorSamples, leftTouchSamples, rightTouchSamples;
- private long ultrasoneTime, leftTouchTime, rightTouchTime;
- private Set<Integer> colorsCollected;
-
- public SensorCollector(SampleProvider ultrasone,
- final SampleProvider color,
- SampleProvider leftTouch,
- SampleProvider rightTouch){
- this.ultrasone = ultrasone;
- this.leftTouch = leftTouch;
- this.rightTouch = rightTouch;
- ultrasoneSamples = new float[ultrasone.sampleSize()];
- colorSamples = new float[color.sampleSize()];
- leftTouchSamples = new float[leftTouch.sampleSize()];
- rightTouchSamples = new float[rightTouch.sampleSize()];
- ultrasoneTime = System.currentTimeMillis();
- leftTouchTime = System.currentTimeMillis();
- rightTouchTime = System.currentTimeMillis();
- colorsCollected = new HashSet<Integer>();
- new Thread(){
- @Override public void run(){
- long time = System.currentTimeMillis();
- while(true){
- if(System.currentTimeMillis() - time > DELAY){
- color.fetchSample(colorSamples, 0);
- colorsCollected.add(color());
- time = System.currentTimeMillis();
- }
- Thread.yield();
- }
- }
- }.start();
+ //Local sensors
+ private SampleProvider ultra, leftLight, rightLight, gyro;
+ private float[] ultraSamples, leftLightSamples, rightLightSamples, gyroSamples;
+ private long ultraTime, leftLightTime, rightLightTime, gyroTime;
+
+ //Remote sensors
+ private int color;
+ private boolean leftTouch, rightTouch;
+ private float frontUltra;
+
+ public SensorCollector(SampleProvider ultra,
+ SampleProvider leftLight,
+ SampleProvider rightLight,
+ SampleProvider gyro){
+ this.ultra = ultra;
+ this.leftLight = leftLight;
+ this.rightLight = rightLight;
+ this.gyro = gyro;
+ ultraSamples = new float[ultra.sampleSize()];
+ gyroSamples = new float[gyro.sampleSize()];
+ leftLightSamples = new float[leftLight.sampleSize()];
+ rightLightSamples = new float[rightLight.sampleSize()];
+ ultraTime = System.currentTimeMillis();
+ leftLightTime = System.currentTimeMillis();
+ rightLightTime = System.currentTimeMillis();
+ gyroTime = System.currentTimeMillis();
+ }
+
+ //Local sensors
+ public float backDistance(){
+ if(System.currentTimeMillis()-ultraTime>DELAY){
+ ultra.fetchSample(ultraSamples, 0);
+ ultraTime = System.currentTimeMillis();
+ }
+ return ultraSamples[0];
+ }
+
+ public boolean leftLight(){
+ if(System.currentTimeMillis()-leftLightTime>DELAY){
+ leftLight.fetchSample(leftLightSamples, 0);
+ leftLightTime = System.currentTimeMillis();
+ }
+ return leftLightSamples[0]>0.5;
}
- public float distance(){
- if(System.currentTimeMillis()-ultrasoneTime>DELAY){
- ultrasone.fetchSample(ultrasoneSamples, 0);
- ultrasoneTime = System.currentTimeMillis();
+ public boolean rightLight(){
+ if(System.currentTimeMillis()-rightLightTime>DELAY){
+ rightLight.fetchSample(rightLightSamples, 0);
+ rightLightTime = System.currentTimeMillis();
}
- return ultrasoneSamples[0];
+ return rightLightSamples[0]>0.5;
}
+ public float gyro(){
+ if(System.currentTimeMillis()-gyroTime>DELAY){
+ gyro.fetchSample(gyroSamples, 0);
+ gyroTime = System.currentTimeMillis();
+ }
+ return gyroSamples[0];
+ }
+
+ //Remote sensors
public int color(){
- return (int)colorSamples[0];
+ return color;
}
public boolean leftTouch(){
- if(System.currentTimeMillis()-leftTouchTime>DELAY){
- leftTouch.fetchSample(leftTouchSamples, 0);
- leftTouchTime = System.currentTimeMillis();
- }
- return leftTouchSamples[0]==1;
+ return leftTouch;
}
public boolean rightTouch(){
- if(System.currentTimeMillis()-rightTouchTime>DELAY){
- rightTouch.fetchSample(rightTouchSamples, 0);
- rightTouchTime = System.currentTimeMillis();
- }
- return rightTouchSamples[0]==1;
+ return rightTouch;
+ }
+
+ public float frontDistance(){
+ return frontUltra;
}
- public boolean collected(int[] is) {
- return colorsCollected.containsAll(Arrays.asList(is));
+ @Override
+ public void handleMessage(String m) {
+ LCDPrinter.print(m);
+ String s = m.substring(1);
+ switch(RemoteSensors.RemoteSensorEnum.values()[Integer.valueOf(m.charAt(0))]){
+ case COLOR:
+ color = Integer.valueOf(s);
+ break;
+ case LEFT:
+ leftTouch = Integer.valueOf(s)==1;
+ break;
+ case RIGHT:
+ rightTouch = Integer.valueOf(s)==1;
+ break;
+ case ULTRA:
+ frontUltra = Float.valueOf(s);
+ break;
+ }
}
}
\ No newline at end of file
@Override public void action(){
LCDPrinter.print("Terminate mission");
- MarsRover.arb.stop();
+ Marster.arb.stop();
}
}
generate taskDSL "http://www.missions.robots/TaskDSL"
Robot:
- 'Name' name=ID
'Acceleration' acc=INT
'Speed' spd=INT
behaviour+=Behaviour+
mission+=Mission+;
-Mission: 'Mission' name=ID 'using' behaviours+=[Behaviour]+ 'and stops when' se=StoppingExpression;
+Mission:
+ 'Mission' name=ID 'using' behaviours+=[Behaviour]+ 'and stops when' se=StoppingExpression;
StoppingExpression:
'(' op=Operator s+=StoppingExpression s+=StoppingExpression+ ')' |
+ 'not ' negscond=StoppingCondition |
scond=StoppingCondition;
StoppingCondition:
'Collected at least' colors+=Color+ |
'Touched on' touch=LeftRight |
- 'Distance' op=Comparison dist=INT 'cm' |
+ 'Light on' light=LeftRight |
+ 'No object in sight on ' dist=BackFront |
'Color is' color=Color;
Behaviour: 'Behaviour' name=ID
Action:
whichMotor=LeftRight 'motor' dir=Direction ('with speed' spd=INT 'acceleration' acc=INT)? |
+ 'measure' measureWhat=RockLake |
'wait' time=Time;
Time: time=INT 'ms' | {Time} 'forever';
+RockLake: d=RockLakeE;
+enum RockLakeE: ROCK='rock' | LAKE='lake';
Direction: d=DirectionE;
enum DirectionE: BACKWARDS = 'backward' | FORWARDS = 'forward';
Operator: d=OperatorE;
enum OperatorE: AND = 'and' | OR = 'or';
+BackFront: d=BackFrontE;
+enum BackFrontE: BACK = 'back' | FRONT = 'front';
Comparison: d=ComparisonE;
enum ComparisonE: GE='>' | LE='<';
LeftRight: d=LeftRightE;
import org.eclipse.xtext.generator.IGenerator
import robots.missions.taskDSL.Behaviour
import robots.missions.taskDSL.Mission
-import robots.missions.taskDSL.OperatorE
import robots.missions.taskDSL.Robot
-import robots.missions.taskDSL.StoppingExpression
/**
* Generates code from your model files on save.
var root = resource.allContents.head as Robot;
if(root != null){
fsa.generateFile("nl/ru/des/Constants.java", makeConstants(root));
- fsa.generateFile("nl/ru/des/Behaviours.java", makeBehaviours(root.behaviour));
- fsa.generateFile("nl/ru/des/Missions.java", makeMissions(root.mission));
+ //fsa.generateFile("nl/ru/des/Behaviours.java", makeBehaviours(root.behaviour));
+ //fsa.generateFile("nl/ru/des/Missions.java", makeMissions(root.mission));
}
}
«ENDFOR»,
new ShutdownBehaviour(sensors, rightMotor, leftMotor, colors){
@Override public boolean takeControl(){
- return «printExpression(m.se)»;
+ return »;
}
}}
«ENDFOR»));
}
«IF b.tc != null»
@Override public boolean takeControl(){
- return «printExpression(b.tc)»;
+ return ;
}
«ENDIF»
«ENDFOR»
}
'''
-
- def CharSequence printExpression(StoppingExpression e)'''
- «IF e.scond != null»
- «IF !e.scond.colors.nullOrEmpty»
- sensors.collected(new int[]{«FOR c : e.scond.colors SEPARATOR ","»«c.d.ordinal»«ENDFOR»})
- «ELSEIF e.scond.touch != null»
- sensors.«e.scond.touch.d.toString()»Touch()
- «ELSEIF e.scond.op != null»
- sensors.distance() «e.scond.op.d.toString()» «e.scond.dist»
- «ELSEIF e.scond.color != null»
- sensors.color() == «e.scond.color.d.ordinal»
- «ENDIF»
- «ELSE»
- «IF e.op.d.equals(OperatorE.AND)»
- «FOR ex : e.s BEFORE "(" SEPARATOR "&&" AFTER ")"»«printExpression(ex)»«ENDFOR»
- «ELSE»
- «FOR ex : e.s BEFORE "(" SEPARATOR "&&" AFTER ")"»«printExpression(ex)»«ENDFOR»
- «ENDIF»
- «ENDIF»
- '''
+
def CharSequence makeConstants(Robot robot)'''
package nl.ru.des;
repositories / des2015.git / commitdiff