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
  65                 turnRandom(30, 45);
  66         }
  67
  68         protected void measureRock(){
  69                 long time = System.currentTimeMillis();
  70                 rightMotor.forward();
  71                 leftMotor.forward();
  72                 while(System.currentTimeMillis()-time<1500){
  73                         Thread.yield();
  74                 }
  75
  76                 rightMotor.stop(true);
  77                 leftMotor.stop(true);
  78
  79                 measMotor.backward();
  80                 while(!suppressed && !measMotor.isStalled()){
  81                         Thread.yield();
  82                 }
  83                 measMotor.forward();
  84                 while(!suppressed && !measMotor.isStalled()){
  85                         Thread.yield();
  86                 }
  87                 measMotor.stop(true);
  88
  89                 turnRandom(30, 45);
  90         }
  91
  92         protected void turnRandom(int from, int to){
  93                 int degrees = Marster.random.nextInt(to-from)+from;
  94                 if(Marster.random.nextBoolean()){
  95                         rightTurn(degrees);
  96                 } else {
  97                         leftTurn(degrees);
  98                 }
  99         }
 100
 101         protected void rightTurn(int angle){
 102                 rightMotor.stop(true);
 103                 leftMotor.stop();
 104                 sensors.resetGyro();
 105                 rightMotor.backward();
 106                 leftMotor.forward();
 107                 while(!suppressed && Math.abs(sensors.gyro()) < angle){
 108                         Thread.yield();
 109                 }
 110                 rightMotor.stop(true);
 111                 leftMotor.stop(true);
 112                 LCDPrinter.print(Float.toString(sensors.gyro()));
 113         }
 114
 115         protected void leftTurn(int angle){
 116                 rightMotor.stop(true);
 117                 leftMotor.stop();
 118                 sensors.resetGyro();
 119                 leftMotor.backward();
 120                 rightMotor.forward();
 121                 while(!suppressed && Math.abs(sensors.gyro()) < angle){
 122                         Thread.yield();
 123                 }
 124                 rightMotor.stop(true);
 125                 leftMotor.stop(true);
 126                 LCDPrinter.print(Float.toString(sensors.gyro()));
 127         }
 128
 129         @Override
 130         public void action() {
 131                 suppressed = false;
 132                 finished = false;
 133                 rightMotor.setSpeed(Constants.speed);
 134                 rightMotor.setAcceleration(Constants.acceleration);
 135                 leftMotor.setSpeed(Constants.speed);
 136                 leftMotor.setAcceleration(Constants.acceleration);
 137                 rightMotor.stop(true);
 138                 leftMotor.stop(true);
 139         }
 140
 141         @Override
 142         public void suppress() {
 143                 suppressed = true;
 144                 finished = true;
 145                 rightMotor.setSpeed(Constants.speed);
 146                 rightMotor.setAcceleration(Constants.acceleration);
 147                 leftMotor.setSpeed(Constants.speed);
 148                 leftMotor.setAcceleration(Constants.acceleration);
 149                 rightMotor.stop(true);
 150                 leftMotor.stop(true);
 151         }
 152
 153         @Override public boolean takeControl(){
 154                 return true;
 155         }
 156 }