dsl/runtime/src/nl/ru/des/BasicBehaviour.java

   1 package nl.ru.des;
   2
   3 import lejos.robotics.RegulatedMotor;
   4 import lejos.robotics.subsumption.Behavior;
   5 import nl.ru.des.sensors.SensorCollector;
   6
   7 public abstract class BasicBehaviour implements Behavior{
   8         protected enum SuppressedState {
   9                 IDLE, IN_ACTION, SUPPRESSED;
  10         }
  11         private SuppressedState suppressed;
  12         protected RegulatedMotor leftMotor, rightMotor, measMotor;
  13         protected SensorCollector sensors;
  14         protected long time;
  15
  16         public BasicBehaviour(SensorCollector sensors, RegulatedMotor leftMotor,
  17                         RegulatedMotor rightMotor, RegulatedMotor measMotor){
  18                 this.leftMotor = leftMotor;
  19                 this.rightMotor = rightMotor;
  20                 this.measMotor = measMotor;
  21                 this.sensors = sensors;
  22         }
  23
  24         protected void reset(){
  25                 rightMotor.setSpeed(Constants.speed);
  26                 rightMotor.setAcceleration(Constants.acceleration);
  27                 leftMotor.setSpeed(Constants.speed);
  28                 leftMotor.setAcceleration(Constants.acceleration);
  29                 rightMotor.stop(true);
  30                 leftMotor.stop(true);
  31                 setSuppressed(SuppressedState.IDLE);
  32                 long time = System.currentTimeMillis();
  33                 while(System.currentTimeMillis()-time > 250){
  34                         Thread.yield();
  35                 }
  36         }
  37
  38         protected void measure(){
  39                 measMotor.backward();
  40                 while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
  41                         Thread.yield();
  42                 }
  43                 measMotor.forward();
  44                 while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
  45                         Thread.yield();
  46                 }
  47                 measMotor.stop(true);
  48                 rightTurn(90);
  49         }
  50
  51         protected void rightTurn(int angle){
  52                 rightMotor.stop();
  53                 leftMotor.stop();
  54                 sensors.resetGyro();
  55                 rightMotor.backward();
  56                 leftMotor.forward();
  57                 while(getSuppressed() == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
  58                         Thread.yield();
  59                 }
  60                 LCDPrinter.print(Float.toString(sensors.gyro()));
  61         }
  62
  63         protected void leftTurn(int angle){
  64                 rightMotor.stop();
  65                 leftMotor.stop();
  66                 sensors.resetGyro();
  67                 leftMotor.backward();
  68                 rightMotor.forward();
  69                 while(getSuppressed() == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
  70                         Thread.yield();
  71                 }
  72                 LCDPrinter.print(Float.toString(sensors.gyro()));
  73         }
  74
  75         protected synchronized void setSuppressed(SuppressedState sup){
  76                 suppressed = sup;
  77         }
  78
  79         protected SuppressedState getSuppressed(){
  80                 return suppressed;
  81         }
  82
  83         @Override
  84         public void action() {
  85                 setSuppressed(SuppressedState.IN_ACTION);
  86         }
  87
  88         @Override
  89         public void suppress() {
  90                 setSuppressed(SuppressedState.SUPPRESSED);
  91         }
  92
  93         @Override public boolean takeControl(){
  94                 return true;
  95         }
  96 }