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