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         protected 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                 suppressed = SuppressedState.IDLE;
  32         }
  33
  34         protected void measure(){
  35                 measMotor.backward();
  36                 while(!measMotor.isStalled()){
  37                         Thread.yield();
  38                 }
  39                 measMotor.forward();
  40                 while(!measMotor.isStalled()){
  41                         Thread.yield();
  42                 }
  43                 measMotor.stop(true);
  44                 rightTurn(90);
  45         }
  46
  47         protected void rightTurn(int angle){
  48                 rightMotor.stop();
  49                 leftMotor.stop();
  50                 sensors.resetGyro();
  51                 rightMotor.backward();
  52                 leftMotor.forward();
  53                 while(suppressed != SuppressedState.SUPPRESSED && Math.abs(sensors.gyro()) < angle){
  54                         Thread.yield();
  55                 }
  56                 LCDPrinter.print(Float.toString(sensors.gyro()));
  57         }
  58
  59         protected void leftTurn(int angle){
  60                 rightMotor.stop();
  61                 leftMotor.stop();
  62                 sensors.resetGyro();
  63                 leftMotor.backward();
  64                 rightMotor.forward();
  65                 while(suppressed != SuppressedState.SUPPRESSED && Math.abs(sensors.gyro()) < angle){
  66                         Thread.yield();
  67                 }
  68                 LCDPrinter.print(Float.toString(sensors.gyro()));
  69         }
  70
  71         @Override
  72         public void action() {
  73                 suppressed = SuppressedState.IN_ACTION;
  74         }
  75
  76         @Override
  77         public void suppress() {
  78                 suppressed = SuppressedState.SUPPRESSED;
  79         }
  80
  81         @Override public boolean takeControl(){
  82                 return true;
  83         }
  84 }