Implemented AvoidObject behaviour and TextLCD

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

diff --git a/mart/ev3/ex2/nl/ru/des/Main.java b/mart/ev3/ex2/nl/ru/des/Main.java
index 3a7e256..1be815b 100644 (file)
--- 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