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 extends ReactiveBehaviour implements Behavior {
   8         private static final long BACKWARDSTIME = 250;
   9         private static final long TURNTIME = 500;
  10         private boolean avoidDirection;
  11         private EV3LargeRegulatedMotor leftMotor, rightMotor;
  12
  13         public AvoidLowObjectBehaviour(EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor,
  14                         SampleProvider touch) {
  15                 super(touch);
  16                 this.samples = new float[touch.sampleSize()];
  17                 this.leftMotor = leftMotor;
  18                 this.rightMotor = rightMotor;
  19         }
  20
  21         @Override
  22         public boolean takeControl() {
  23                 fetchSample();
  24                 if(samples[0]>0){
  25                         avoidDirection = false;
  26                         return true;
  27                 }
  28                 if(samples[1]>0){
  29                         avoidDirection = true;
  30                         return true;
  31                 }
  32                 return false;
  33         }
  34
  35         @Override
  36         public void action() {
  37                 int leftacc = leftMotor.getAcceleration();
  38                 int rightacc = rightMotor.getAcceleration();
  39                 leftMotor.setAcceleration(10000);
  40                 rightMotor.setAcceleration(10000);
  41                 leftMotor.backward();
  42                 rightMotor.backward();
  43                 long time = System.currentTimeMillis();
  44                 while (!suppressed && System.currentTimeMillis() - time < BACKWARDSTIME) {
  45                         Thread.yield();
  46                 }
  47
  48                 if(avoidDirection){
  49                         leftMotor.backward();
  50                         rightMotor.forward();
  51                 } else {
  52                         leftMotor.forward();
  53                         rightMotor.backward();
  54                 }
  55                 time = System.currentTimeMillis();
  56                 while (!suppressed && System.currentTimeMillis() - time < TURNTIME) {
  57                         Thread.yield();
  58                 }
  59                 leftMotor.stop(true);
  60                 leftMotor.stop(true);
  61                 leftMotor.setAcceleration(leftacc);
  62                 leftMotor.setAcceleration(rightacc);
  63         }
  64 }