diff --git a/.gitignore b/.gitignore index 6ac360c..8e6eec3 100644 --- a/.gitignore +++ b/.gitignore @@ -25,3 +25,4 @@ hs_err_pid* /Examples/MyFirst/build/ /project/nbproject/private/ /project/build/ +/project/dist/ diff --git a/project/src/project/MyRobot.java b/project/src/project/MyRobot.java index 229e0f0..4dc3fed 100644 --- a/project/src/project/MyRobot.java +++ b/project/src/project/MyRobot.java @@ -6,6 +6,7 @@ enum RobotStatus { MOVE_FWD, REORIENT, + CIRVUMNAVIGATE, STOP, } @@ -13,24 +14,30 @@ public class MyRobot extends Agent { LightSensor leftLight; LightSensor rightLight; RangeSensorBelt sonars; + RangeSensorBelt bumpers; RobotStatus status; + double leftLuxSum; + double rightLuxSum; double leftLux; double rightLux; double prevLeftLux; double prevRightLux; + double prevPrevLux; + double iL; + double iH; int cnt; static double K_ROT = 10; static double K_ROT_2 = 0.03; - static double K_TRANSL = 40; + static double K_TRANSL = 35; static double K_TRANSL_2 = 0.01; static double GOAL_LUX = 0.053; + static boolean CLOCKWISE = false; static double SAFETY = 0.8; - static double K1 = 5; - static double K2 = 0.9; + static double K1 = 2; + static double K2 = 1; static double K3 = 3; - static boolean CLOCKWISE = true; public MyRobot (Vector3d position, String name) { super(position, name); @@ -38,47 +45,93 @@ public MyRobot (Vector3d position, String name) { leftLight = RobotFactory.addLightSensorLeft(this); rightLight = RobotFactory.addLightSensorRight(this); sonars = RobotFactory.addSonarBeltSensor(this, 12, 1.5f); - status = RobotStatus.MOVE_FWD; + bumpers = RobotFactory.addBumperBeltSensor(this, 8); + status = RobotStatus.REORIENT; + leftLuxSum = 0; + rightLuxSum = 0; leftLux = 0; rightLux = 0; - prevLeftLux = 1; - prevRightLux = 1; + prevLeftLux = 0; + prevRightLux = 0; + prevPrevLux = 0; + iL = 0; + iH = 0; cnt = 0; } - private boolean shouldStop() { - double avgLux = 0.5*(leftLux + rightLux); + private double getIntensity() { + return 0.5*(leftLux + rightLux); + } + + private void updateLux() { + double curLeftLux = leftLight.getLux(); + double curRightLux = rightLight.getLux(); + + cnt++; + + if (cnt == 10) { + leftLux = leftLuxSum/cnt; + rightLux = rightLuxSum/cnt; + leftLuxSum = 0; + rightLuxSum = 0; + cnt = 0; + } else { + leftLuxSum += curLeftLux; + rightLuxSum += curRightLux; + } + } + + private void updatePrevLux() { + if (cnt == 0) { + prevPrevLux = 0.5*(prevLeftLux + prevRightLux); + prevLeftLux = leftLux; + prevRightLux = rightLux; + } + } + + private boolean foundLocalMax() { + double lux = 0.5*(leftLux + rightLux); + double prevLux = 0.5*(prevLeftLux + prevRightLux); + + return prevLux > prevPrevLux && prevLux > lux; + } + + private boolean reachedLight() { + double lux = 0.5*(leftLux + rightLux); Point3d pos = new Point3d(); getCoords(pos); - return avgLux > GOAL_LUX; + return lux > GOAL_LUX; } private void stop() { + status = RobotStatus.STOP; setTranslationalVelocity(0); setRotationalVelocity(0); } private boolean shouldReorient() { - double avgLux = 0.5*(leftLux + rightLux); - double prevAvgLux = 0.5*(prevLeftLux + prevRightLux); - double diff = avgLux - prevAvgLux; + double lux = 0.5*(leftLux + rightLux); + double prevLux = 0.5*(prevLeftLux + prevRightLux); + double diff = lux - prevLux; return diff < 0; } private void reorient() { + status = RobotStatus.REORIENT; + double diff = leftLux - rightLux; - double avgLux = 0.5*(leftLux + rightLux); - double goalDiff = GOAL_LUX - avgLux; + double lux = 0.5*(leftLux + rightLux); + double goalDiff = GOAL_LUX - lux; setTranslationalVelocity(0); setRotationalVelocity(K_ROT * Math.signum(diff) * (Math.log10(Math.abs(goalDiff) + 1) + K_ROT_2)); } - private boolean shouldMoveFwd() { + private boolean isAlignedWithLight() { double diff = leftLux - rightLux; double prevDiff = prevLeftLux - prevRightLux; @@ -86,8 +139,10 @@ private boolean shouldMoveFwd() { } private void moveFwd() { - double avgLux = 0.5*(leftLux + rightLux); - double goalDiff = GOAL_LUX - avgLux; + status = RobotStatus.MOVE_FWD; + + double lux = 0.5*(leftLux + rightLux); + double goalDiff = GOAL_LUX - lux; setTranslationalVelocity(K_TRANSL*(Math.log10(Math.abs(goalDiff) + 1) + K_TRANSL_2)); setRotationalVelocity(0); @@ -111,6 +166,8 @@ private Point3d getSensedPoint(int sonar){ } private void circumNavigate() { + status = RobotStatus.CIRVUMNAVIGATE; + int min = 0; for (int i = 1; i < sonars.getNumSensors(); i++) { @@ -120,7 +177,7 @@ private void circumNavigate() { } Point3d p = getSensedPoint(min); - double d = p.distance(new Point3d(0, 0, 0)); + double d = p.distance(new Point3d(0, 0, 0)); Vector3d v = CLOCKWISE ? new Vector3d(-p.z, 0, p.x) : new Vector3d(p.z, 0, -p.x); double phLin = Math.atan2(v.z, v.x); @@ -136,6 +193,10 @@ private void circumNavigate() { setTranslationalVelocity(K2*Math.cos(phRef)); } + private boolean bumperWasHit() { + return bumpers.oneHasHit(); + } + @Override public void initBehavior() { } @@ -145,47 +206,45 @@ public void performBehavior() { if (status == RobotStatus.STOP) { return; } - - double curLeftLux = leftLight.getLux(); - double curRightLux = rightLight.getLux(); - cnt++; - if (cnt == 1) { - leftLux = 0; - rightLux = 0; - } - if (cnt < 10) { - leftLux += curLeftLux; - rightLux += curRightLux; - return; - } - leftLux /= cnt; - rightLux /= cnt; - cnt = 0; - - if (shouldStop()) { + updateLux(); + + if (reachedLight()) { stop(); - status = RobotStatus.STOP; return; } - if (status == RobotStatus.MOVE_FWD) { - if (shouldReorient()) { - reorient(); - status = RobotStatus.REORIENT; - } else { - moveFwd(); - } - } else if (status == RobotStatus.REORIENT) { - if (shouldMoveFwd()) { - moveFwd(); - status = RobotStatus.MOVE_FWD; - } else { - reorient(); - } + switch (status) { + case MOVE_FWD: + if (bumperWasHit()) { + if (iL != getIntensity()) iH = getIntensity(); + circumNavigate(); + } else if (shouldReorient()) { + if (iL != getIntensity()) iH = getIntensity(); + reorient(); + } else { + moveFwd(); + } + break; + case REORIENT: + if (isAlignedWithLight()) { + iL = getIntensity(); + moveFwd(); + } else { + reorient(); + } + break; + case CIRVUMNAVIGATE: + if (foundLocalMax() && getIntensity() > iH) { + reorient(); + } else { + circumNavigate(); + } + break; + default: + break; } - prevLeftLux = leftLux; - prevRightLux = rightLux; + updatePrevLux(); } }