mart/ev3/ex2/nl/ru/des/behaviours/AvoidLowObjectBehaviour.java

   1 package nl.ru.des.behaviours;
   2
   3 import lejos.hardware.motor.EV3LargeRegulatedMotor;
   4 import lejos.robotics.SampleProvider;
   5 import lejos.robotics.subsumption.Behavior;
   6
   7 public class AvoidLowObjectBehaviour implements Behavior {
   8         private static final long BACKWARDSTIME = 250;
   9         private static final long TURNTIME = 250;
  10         private SampleProvider touch;
  11         private float[] samples;
  12         private boolean avoidDirection;
  13         private boolean suppressed;
  14         private EV3LargeRegulatedMotor leftMotor, rightMotor;
  15
  16         public AvoidLowObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor,
  17                         SampleProvider touch) {
  18                 this.touch = touch;
  19                 this.samples = new float[touch.sampleSize()];
  20                 this.leftMotor = leftMotor;
  21                 this.rightMotor = rightMotor;
  22         }
  23
  24         @Override
  25         public boolean takeControl() {
  26                 touch.fetchSample(samples, 0);
  27                 if(samples[0]>0){
  28                         avoidDirection = false;
  29                         return true;
  30                 }
  31                 if(samples[1]>0){
  32                         avoidDirection = true;
  33                         return true;
  34                 }
  35                 return false;
  36         }
  37
  38         @Override
  39         public void action() {
  40                 suppressed = false;
  41                 int leftacc = leftMotor.getAcceleration();
  42                 int rightacc = rightMotor.getAcceleration();
  43                 leftMotor.setAcceleration(10000);
  44                 rightMotor.setAcceleration(10000);
  45                 leftMotor.backward();
  46                 rightMotor.backward();
  47                 long time = System.currentTimeMillis();
  48                 while (!suppressed && System.currentTimeMillis() - time > BACKWARDSTIME) {
  49                         Thread.yield();
  50                 }
  51
  52                 if(avoidDirection){
  53                         leftMotor.backward();
  54                         rightMotor.forward();
  55                 } else {
  56                         leftMotor.forward();
  57                         rightMotor.backward();
  58                 }
  59                 time = System.currentTimeMillis();
  60                 while (!suppressed && System.currentTimeMillis() - time > TURNTIME) {
  61                         Thread.yield();
  62                 }
  63                 leftMotor.stop(true);
  64                 leftMotor.stop(true);
  65                 leftMotor.setAcceleration(leftacc);
  66                 leftMotor.setAcceleration(rightacc);
  67         }
  68
  69         @Override
  70         public void suppress() {
  71                 suppressed = true;
  72         }
  73 }