From 7ac35f28a497acbf523efd2685d95cbefc2e3102 Mon Sep 17 00:00:00 2001 From: Mart Lubbers Date: Thu, 15 Oct 2015 17:09:05 +0200 Subject: [PATCH] working robot --- mart/ev3/.gitignore | 1 + mart/ev3/ex1/nl/ru/des/AvoidBehaviour.java | 37 ++++++++++ .../nl/ru/des/AvoidHighObjectBehaviour.java | 38 +++++++++++ .../nl/ru/des/AvoidLowObjectBehaviour.java | 53 +++++++++++++++ mart/ev3/ex1/nl/ru/des/ButtonListener.java | 6 +- mart/ev3/ex1/nl/ru/des/LCDPrintBehaviour.java | 24 ------- mart/ev3/ex1/nl/ru/des/LCDPrinter.java | 39 +++++++++++ mart/ev3/ex1/nl/ru/des/Main.java | 68 +++++++++++++++++-- .../ex1/nl/ru/des/StayInFieldBehaviour.java | 48 +++++++++++++ mart/ev3/ex1/nl/ru/des/WandererBehaviour.java | 11 +-- mart/ev3/ex1/nl/ru/des/WavPlayer.java | 31 +++++++++ mart/ev3/ex1/nl/ru/des/sounds/makeall.sh | 7 ++ 12 files changed, 321 insertions(+), 42 deletions(-) create mode 100644 mart/ev3/.gitignore create mode 100644 mart/ev3/ex1/nl/ru/des/AvoidBehaviour.java create mode 100644 mart/ev3/ex1/nl/ru/des/AvoidHighObjectBehaviour.java create mode 100644 mart/ev3/ex1/nl/ru/des/AvoidLowObjectBehaviour.java delete mode 100644 mart/ev3/ex1/nl/ru/des/LCDPrintBehaviour.java create mode 100644 mart/ev3/ex1/nl/ru/des/LCDPrinter.java create mode 100644 mart/ev3/ex1/nl/ru/des/WavPlayer.java create mode 100644 mart/ev3/ex1/nl/ru/des/sounds/makeall.sh diff --git a/mart/ev3/.gitignore b/mart/ev3/.gitignore new file mode 100644 index 0000000..d8dd753 --- /dev/null +++ b/mart/ev3/.gitignore @@ -0,0 +1 @@ +*.wav diff --git a/mart/ev3/ex1/nl/ru/des/AvoidBehaviour.java b/mart/ev3/ex1/nl/ru/des/AvoidBehaviour.java new file mode 100644 index 0000000..be21145 --- /dev/null +++ b/mart/ev3/ex1/nl/ru/des/AvoidBehaviour.java @@ -0,0 +1,37 @@ +package nl.ru.des; + +import java.io.File; + +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.robotics.subsumption.Behavior; + +public abstract class AvoidBehaviour implements Behavior { + protected EV3LargeRegulatedMotor rightMotor, leftMotor, avoidMotor; + private File audioFile; + + public AvoidBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor, File audioFile){ + this.rightMotor = rightMotor; + this.leftMotor = leftMotor; + this.avoidMotor = leftMotor; + this.audioFile = audioFile; + } + + public void setAvoidDirection(boolean avoidLeft){ + avoidMotor = avoidLeft ? leftMotor : rightMotor; + } + + @Override + public void action() { + WavPlayer.playWav(audioFile); + avoidMotor.setAcceleration(6000); + avoidMotor.backward(); + while(takeControl()); + avoidMotor.forward(); + avoidMotor.setAcceleration(1000); + } + + @Override + public void suppress() { + avoidMotor.forward(); + } +} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/AvoidHighObjectBehaviour.java b/mart/ev3/ex1/nl/ru/des/AvoidHighObjectBehaviour.java new file mode 100644 index 0000000..7a30ba1 --- /dev/null +++ b/mart/ev3/ex1/nl/ru/des/AvoidHighObjectBehaviour.java @@ -0,0 +1,38 @@ +package nl.ru.des; + +import java.io.File; + +import lejos.hardware.Button; +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.hardware.sensor.EV3UltrasonicSensor; +import lejos.robotics.SampleProvider; + +public class AvoidHighObjectBehaviour extends AvoidBehaviour { + private SampleProvider ultrasoneSample; + private float[] samples; + private float limit; + + public AvoidHighObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor, + EV3UltrasonicSensor ultraSone) { + super(leftMotor, rightMotor, new File("detect.wav")); + ultrasoneSample = ultraSone.getDistanceMode(); + samples = new float[ultrasoneSample.sampleSize()]; + calibrate(); + } + + private void calibrate(){ + LCDPrinter.print("Calibrate ultrasone..."); + LCDPrinter.print("Place object in"); + LCDPrinter.print(" turn radius"); + Button.waitForAnyPress(); + ultrasoneSample.fetchSample(samples, 0); + limit = samples[0]; + LCDPrinter.print("Limit: " + limit); + } + + @Override + public boolean takeControl() { + ultrasoneSample.fetchSample(samples, 0); + return samples[0] < limit; + } +} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/AvoidLowObjectBehaviour.java b/mart/ev3/ex1/nl/ru/des/AvoidLowObjectBehaviour.java new file mode 100644 index 0000000..82309dc --- /dev/null +++ b/mart/ev3/ex1/nl/ru/des/AvoidLowObjectBehaviour.java @@ -0,0 +1,53 @@ +package nl.ru.des; + +import java.io.File; + +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.hardware.sensor.EV3TouchSensor; +import lejos.robotics.SampleProvider; +import lejos.utility.Delay; + +public class AvoidLowObjectBehaviour extends AvoidBehaviour { + private SampleProvider rightSample, leftSample; + private float[] samples; + private long lastPush = 0; + + public AvoidLowObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor, + EV3TouchSensor leftTouch, EV3TouchSensor rightTouch) { + super(leftMotor, rightMotor, new File("bump.wav")); + rightSample = rightTouch.getTouchMode(); + leftSample = leftTouch.getTouchMode(); + samples = new float[rightTouch.sampleSize()]; + } + + @Override + public boolean takeControl() { + //Check if left sensor is pressed + leftSample.fetchSample(samples, 0); + boolean takeControl = false; + if(samples[0] == 1){ + super.setAvoidDirection(false); + takeControl = true; + lastPush = System.currentTimeMillis(); + } + + //Check if right sensor is pressed + rightSample.fetchSample(samples, 0); + if(samples[0] == 1){ + super.setAvoidDirection(true); + takeControl = true; + lastPush = System.currentTimeMillis(); + } + return takeControl || System.currentTimeMillis()-lastPush < 1000; + } + + @Override + public void action() { + rightMotor.backward(); + leftMotor.backward(); + Delay.msDelay(200); + leftMotor.forward(); + rightMotor.forward(); + super.action(); + } +} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/ButtonListener.java b/mart/ev3/ex1/nl/ru/des/ButtonListener.java index 0f7ef44..d4d4f8a 100644 --- a/mart/ev3/ex1/nl/ru/des/ButtonListener.java +++ b/mart/ev3/ex1/nl/ru/des/ButtonListener.java @@ -2,15 +2,11 @@ package nl.ru.des; import lejos.hardware.Key; import lejos.hardware.KeyListener; -import lejos.hardware.lcd.LCD; -import lejos.utility.Delay; class ButtonListener implements KeyListener { @Override public void keyPressed(Key k) { - LCD.clear(); - LCD.drawString("Bye...", 0, 0); - Delay.msDelay(500); + LCDPrinter.shutdown("Bye..."); System.exit(0); } diff --git a/mart/ev3/ex1/nl/ru/des/LCDPrintBehaviour.java b/mart/ev3/ex1/nl/ru/des/LCDPrintBehaviour.java deleted file mode 100644 index cd5b58b..0000000 --- a/mart/ev3/ex1/nl/ru/des/LCDPrintBehaviour.java +++ /dev/null @@ -1,24 +0,0 @@ -package nl.ru.des; - -import lejos.robotics.subsumption.Behavior; - -class LCDPrintr implements Behavior{ - - @Override - public boolean takeControl() { - return false; - } - - @Override - public void action() { - // TODO Auto-generated method stub - - } - - @Override - public void suppress() { - // TODO Auto-generated method stub - - } - -} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/LCDPrinter.java b/mart/ev3/ex1/nl/ru/des/LCDPrinter.java new file mode 100644 index 0000000..d73e12b --- /dev/null +++ b/mart/ev3/ex1/nl/ru/des/LCDPrinter.java @@ -0,0 +1,39 @@ +package nl.ru.des; + +import java.util.LinkedList; +import java.util.Queue; + +import lejos.hardware.lcd.LCD; +import lejos.utility.Delay; + +public class LCDPrinter extends Thread{ + public static final int PRINTDELAY = 250; + + private static Queue buffer = new LinkedList(); + private static boolean shutdown = false; + + public static void print(String s){ + buffer.add(s); + } + + public void run(){ + int y = 0; + while(!shutdown || (shutdown && !buffer.isEmpty())){ + if(!buffer.isEmpty()){ + LCD.clear(y); + LCD.clear(Math.max(0, y+1)); + LCD.drawString(buffer.remove(), 0, y); + y = y < LCD.DISPLAY_CHAR_DEPTH-1 ? y + 1 : 0; + } + Delay.msDelay(PRINTDELAY); + } + } + + public static void shutdown(String s) { + buffer.add(s); + shutdown = true; + while(!buffer.isEmpty()){ + Delay.msDelay(200); + } + } +} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/Main.java b/mart/ev3/ex1/nl/ru/des/Main.java index c8a1b5b..00593bc 100644 --- a/mart/ev3/ex1/nl/ru/des/Main.java +++ b/mart/ev3/ex1/nl/ru/des/Main.java @@ -1,18 +1,76 @@ package nl.ru.des; +import java.io.File; + +import lejos.hardware.Audio; +import lejos.hardware.ev3.EV3; import lejos.hardware.ev3.LocalEV3; +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.hardware.port.MotorPort; +import lejos.hardware.sensor.EV3TouchSensor; +import lejos.hardware.sensor.EV3UltrasonicSensor; +import lejos.hardware.sensor.NXTLightSensor; import lejos.robotics.subsumption.Arbitrator; import lejos.robotics.subsumption.Behavior; +import lejos.utility.Delay; public class Main { public static void main(String[] args) { - LocalEV3.get().getKey("Escape").addKeyListener(new ButtonListener()); - Behavior[] behaviorList = new Behavior[]{ - new WandererBehaviour(), - //new StayInFieldBehaviour(), + LCDPrinter lcdprinter = new LCDPrinter(); + LCDPrinter.print("Starting up"); + lcdprinter.start(); + LCDPrinter.print("Brick..."); + EV3 brick = LocalEV3.get(); + + LCDPrinter.print("Audio..."); + Audio audio = brick.getAudio(); + WavPlayer wavplayer = new WavPlayer(audio); + wavplayer.start(); + WavPlayer.playWav(new File("bootaudio.wav")); + + LCDPrinter.print("Keylistener..."); + brick.getKey("Escape").addKeyListener(new ButtonListener()); + + //Get motors + LCDPrinter.print("Motors..."); + EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.D); + EV3LargeRegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.A); + rightMotor.setSpeed(300); + leftMotor.setSpeed(300); + rightMotor.setAcceleration(1000); + leftMotor.setAcceleration(1000); + + //Get sensors + LCDPrinter.print("Touch..."); + EV3TouchSensor leftTouch = new EV3TouchSensor(brick.getPort("S1")); + EV3TouchSensor rightTouch = new EV3TouchSensor(brick.getPort("S4")); + LCDPrinter.print("Light..."); + NXTLightSensor lightSensor = new NXTLightSensor(brick.getPort("S2")); + LCDPrinter.print("Ultrasone..."); + EV3UltrasonicSensor ultraSensor = new EV3UltrasonicSensor(brick.getPort("S3")); + + WavPlayer.playWav(new File("bootsystem.wav")); + + //Initialize behaviours + LCDPrinter.print("Behaviours..."); + Behavior[] behaviorList = new Behavior[] { + new WandererBehaviour(leftMotor, rightMotor), + new AvoidHighObjectBehaviour(leftMotor, rightMotor, ultraSensor), + new AvoidLowObjectBehaviour(leftMotor, rightMotor, leftTouch, rightTouch), + new StayInFieldBehaviour(lightSensor, leftMotor, rightMotor), }; + + WavPlayer.playWav(new File("calibrate.wav")); + + LCDPrinter.print("Arbitrator..."); Arbitrator arb = new Arbitrator(behaviorList); + + WavPlayer.playWav(new File("takeoff.wav")); + Delay.msDelay(1850); + LCDPrinter.print("Takeoff!"); arb.start(); + lightSensor.close(); + ultraSensor.close(); } -} +} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/StayInFieldBehaviour.java b/mart/ev3/ex1/nl/ru/des/StayInFieldBehaviour.java index e69de29..8f0823a 100644 --- a/mart/ev3/ex1/nl/ru/des/StayInFieldBehaviour.java +++ b/mart/ev3/ex1/nl/ru/des/StayInFieldBehaviour.java @@ -0,0 +1,48 @@ +package nl.ru.des; + +import java.io.File; + +import lejos.hardware.Button; +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.hardware.sensor.NXTLightSensor; +import lejos.robotics.SampleProvider; + +public class StayInFieldBehaviour extends AvoidBehaviour{ + private SampleProvider light; + private float[] samples; + private float black, white, dist; + + public StayInFieldBehaviour(NXTLightSensor lightSensor, EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor) { + super(rightMotor, leftMotor, new File("bound.wav")); + light = lightSensor.getRedMode(); + samples = new float[light.sampleSize()]; + calibrate(); + dist = Math.abs(black-white); + } + + private void calibrate(){ + LCDPrinter.print("Calibrate Light..."); + //Get the black value + LCDPrinter.print("Place on black"); + Button.waitForAnyPress(); + light.fetchSample(samples, 0); + black = samples[0]; + LCDPrinter.print("Black: " + black); + + //Get the white value + LCDPrinter.print("Place on white"); + Button.waitForAnyPress(); + light.fetchSample(samples, 0); + white = samples[0]; + LCDPrinter.print("White: " + white); + + //Calculate the delta + dist = Math.abs(black-white); + } + + @Override + public boolean takeControl() { + this.light.fetchSample(samples, 0); + return samples[0] < Math.abs(white - dist/2); + } +} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/WandererBehaviour.java b/mart/ev3/ex1/nl/ru/des/WandererBehaviour.java index 66e85f6..a915f4d 100644 --- a/mart/ev3/ex1/nl/ru/des/WandererBehaviour.java +++ b/mart/ev3/ex1/nl/ru/des/WandererBehaviour.java @@ -1,17 +1,14 @@ package nl.ru.des; import lejos.hardware.motor.EV3LargeRegulatedMotor; -import lejos.hardware.port.MotorPort; import lejos.robotics.subsumption.Behavior; public class WandererBehaviour implements Behavior { private EV3LargeRegulatedMotor leftMotor, rightMotor; - public WandererBehaviour(){ - leftMotor = new EV3LargeRegulatedMotor(MotorPort.A); - leftMotor.setSpeed(10000); - rightMotor = new EV3LargeRegulatedMotor(MotorPort.D); - rightMotor.setSpeed(10000); + public WandererBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor){ + this.leftMotor = leftMotor; + this.rightMotor = rightMotor; } @Override @@ -27,7 +24,5 @@ public class WandererBehaviour implements Behavior { @Override public void suppress() { - leftMotor.stop(true); - rightMotor.stop(); } } \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/WavPlayer.java b/mart/ev3/ex1/nl/ru/des/WavPlayer.java new file mode 100644 index 0000000..87a490e --- /dev/null +++ b/mart/ev3/ex1/nl/ru/des/WavPlayer.java @@ -0,0 +1,31 @@ +package nl.ru.des; + +import java.io.File; + +import lejos.hardware.Audio; +import lejos.utility.Delay; + +public class WavPlayer extends Thread{ + private static File current; + private Audio audio = null; + + public static void playWav(File c){ + current = c; + } + + public WavPlayer(Audio audio){ + this.audio = audio; + audio.setVolume(100); + } + + @Override + public void run(){ + while(true){ + if(current != null){ + audio.playSample(current); + current = null; + } + Delay.msDelay(200); + } + } +} \ No newline at end of file diff --git a/mart/ev3/ex1/nl/ru/des/sounds/makeall.sh b/mart/ev3/ex1/nl/ru/des/sounds/makeall.sh new file mode 100644 index 0000000..58db755 --- /dev/null +++ b/mart/ev3/ex1/nl/ru/des/sounds/makeall.sh @@ -0,0 +1,7 @@ +echo "Audio system loaded" | espeak --stdout | sox - -r 8000 -b 8 bootaudio.wav +echo "Ready for calibration" | espeak --stdout | sox - -r 8000 -b 8 bootsystem.wav +echo "Succesfull calibration" | espeak --stdout | sox - -r 8000 -b 8 calibrate.wav +echo "Takeoff in 3 2 1 go" | espeak --stdout | sox - -r 8000 -b 8 takeoff.wav +echo "Almost out of bounds" | espeak --stdout | sox - -r 8000 -b 8 bounds.wav +echo "Bumped against low object" | espeak --stdout | sox - -r 8000 -b 8 bump.wav +echo "Detected big object" | espeak --stdout | sox - -r 8000 -b 8 detect.wav -- 2.20.1