Probably finished

agaitanis · May 2, 2022 · 49ce8c6 · 49ce8c6
1 parent aee1b80
commit 49ce8c6
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 17 deletions.
diff --git a/project/src/project/Env.java b/project/src/project/Env.java
@@ -5,7 +5,7 @@
 
 public class Env extends EnvironmentDescription {
     public Env() {
-        Point3d light = new Point3d(-6, 2, 3);
+        Point3d light = new Point3d(-5, 2, 3);
         light1SetPosition(light.x, light.y, light.z);
         light2SetPosition(light.x, light.y, light.z);
 
@@ -21,7 +21,7 @@ public Env() {
         add(new Box(new Vector3d(2, 0, 0), new Vector3f(2, 1, 7), this));
         add(new Box(new Vector3d(-2, 0, -1), new Vector3f(2, 1, 10), this));
 
-        add(new Arch(new Vector3d(-6, 0, 0), this));
+        add(new Arch(new Vector3d(-5, 0, 0), this));
 
         Line line = new Line(new Vector3d(-4, 0, 8), 12, this);
         line.rotate90(1);
@@ -37,15 +37,15 @@ public Env() {
         line3.rotate90(1);
         add(line3);
 
-        add(new Line(new Vector3d(-6, 0, -8), 8, this));
+        add(new Line(new Vector3d(-5, 0, -8), 8, this));
         add(new Line(new Vector3d(2, 0, -8), 5, this));
-        add(new Line(new Vector3d(-8, 0, -2), 2, this));
+        add(new Line(new Vector3d(-7, 0, -2), 2, this));
 
-        Line line4 = new Line(new Vector3d(-8, 0, -2), 2, this);
+        Line line4 = new Line(new Vector3d(-7, 0, -2), 2, this);
         line4.rotate90(1);
         add(line4);
 
-        Line line5 = new Line(new Vector3d(-6, 0, -4), 4, this);
+        Line line5 = new Line(new Vector3d(-5, 0, -4), 2, this);
         line5.rotate90(1);
         add(line5);
     }

diff --git a/project/src/project/MyRobot.java b/project/src/project/MyRobot.java
@@ -220,23 +220,29 @@ private void followLine() {
             right += line.hasHit(line.getNumSensors() - i - 1) ? 1 : 0;
             k++;
         }
-               
+
         if (left == 1 && right == 1) {
             status = RobotStatus.FOLLOW_LINE_STRAIGHT;
-        } else if (left >= 3 && right >= 3) {
-            if (leftLux > rightLux) {
-                status = RobotStatus.FOLLOW_LINE_LEFT;
-            } else {
-                status = RobotStatus.FOLLOW_LINE_RIGHT;
-            }
         }
 
         if (status == RobotStatus.FOLLOW_LINE_LEFT) {
             right = 0;
         } else if (status == RobotStatus.FOLLOW_LINE_RIGHT) {
             left = 0;
+        } else {
+            if (left >= 3 && right >= 3) {
+                if (leftLux > rightLux) {
+                    status = RobotStatus.FOLLOW_LINE_LEFT;
+                    right = 0;
+                } else {
+                    status = RobotStatus.FOLLOW_LINE_RIGHT;
+                    left = 0;
+                } 
+            } else {
+                status = RobotStatus.FOLLOW_LINE_STRAIGHT;
+            }
         }
-               
+
         double diff = (left - right)/(float)k;
 
         setRotationalVelocity(diff);
@@ -245,8 +251,7 @@ private void followLine() {
 
     @Override
     public void initBehavior() {
-        status = RobotStatus.FOLLOW_LINE_STRAIGHT;
-        setTranslationalVelocity(1);
+
     }
 
     @Override
@@ -290,7 +295,9 @@ public void performBehavior() {
                 }
                 break;
             case REORIENT:
-                if (isAlignedWithLight()) {
+                if (lineWasDetected()) {
+                    followLine();
+                } else if (isAlignedWithLight()) {
                     iL = getIntensity();
                     moveFwd();
                 } else {