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 boolean suppressed;
   9         protected boolean finished;
  10         protected RegulatedMotor leftMotor, rightMotor, measMotor;
  11         protected SensorCollector sensors;
  12         protected long time;
  13
  14         public BasicBehaviour(SensorCollector sensors, RegulatedMotor leftMotor,
  15                         RegulatedMotor rightMotor, RegulatedMotor measMotor){
  16                 this.leftMotor = leftMotor;
  17                 this.rightMotor = rightMotor;
  18                 this.measMotor = measMotor;
  19                 this.sensors = sensors;
  20                 suppressed = false;
  21                 finished = true;
  22         }
  23
  24         protected void reset(){
  25                 finished = true;
  26                 rightMotor.setSpeed(Constants.speed);
  27                 rightMotor.setAcceleration(Constants.acceleration);
  28                 leftMotor.setSpeed(Constants.speed);
  29                 leftMotor.setAcceleration(Constants.acceleration);
  30                 rightMotor.stop(true);
  31                 leftMotor.stop(true);
  32         }
  33
  34         protected void measureLake(){
  35                 long time = System.currentTimeMillis();
  36                 rightMotor.backward();
  37                 leftMotor.backward();
  38                 while(System.currentTimeMillis()-time<500){
  39                         Thread.yield();
  40                 }
  41
  42                 rightMotor.stop(true);
  43                 leftMotor.stop(true);
  44                 rightMotor.setSpeed(25);
  45                 leftMotor.setSpeed(25);
  46                 if(sensors.leftLight() && !sensors.rightLight()){
  47                         leftTurn(15);
  48                 } else if (sensors.rightLight() && !sensors.leftLight()){
  49                         rightTurn(15);
  50                 }
  51
  52                 rightMotor.stop(true);
  53                 leftMotor.stop(true);
  54
  55                 measMotor.backward();
  56                 while(!suppressed && !measMotor.isStalled()){
  57                         Thread.yield();
  58                 }
  59                 measMotor.forward();
  60                 while(!suppressed && !measMotor.isStalled()){
  61                         Thread.yield();
  62                 }
  63                 measMotor.stop(true);
  64                 reset();
  65                 rightTurn(45);
  66         }
  67
  68         protected void turnRandom(int from, int to){
  69                 int degrees = Marster.random.nextInt(to-from)+from;
  70                 if(Marster.random.nextBoolean()){
  71                         rightTurn(degrees);
  72                 } else {
  73                         leftTurn(degrees);
  74                 }
  75         }
  76
  77         protected void rightTurn(int angle){
  78                 rightMotor.stop(true);
  79                 leftMotor.stop();
  80                 sensors.resetGyro();
  81                 rightMotor.backward();
  82                 leftMotor.forward();
  83                 while(!suppressed && Math.abs(sensors.gyro()) < angle){
  84                         Thread.yield();
  85                 }
  86                 rightMotor.stop(true);
  87                 leftMotor.stop(true);
  88                 LCDPrinter.print(Float.toString(sensors.gyro()));
  89         }
  90
  91         protected void leftTurn(int angle){
  92                 rightMotor.stop(true);
  93                 leftMotor.stop();
  94                 sensors.resetGyro();
  95                 leftMotor.backward();
  96                 rightMotor.forward();
  97                 while(!suppressed && Math.abs(sensors.gyro()) < angle){
  98                         Thread.yield();
  99                 }
 100                 rightMotor.stop(true);
 101                 leftMotor.stop(true);
 102                 LCDPrinter.print(Float.toString(sensors.gyro()));
 103         }
 104
 105         @Override
 106         public void action() {
 107                 suppressed = false;
 108                 finished = false;
 109                 rightMotor.setSpeed(Constants.speed);
 110                 rightMotor.setAcceleration(Constants.acceleration);
 111                 leftMotor.setSpeed(Constants.speed);
 112                 leftMotor.setAcceleration(Constants.acceleration);
 113                 rightMotor.stop(true);
 114                 leftMotor.stop(true);
 115         }
 116
 117         @Override
 118         public void suppress() {
 119                 suppressed = true;
 120                 finished = true;
 121                 rightMotor.setSpeed(Constants.speed);
 122                 rightMotor.setAcceleration(Constants.acceleration);
 123                 leftMotor.setSpeed(Constants.speed);
 124                 leftMotor.setAcceleration(Constants.acceleration);
 125                 rightMotor.stop(true);
 126                 leftMotor.stop(true);
 127         }
 128
 129         @Override public boolean takeControl(){
 130                 return true;
 131         }
 132 }