code 17.10.19

import ev3dev.sensors.ev3.EV3ColorSensor;
import ev3dev.sensors.ev3.EV3TouchSensor;
import ev3dev.actuators.lego.motors.EV3LargeRegulatedMotor;
import lejos.hardware.port.SensorPort;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;
import lejos.hardware.port.MotorPort;

public class Lynet {

    private static final EV3TouchSensor trykkSensor = new EV3TouchSensor(SensorPort.S2); // Fikse hvilken port den er i????

    private static final EV3LargeRegulatedMotor motorC = new EV3LargeRegulatedMotor(MotorPort.C);
    private static final EV3LargeRegulatedMotor motorB = new EV3LargeRegulatedMotor(MotorPort.B);
    private static final EV3ColorSensor fargeSensorV = new EV3ColorSensor(SensorPort.S3);
    private static final EV3ColorSensor fargeSensorH = new EV3ColorSensor(SensorPort.S4);

    public static void main(String[] args) {
        SampleProvider sp1 = fargeSensorV.getRedMode();
        SampleProvider sp2 = fargeSensorH.getRedMode();
        float sampleV[] = new float[sp1.sampleSize()];
        float sampleH[] = new float[sp2.sampleSize()];

        //motorB.setSpeed(0);
        //motorC.setSpeed(0);
        int sort = 40;

        while(true){

            //sensor 1
            sp1.fetchSample(sampleV, 0);

            //Sensor 2
            sp2.fetchSample(sampleH, 0);
            System.out.println("SensorV: " + sampleV[0] + " og " + "SensorH: " + sampleH[0]);

            if (trykkSensor.isPressed()) {
                motorB.stop();
                motorC.stop();
            }
            else if(sampleV[0]<sort)
            {
                kjorV();
            }
            else if(sampleH[0]<sort)
            {
                kjorH();
            }

            else {kjorRett();}
        }
    }
    public static void kjorRett()
    {
        motorB.setSpeed(400);
        motorB.forward();
        motorC.setSpeed(400);
        motorC.forward();
    }

    public static void kjorV()
    {
    	motorB.setSpeed(0);
        motorB.forward();
        motorC.setSpeed(200);
        motorC.forward();
        Delay.msDelay(300);
    }

    public static void kjorH()
    {
    	motorB.setSpeed(200);
        motorB.forward();
        motorC.setSpeed(0);
        motorC.forward();
    }
}



/*else if(sampleV[0]<sort && sampleH[0]<sort)
            {
                kjorRett();
                Delay.msDelay(300);
            }*/


            /*else if(stop = true)
            {
                motorB.setSpeed(1000);
                motorB.forward();
                motorC.setSpeed(1000);
                motorC.forward();
                Delay.msDelay(1000);
                stop = false;
            }*/
            /*else if(sampleV[0]<sort && sampleH[0]<sort)
            {
                kjorRett();
                Delay.msDelay(400);
            }*/

             //stop = true;
            //boolean stop = false;