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