package nl.ru.des;
-import lejos.hardware.motor.EV3LargeRegulatedMotor;
+import lejos.robotics.RegulatedMotor;
import lejos.robotics.subsumption.Behavior;
+import nl.ru.des.sensors.SensorCollector;
public abstract class BasicBehaviour implements Behavior{
- protected EV3LargeRegulatedMotor leftMotor, rightMotor;
+ protected enum SuppressedState {
+ IDLE, IN_ACTION, SUPPRESSED;
+ }
+ private SuppressedState suppressed;
+ protected RegulatedMotor leftMotor, rightMotor, measMotor;
+ protected SensorCollector sensors;
+ protected long time;
- public BasicBehaviour(nl.ru.des.Sensors sensors, EV3LargeRegulatedMotor leftMotor, EV3LargeRegulatedMotor rightMotor){
+ public BasicBehaviour(SensorCollector sensors, RegulatedMotor leftMotor,
+ RegulatedMotor rightMotor, RegulatedMotor measMotor){
this.leftMotor = leftMotor;
this.rightMotor = rightMotor;
+ this.measMotor = measMotor;
+ this.sensors = sensors;
+ }
+
+ protected void reset(){
+ rightMotor.setSpeed(Constants.speed);
+ rightMotor.setAcceleration(Constants.acceleration);
+ leftMotor.setSpeed(Constants.speed);
+ leftMotor.setAcceleration(Constants.acceleration);
+ rightMotor.stop(true);
+ leftMotor.stop(true);
+ setSuppressed(SuppressedState.IDLE);
+ long time = System.currentTimeMillis();
+ while(System.currentTimeMillis()-time > 250){
+ Thread.yield();
+ }
+ }
+
+ protected void measure(){
+ measMotor.backward();
+ while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+ Thread.yield();
+ }
+ measMotor.forward();
+ while(getSuppressed() == SuppressedState.IN_ACTION && !measMotor.isStalled()){
+ Thread.yield();
+ }
+ measMotor.stop(true);
+ rightTurn(90);
+ }
+
+ protected void rightTurn(int angle){
+ rightMotor.stop();
+ leftMotor.stop();
+ sensors.resetGyro();
+ rightMotor.backward();
+ leftMotor.forward();
+ while(getSuppressed() == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
+ Thread.yield();
+ }
+ LCDPrinter.print(Float.toString(sensors.gyro()));
+ }
+
+ protected void leftTurn(int angle){
+ rightMotor.stop();
+ leftMotor.stop();
+ sensors.resetGyro();
+ leftMotor.backward();
+ rightMotor.forward();
+ while(getSuppressed() == SuppressedState.IN_ACTION && Math.abs(sensors.gyro()) < angle){
+ Thread.yield();
+ }
+ LCDPrinter.print(Float.toString(sensors.gyro()));
+ }
+
+ protected synchronized void setSuppressed(SuppressedState sup){
+ suppressed = sup;
+ }
+
+ protected SuppressedState getSuppressed(){
+ return suppressed;
}
@Override
- public boolean takeControl() {
- return true;
+ public void action() {
+ setSuppressed(SuppressedState.IN_ACTION);
}
@Override
- public void suppress() {}
+ public void suppress() {
+ setSuppressed(SuppressedState.SUPPRESSED);
+ }
+
+ @Override public boolean takeControl(){
+ return true;
+ }
}