working robot

[des2015.git] / mart / ev3 / ex1 / nl / ru / des / Main.java

diff --git a/mart/ev3/ex1/nl/ru/des/Main.java b/mart/ev3/ex1/nl/ru/des/Main.java
index c8a1b5b..00593bc 100644 (file)
--- 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