import lejos.hardware.Button;
import lejos.hardware.motor.Motor;
import lejos.hardware.port.SensorPort;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.utility.Delay;


public class BotBattle1 {
        static USDeclaration usSensor;
        static CDeclaration cSensor;
        public static void main(String args[]){


                cSensor = new CDeclaration(SensorPort.S1);
                cSensor.setDaemon(true);
                cSensor.start();

                usSensor = new USDeclaration(SensorPort.S3);
                usSensor.setDaemon(true);
                usSensor.start();

                Behavior b1 = new ColorCheck();
                Behavior b2 = new DistanceCheck();
                Behavior b3 = new Search();
                Behavior[] behaviorArray = {b2, b3};

                Arbitrator arbitrator = new Arbitrator(behaviorArray);

                Button.waitForAnyPress();
                Delay.msDelay(5000);
                Motor.A.setSpeed(12000);
                Motor.B.setSpeed(12000);
                Motor.A.setAcceleration(12000);
                Motor.B.setAcceleration(12000);
                arbitrator.start();


        }
}


import lejos.hardware.motor.Motor;
import lejos.robotics.subsumption.Behavior;

public class DistanceCheck implements Behavior{
        private boolean suppressed = false;

        float myDistance;

        public boolean takeControl() {
                //Check distance
                myDistance = BotBattle1.usSensor.distance;
                //System.out.println(myDistance);
                if(myDistance < .6){
                        return true;
                }else{
                        return false;
                }

        }

        public void action() {
                //Move toward target
                suppressed = false;
                Motor.A.forward();
                Motor.B.forward();
                while(!suppressed)
                        Thread.yield();
                Motor.A.stop();
                Motor.B.stop();

        }


        public void suppress() {
                Motor.A.stop();
                Motor.B.stop();
                suppressed = true;
        }
}

import lejos.hardware.motor.Motor;
import lejos.robotics.Color;
import lejos.robotics.subsumption.Behavior;
import lejos.utility.Delay;


class ColorCheck implements Behavior{
        private boolean suppressed = false;

        float myColor;

        public boolean takeControl()
        {
                //Check the color
                myColor = BotBattle1.cSensor.color;
                if (myColor ==Color.BLACK)
                {
                        return true;
                }
                else
                {
                        return false;
                }
        }
        public void action() {
                //Move backward and pivot 180°
                Motor.A.backward();
                Motor.B.backward();
                Delay.msDelay(3000);
                while( !suppressed )
                        Thread.yield();
                Motor.A.stop(); // clean up
                Motor.B.stop();
                //Motor.A.resetTachoCount();
                //Motor.B.resetTachoCount();
                //Motor.A.rotate(180);
                //Motor.B.rotate(-180);

        }
        public void suppress() {
                Motor.A.stop();
                Motor.B.stop();
                suppressed = true;

        }
}import lejos.hardware.motor.Motor;
import lejos.robotics.subsumption.Behavior;


class Search implements Behavior{
        private boolean suppressed = false;

        public boolean takeControl() {
                return true;
        }

        public void action() {
                suppressed = false;
                Motor.A.forward();
                Motor.B.backward();
                while( !suppressed )
                        Thread.yield();
                Motor.A.stop(); // clean up
                Motor.B.stop();

        }

        public void suppress() {
                Motor.A.stop();
                Motor.B.stop();
                suppressed = true;
        }


}
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.robotics.SampleProvider;


public class USDeclaration extends Thread{
        EV3UltrasonicSensor us;
        SampleProvider sp;
        public float distance = 0;
    float [] sample = new float[1];

    public USDeclaration(Port usPort){
        us = new EV3UltrasonicSensor(usPort);
        sp = us.getDistanceMode();
    }

    public void run()
    {
        while (true)
        {
            us.fetchSample(sample, 0);
            distance = sample[0];
        }

    }

}
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.robotics.SampleProvider;


public class CDeclaration extends Thread{
        EV3ColorSensor c;
        SampleProvider sp;
        public float color = 0;
    float [] sample = new float[1];

    public CDeclaration(Port usPort){
        c = new EV3ColorSensor(usPort);
        sp = c.getColorIDMode();
    }

    public void run()
    {
        while (true)
        {
            c.fetchSample(sample, 0);
            color = sample[0];
        }

    }
}