From: Mart Lubbers Date: Thu, 3 Dec 2015 15:32:22 +0000 (+0100) Subject: started with new rover, bt still not worknig:( X-Git-Url: https://git.martlubbers.net/?a=commitdiff_plain;h=34dae1e7f9c49481d90b3d26978cc5ab52ccc050;p=des2015.git started with new rover, bt still not worknig:( --- diff --git a/dsl/runtime/specs/spec1.tdsl b/dsl/runtime/specs/spec1.tdsl index b1d6f6a..57bd7ba 100644 --- a/dsl/runtime/specs/spec1.tdsl +++ b/dsl/runtime/specs/spec1.tdsl @@ -1,4 +1,3 @@ -Name MartNatanael Acceleration 1000 Speed 200 Behaviour Wander @@ -26,7 +25,7 @@ Behaviour AvoidLowRightObjects left motor backward wait 250 ms Behaviour AvoidHighObjects - take control: Distance < 50 cm + take control: action: right motor forward wait 250 ms diff --git a/dsl/runtime/src/nl/ru/des/BTController.java b/dsl/runtime/src/nl/ru/des/BTController.java new file mode 100644 index 0000000..5c75784 --- /dev/null +++ b/dsl/runtime/src/nl/ru/des/BTController.java @@ -0,0 +1,60 @@ +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 startSlave() { + BTConnector btconnector = new BTConnector(); + NXTConnection conn = btconnector.waitForConnection(60000, NXTConnection.RAW); + final DataOutputStream dataOutput = conn.openDataOutputStream(); + final Queue buf = new LinkedList(); + 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 diff --git a/dsl/runtime/src/nl/ru/des/MarsRover.java b/dsl/runtime/src/nl/ru/des/MarsRover.java deleted file mode 100644 index 8561c0a..0000000 --- a/dsl/runtime/src/nl/ru/des/MarsRover.java +++ /dev/null @@ -1,64 +0,0 @@ -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 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 diff --git a/dsl/runtime/src/nl/ru/des/Marster.java b/dsl/runtime/src/nl/ru/des/Marster.java new file mode 100644 index 0000000..b31bb87 --- /dev/null +++ b/dsl/runtime/src/nl/ru/des/Marster.java @@ -0,0 +1,86 @@ +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 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 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 diff --git a/dsl/runtime/src/nl/ru/des/MessageHandler.java b/dsl/runtime/src/nl/ru/des/MessageHandler.java new file mode 100644 index 0000000..ecd00e9 --- /dev/null +++ b/dsl/runtime/src/nl/ru/des/MessageHandler.java @@ -0,0 +1,5 @@ +package nl.ru.des; + +public interface MessageHandler { + public void handleMessage(String message); +} diff --git a/dsl/runtime/src/nl/ru/des/RemoteSensors.java b/dsl/runtime/src/nl/ru/des/RemoteSensors.java new file mode 100644 index 0000000..afebb11 --- /dev/null +++ b/dsl/runtime/src/nl/ru/des/RemoteSensors.java @@ -0,0 +1,60 @@ +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 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(); + } + } +} diff --git a/dsl/runtime/src/nl/ru/des/SensorCollector.java b/dsl/runtime/src/nl/ru/des/SensorCollector.java index 529559b..9a58ae4 100644 --- a/dsl/runtime/src/nl/ru/des/SensorCollector.java +++ b/dsl/runtime/src/nl/ru/des/SensorCollector.java @@ -1,78 +1,105 @@ 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 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(); - 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 diff --git a/dsl/runtime/src/nl/ru/des/ShutdownBehaviour.java b/dsl/runtime/src/nl/ru/des/ShutdownBehaviour.java index 430b5df..ba433ba 100644 --- a/dsl/runtime/src/nl/ru/des/ShutdownBehaviour.java +++ b/dsl/runtime/src/nl/ru/des/ShutdownBehaviour.java @@ -10,6 +10,6 @@ public class ShutdownBehaviour extends BasicBehaviour{ @Override public void action(){ LCDPrinter.print("Terminate mission"); - MarsRover.arb.stop(); + Marster.arb.stop(); } } diff --git a/dsl/xtend/src/robots/missions/TaskDSL.xtext b/dsl/xtend/src/robots/missions/TaskDSL.xtext index 7a2de03..1dd8afd 100644 --- a/dsl/xtend/src/robots/missions/TaskDSL.xtext +++ b/dsl/xtend/src/robots/missions/TaskDSL.xtext @@ -3,22 +3,24 @@ grammar robots.missions.TaskDSL with org.eclipse.xtext.common.Terminals 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 @@ -27,14 +29,19 @@ 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; diff --git a/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend b/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend index d7ee2ed..d4f6915 100644 --- a/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend +++ b/dsl/xtend/src/robots/missions/generator/TaskDSLGenerator.xtend @@ -9,9 +9,7 @@ import org.eclipse.xtext.generator.IFileSystemAccess 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. @@ -24,8 +22,8 @@ class TaskDSLGenerator implements IGenerator { 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)); } } @@ -49,7 +47,7 @@ public class Missions{ «ENDFOR», new ShutdownBehaviour(sensors, rightMotor, leftMotor, colors){ @Override public boolean takeControl(){ - return «printExpression(m.se)»; + return »; } }} «ENDFOR»)); @@ -72,7 +70,7 @@ public class Behaviours{ } «IF b.tc != null» @Override public boolean takeControl(){ - return «printExpression(b.tc)»; + return ; } «ENDIF» @@ -98,26 +96,7 @@ public class Behaviours{ «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;