public abstract class BasicBehaviour implements Behavior{
protected boolean suppressed;
+ protected boolean finished;
protected RegulatedMotor leftMotor, rightMotor, measMotor;
protected SensorCollector sensors;
protected long time;
this.rightMotor = rightMotor;
this.measMotor = measMotor;
this.sensors = sensors;
+ suppressed = false;
+ finished = true;
}
protected void reset(){
+ finished = true;
rightMotor.setSpeed(Constants.speed);
rightMotor.setAcceleration(Constants.acceleration);
leftMotor.setSpeed(Constants.speed);
leftMotor.setAcceleration(Constants.acceleration);
rightMotor.stop(true);
leftMotor.stop(true);
+ measMotor.stop(true);
}
- protected void rockMeasure(){
+ protected void measureLake(){
+ long time = System.currentTimeMillis();
+ rightMotor.backward();
+ leftMotor.backward();
+ while(System.currentTimeMillis()-time < 250){
+ Thread.yield();
+ }
+ rightMotor.stop(true);
+ leftMotor.stop(true);
+ rightMotor.setSpeed(25);
+ leftMotor.setSpeed(25);
+ if(sensors.leftLight() && !sensors.rightLight()){
+ leftTurn(10);
+ } else if (sensors.rightLight() && !sensors.leftLight()){
+ rightTurn(10);
+ }
+
+ rightMotor.stop(true);
+ leftMotor.stop(true);
+
+ measMotor.rotate(-100, true);
+ while(!suppressed && !measMotor.isStalled() && measMotor.isMoving()){
+ Thread.yield();
+ }
+ measMotor.rotate(100, true);
+ while(!suppressed && !measMotor.isStalled() && measMotor.isMoving()){
+ Thread.yield();
+ }
+ measMotor.stop(true);
+
+ reset();
+ rightMotor.backward();
+ leftMotor.backward();
+ while(System.currentTimeMillis()-time < 250){
+ Thread.yield();
+ }
+ turnRandom(45, 60);
}
- protected void lakeMeasure(){
+ protected void measureRock(){
+ long time = System.currentTimeMillis();
+ rightMotor.forward();
+ leftMotor.forward();
+ while(System.currentTimeMillis()-time<1000){
+ Thread.yield();
+ }
+
+ rightMotor.stop(true);
+ leftMotor.stop(true);
+
+ measMotor.backward();
+ while(!suppressed && !measMotor.isStalled()){
+ Thread.yield();
+ }
+ measMotor.forward();
+ while(!suppressed && !measMotor.isStalled()){
+ Thread.yield();
+ }
+ measMotor.stop(true);
+ rightMotor.backward();
+ leftMotor.backward();
+ while(System.currentTimeMillis()-time<1000){
+ Thread.yield();
+ }
+ turnRandom(30, 45);
+ }
+
+ protected void turnRandom(int from, int to){
+ int degrees = Marster.random.nextInt(to-from)+from;
+ if(Marster.random.nextBoolean()){
+ rightTurn(degrees);
+ } else {
+ leftTurn(degrees);
+ }
}
protected void rightTurn(int angle){
-
+ rightMotor.stop(true);
+ leftMotor.stop();
+ sensors.resetGyro();
+ rightMotor.backward();
+ leftMotor.forward();
+ while(!suppressed && Math.abs(sensors.gyro()) < angle){
+ Thread.yield();
+ }
+ rightMotor.stop(true);
+ leftMotor.stop(true);
+ System.out.println(Float.toString(sensors.gyro()));
}
protected void leftTurn(int angle){
-
+ rightMotor.stop(true);
+ leftMotor.stop();
+ sensors.resetGyro();
+ leftMotor.backward();
+ rightMotor.forward();
+ while(!suppressed && Math.abs(sensors.gyro()) < angle){
+ Thread.yield();
+ }
+ rightMotor.stop(true);
+ leftMotor.stop(true);
+ System.out.println(Float.toString(sensors.gyro()));
}
@Override
public void action() {
suppressed = false;
+ finished = false;
+ rightMotor.setSpeed(Constants.speed);
+ rightMotor.setAcceleration(Constants.acceleration);
+ leftMotor.setSpeed(Constants.speed);
+ leftMotor.setAcceleration(Constants.acceleration);
+ rightMotor.stop(true);
+ leftMotor.stop(true);
}
@Override
public void suppress() {
suppressed = true;
+ finished = true;
+ rightMotor.setSpeed(Constants.speed);
+ rightMotor.setAcceleration(Constants.acceleration);
+ leftMotor.setSpeed(Constants.speed);
+ leftMotor.setAcceleration(Constants.acceleration);
+ rightMotor.stop(true);
+ leftMotor.stop(true);
}
- @Override
- public boolean takeControl() {
+ @Override public boolean takeControl(){
return true;
}
}