natanael/ev3/Main.java

   1 import lejos.hardware.lcd.LCD;
   2 import lejos.hardware.motor.EV3LargeRegulatedMotor;
   3 import lejos.hardware.port.MotorPort;
   4 import lejos.hardware.sensor.EV3TouchSensor;
   5 import lejos.hardware.sensor.EV3UltrasonicSensor;
   6 import lejos.hardware.sensor.NXTLightSensor;
   7 import lejos.robotics.SampleProvider;
   8 import lejos.utility.Delay;
   9
  10 import java.util.Random;
  11
  12 import lejos.hardware.*;
  13 import lejos.hardware.ev3.LocalEV3;
  14
  15 public class Main {
  16
  17         static float BLACK_LINE = (float) 0.5;
  18         static float SOMETHING_IN_FRONT = (float) 0.24;
  19         static float SOMETHING_IN_LEFT = 1;
  20         static float SOMETHING_IN_RIGHT = 1;
  21         static int MINIMUM_TURN = 120;
  22         static int MAXIMUM_TURN = 180;
  23
  24         static EV3LargeRegulatedMotor leftMotor, rightMotor;
  25         static EV3TouchSensor touchLeftSensor, touchRightSensor;
  26         static NXTLightSensor lightSensor;
  27         static EV3UltrasonicSensor distanceSensor;
  28
  29         private static Random rnd = new Random();
  30         private static long StartTime = 0;
  31
  32         public static void main(String[] args) {
  33                 leftMotor = new EV3LargeRegulatedMotor(MotorPort.A);
  34                 rightMotor = new EV3LargeRegulatedMotor(MotorPort.D);
  35
  36                 touchLeftSensor = new EV3TouchSensor(LocalEV3.get().getPort("S1"));
  37                 touchRightSensor = new EV3TouchSensor(LocalEV3.get().getPort("S4"));
  38                 lightSensor = new NXTLightSensor(LocalEV3.get().getPort("S2"));
  39                 distanceSensor = new EV3UltrasonicSensor(LocalEV3.get().getPort("S3"));
  40
  41                 SampleProvider touchLeft = touchLeftSensor.getTouchMode(), touchRight = touchRightSensor.getTouchMode(),
  42                                 light = lightSensor.getRedMode(), distance = distanceSensor.getDistanceMode();
  43
  44                 float[] touchLeftSamples = new float[touchLeft.sampleSize()],
  45                                 touchRightSamples = new float[touchRight.sampleSize()], lightSamples = new float[light.sampleSize()],
  46                                 distanceSamples = new float[distance.sampleSize()];
  47
  48                 // Main Looping
  49                 while (!Button.ESCAPE.isDown()) {
  50                         StartTime = System.currentTimeMillis();
  51
  52                         touchLeft.fetchSample(touchLeftSamples, 0);
  53                         touchRight.fetchSample(touchRightSamples, 0);
  54                         light.fetchSample(lightSamples, 0);
  55                         distance.fetchSample(distanceSamples, 0);
  56
  57                         LCD.drawString("TL: " + touchLeftSamples[0], 0, 0);
  58                         LCD.drawString("TR: " + touchRightSamples[0], 0, 1);
  59                         LCD.drawString("LI: " + lightSamples[0], 0, 2);
  60                         LCD.drawString("DI: " + distanceSamples[0], 0, 3); // distance in
  61                                                                                                                                 // meter
  62
  63                         if (lightSamples[0] < BLACK_LINE) {
  64                                 LCD.drawString("BLACK", 0, 4);
  65                                 Stop();
  66                                 if (rnd.nextBoolean()) {
  67                                         TurnLeft(GetRandomTurn());
  68                                 } else {
  69                                         TurnRight(GetRandomTurn());
  70                                 }
  71                         } else {
  72                                 LCD.drawString("WHITE", 0, 4);
  73                                 GoForward();
  74                         }
  75
  76                         if (distanceSamples[0] < SOMETHING_IN_FRONT) {
  77                                 LCD.drawString("STOP", 0, 5);
  78                         } else {
  79                                 LCD.drawString("GO  ", 0, 5);
  80                         }
  81
  82                         LCD.drawString("T: " + (System.currentTimeMillis() - StartTime), 0, 6);
  83
  84                         if (System.currentTimeMillis() - StartTime < 450) {
  85
  86                         }
  87                 }
  88         }
  89
  90         public static int GetRandomTurn() {
  91                 return rnd.nextInt((MAXIMUM_TURN - MINIMUM_TURN) + 1) + MINIMUM_TURN;
  92         }
  93
  94         public static void GoForward() {
  95                 leftMotor.setSpeed(300);
  96                 rightMotor.setSpeed(300);
  97
  98                 leftMotor.forward();
  99                 rightMotor.forward();
 100         }
 101
 102         public static void GoBack() {
 103                 leftMotor.setSpeed(300);
 104                 rightMotor.setSpeed(300);
 105
 106                 leftMotor.backward();
 107                 rightMotor.backward();
 108                 Delay.msDelay(100);
 109                 Stop();
 110
 111         }
 112
 113         public static void Stop() {
 114                 leftMotor.stop(true);
 115                 rightMotor.stop();
 116         }
 117
 118         public static void TurnRight(int degree) {
 119                 leftMotor.rotate(degree, true);
 120                 rightMotor.rotate(-degree);
 121         }
 122
 123         public static void TurnLeft(int degree) {
 124                 leftMotor.rotate(-degree, true);
 125                 rightMotor.rotate(degree);
 126
 127         }
 128 }