motorsetup

import lejos.hardware.Sound;
import lejos.hardware.ev3.LocalEV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.motor.EV3MediumRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.hardware.sensor.SensorMode;
import lejos.hardware.sensor.SensorModes;
import lejos.robotics.RegulatedMotor;


public class HelloWorld
{
    public static final RegulatedMotor MOTORB = new EV3LargeRegulatedMotor(MotorPort.B);
    public static final RegulatedMotor MOTORC = new EV3LargeRegulatedMotor(MotorPort.C);
    public static final RegulatedMotor SERVO = new EV3MediumRegulatedMotor(MotorPort.A);
    public static final EV3UltrasonicSensor SENSOR_SONIC = new EV3UltrasonicSensor(LocalEV3.get().getPort("S4"));
    public static final EV3GyroSensor SENSOR_GYRO = new EV3GyroSensor(LocalEV3.get().getPort("S2"));
    public static final EV3TouchSensor SENSOR_TOUCH = new EV3TouchSensor(LocalEV3.get().getPort("S1"));
    public static final EV3ColorSensor SENSOR_COLOR = new EV3ColorSensor(LocalEV3.get().getPort("S3"));

    public static void main(String[] args)
    {
        float[] sample = new float[SENSOR_TOUCH.sampleSize()];
        SENSOR_TOUCH.fetchSample(sample, 0);
        SensorMode rgbmode = SENSOR_COLOR.getRGBMode();

        LCD.drawString("Select function", 0, 2);
        LCD.drawString("YELLOW: Drive", 0, 3);
        LCD.drawString("BLUE: Dance", 0, 4);
        LCD.drawString("GREEN: ", 0, 5);
        LCD.drawString("RED: ", 0, 6);

        String mode = "";

        while(sample[0] == 0)
        {
            float[] rgb = new float[rgbmode.sampleSize()];

            rgbmode.fetchSample(rgb, 0);

            if (rgb[0] > 0.1 && rgb[1] > 0.1)
            {
                mode = "yellow";
                LCD.drawString("YELLOW selected", 0, 1);
                LCD.drawString("Spank to start", 0, 2);
            }
            else if (rgb[0] > 0.1)
            {
                mode = "red";
                LCD.drawString("RED selected", 0, 1);
                LCD.drawString("Spank to start", 0, 2);
            }
            else if (rgb[1] > 0.1)
            {
                mode = "green";
                LCD.drawString("GREEN selected", 0, 1);
                LCD.drawString("Spank to start", 0, 2);
            }
            else if (rgb[2] > 0.1)
            {
                mode = "blue";
                LCD.drawString("BLUE selected", 0, 1);
                LCD.drawString("Spank to start", 0, 2);
            }

            SENSOR_TOUCH.fetchSample(sample, 0);
        }

        cmd_sayhi();

    }

    public static void cmd_sayhi()
    {
        Sound.systemSound(true, 3);
        SERVO.setSpeed(750);
        SERVO.rotateTo(405);
        SERVO.rotateTo(0);
        SERVO.rotateTo(405);
        SERVO.rotateTo(0);
    }

    public static void cmd_drive()
    {
        LocalEV3.get().getKeys().getButtons();
    }
}