From 2f829de04b9de46d17f7890d769c17032aa9175c Mon Sep 17 00:00:00 2001 From: Miguel Angel Astor Romero Date: Thu, 23 Jun 2016 14:21:34 -0400 Subject: [PATCH] Fixed up to ball search. --- .../ucv/ciens/cicore/icaro/ryabi/RyABI.java | 2 +- .../ryabi/behaviors/SearchBallBehavior.java | 102 +++++++++--------- .../behaviors/SensorCalibrationBehavior.java | 2 +- .../cicore/icaro/ryabi/utils/Rotations.java | 4 +- 4 files changed, 55 insertions(+), 55 deletions(-) diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java index 6f1c78c..b96c2c8 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/RyABI.java @@ -78,7 +78,7 @@ public class RyABI { pilot = new CompassPilot(compass, WHEEL_DIAMETER, TRACK_WIDTH, Motor.A, Motor.C); /* Create the feature detectors. */ - rangeDetector = new RangeFeatureDetector(sonar, 20, 200); + rangeDetector = new RangeFeatureDetector(sonar, 30, 200); touchDetector = new TouchFeatureDetector(touch); lightDetector = new LightFeatureDetector(light, invertLightDetector); diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java index 3e69623..defb76d 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SearchBallBehavior.java @@ -96,66 +96,64 @@ public class SearchBallBehavior extends BaseBehavior { @Override public void action() { - while(!supress) { - if(queue.hasNextRangeSensorEvent()) { - /* If the pedestal has been found then stop the robot and set the state to BALL_FOUND. */ - if(pilot.isMoving()) - pilot.stop(); - ballFound = true; - state.setState(States.BALL_FOUND); + if(queue.hasNextRangeSensorEvent()) { + /* If the pedestal has been found then stop the robot and set the state to BALL_FOUND. */ + if(pilot.isMoving()) + pilot.stop(); + ballFound = true; + state.setState(States.BALL_FOUND); - /* Discard unneeded range features. */ - while(queue.hasNextRangeSensorEvent()) - queue.getNextRangeSensorEvent(); + /* Discard unneeded range features. */ + while(queue.hasNextRangeSensorEvent()) + queue.getNextRangeSensorEvent(); + + } else { + if(turnLeft) { + /* Search to the left of the robot. */ + Rotations.rotateM90(compass, pilot); + pilot.travel(100); + + if(queue.hasNextTouchSensorEvent()) { + /* If an obstacle is found while searching then start searching + * to the opposite side. */ + pilot.travel(-200); + + turnLeft = false; + + /* Discard unneeded touch events. */ + while(queue.hasNextTouchSensorEvent()) + queue.getNextTouchSensorEvent(); + + detectorHandler.enableTouchDetector(); + } + + Rotations.rotate90(compass, pilot); } else { - if(turnLeft) { - /* Search to the left of the robot. */ - Rotations.rotateM90(compass, pilot); - pilot.travel(100); + /* Search to the right of the robot. */ + Rotations.rotate90(compass, pilot); + pilot.travel(100); - if(queue.hasNextTouchSensorEvent()) { - /* If an obstacle is found while searching then start searching - * to the opposite side. */ - pilot.travel(-200); + if(queue.hasNextTouchSensorEvent()) { + /* If an obstacle is found while searching then give up and go back + * to the start line. */ + pilot.travel(-200); - turnLeft = false; + state.setState(States.WANDER); + ballFound = true; - /* Discard unneeded touch events. */ - while(queue.hasNextTouchSensorEvent()) - queue.getNextTouchSensorEvent(); + /* Discard unneeded touch events. */ + while(queue.hasNextTouchSensorEvent()) + queue.getNextTouchSensorEvent(); - detectorHandler.enableTouchDetector(); - } - - Rotations.rotate90(compass, pilot); - - } else { - /* Search to the right of the robot. */ - Rotations.rotate90(compass, pilot); - pilot.travel(100); - - if(queue.hasNextTouchSensorEvent()) { - /* If an obstacle is found while searching then give up and go back - * to the start line. */ - pilot.travel(-200); - - state.setState(States.WANDER); - ballFound = true; - - /* Discard unneeded touch events. */ - while(queue.hasNextTouchSensorEvent()) - queue.getNextTouchSensorEvent(); - - detectorHandler.enableTouchDetector(); - } - - Rotations.rotateM90(compass, pilot); - - /* If the ball was found or the robot gave up then turn around towards the start line. */ - if(ballFound) - Rotations.rotate180(compass, pilot); + detectorHandler.enableTouchDetector(); } + + Rotations.rotateM90(compass, pilot); + + /* If the ball was found or the robot gave up then turn around towards the start line. */ + if(ballFound) + Rotations.rotate180(compass, pilot); } } } diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java index 917bd41..dd4455a 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/behaviors/SensorCalibrationBehavior.java @@ -54,7 +54,7 @@ public class SensorCalibrationBehavior extends BaseBehavior { /* Calibrate the compass by turning slowly by 720 degrees. */ System.out.println("Calib. compass"); DifferentialPilot p = new DifferentialPilot(wheelDiameter, trackWidth, Motor.A, Motor.C); - p.setRotateSpeed(25); + p.setRotateSpeed(20); compass.startCalibration(); p.rotate(720, false); compass.stopCalibration(); diff --git a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java index 39b8db2..f73ab0c 100644 --- a/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java +++ b/src/ve/ucv/ciens/cicore/icaro/ryabi/utils/Rotations.java @@ -36,8 +36,9 @@ public abstract class Rotations { diff = 90.0f - cMeasure; do { pilot.rotate(diff); - cMeasure = compass.getDegreesCartesian(); + cMeasure = 360.0f - compass.getDegreesCartesian(); diff = 90.0f - cMeasure; + System.out.println(cMeasure + " " + diff); } while(Math.abs(diff) > 1.0f); pilot.stop(); @@ -60,6 +61,7 @@ public abstract class Rotations { pilot.rotate(diff); cMeasure = compass.getDegreesCartesian(); diff = -90 + cMeasure; + System.out.println(cMeasure + " " + diff); } while(Math.abs(diff) > 1.0f); pilot.stop();