Made sure status angle is an int and made goal pairing better

1 files changed, 88 insertions, 18 deletions

diff --git a/arduino/arduino.ino b/arduino/arduino.ino
index c4c6c99..35ebe17 100644
--- a/arduino/arduino.ino
+++ b/arduino/arduino.ino
@@ -251,7 +251,7 @@ void loop() {
       }
       else if (digitalRead(RIR)) {
         delay(50);
-        analogWrite(D1, 150);
+        analogWrite(D1, 100);
         digitalWrite(D2, LOW);
         digitalWrite(D3, LOW);
         analogWrite(D4, 150);
@@ -261,13 +261,13 @@ void loop() {
         delay(50);
         digitalWrite(D1, LOW);
         analogWrite(D2, 150);
-        analogWrite(D3, 150);
+        analogWrite(D3, 100);
         digitalWrite(D4, LOW);
         delay(100);
       }
       else if (digitalRead(FRIR)) {
         delay(50);
-        analogWrite(D1, 255);
+        analogWrite(D1, 200);
         digitalWrite(D2, LOW);
         digitalWrite(D3, LOW);
         analogWrite(D4, 255);
@@ -277,7 +277,7 @@ void loop() {
         delay(50);
         digitalWrite(D1, LOW);
         analogWrite(D2, 255);
-        analogWrite(D3, 255);
+        analogWrite(D3, 200);
         digitalWrite(D4, LOW);
         delay(150);
       }
@@ -291,34 +291,104 @@ void loop() {
     case checkRight:
       command = checkLeft;
 
-      analogWrite(D1, 100);
-      digitalWrite(D2, LOW);
-      digitalWrite(D3, LOW);
-      analogWrite(D4, 100);
-      delay(100 + i);
+      if (digitalRead(RIR)) {
+        delay(50);
+        analogWrite(D1, 150);
+        digitalWrite(D2, LOW);
+        digitalWrite(D3, LOW);
+        analogWrite(D4, 150);
+        delay(100);
+      }
+      else if (digitalRead(LIR)) {
+        delay(50);
+        digitalWrite(D1, LOW);
+        analogWrite(D2, 150);
+        analogWrite(D3, 150);
+        digitalWrite(D4, LOW);
+        delay(100);
+      }
+      else if (digitalRead(FRIR)) {
+        delay(50);
+        analogWrite(D1, 255);
+        digitalWrite(D2, LOW);
+        digitalWrite(D3, LOW);
+        analogWrite(D4, 255);
+        delay(150);
+      }
+      else if (digitalRead(FLIR)) {
+        delay(50);
+        digitalWrite(D1, LOW);
+        analogWrite(D2, 255);
+        analogWrite(D3, 255);
+        digitalWrite(D4, LOW);
+        delay(150);
+      }
+      else {
+        analogWrite(D1, 100);
+        digitalWrite(D2, LOW);
+        digitalWrite(D3, LOW);
+        analogWrite(D4, 100);
+        delay(100 + i);
+        i += 10;
+      }
+
       digitalWrite(D1, LOW);
       digitalWrite(D2, LOW);
       digitalWrite(D3, LOW);
       digitalWrite(D4, LOW);
-      delay(500);
+      delay(200);
 
-      i += 10;
+      
       break;
     case checkLeft:
       command = checkRight;
 
-      digitalWrite(D1, LOW);
-      analogWrite(D2, 100);
-      analogWrite(D3, 100);
-      digitalWrite(D4, LOW);
-      delay(100 + i);
+      if (digitalRead(RIR)) {
+        delay(50);
+        analogWrite(D1, 150);
+        digitalWrite(D2, LOW);
+        digitalWrite(D3, LOW);
+        analogWrite(D4, 150);
+        delay(100);
+      }
+      else if (digitalRead(LIR)) {
+        delay(50);
+        digitalWrite(D1, LOW);
+        analogWrite(D2, 150);
+        analogWrite(D3, 150);
+        digitalWrite(D4, LOW);
+        delay(100);
+      }
+      else if (digitalRead(FRIR)) {
+        delay(50);
+        analogWrite(D1, 255);
+        digitalWrite(D2, LOW);
+        digitalWrite(D3, LOW);
+        analogWrite(D4, 255);
+        delay(150);
+      }
+      else if (digitalRead(FLIR)) {
+        delay(50);
+        digitalWrite(D1, LOW);
+        analogWrite(D2, 255);
+        analogWrite(D3, 255);
+        digitalWrite(D4, LOW);
+        delay(150);
+      }
+      else {
+        digitalWrite(D1, LOW);
+        analogWrite(D2, 100);
+        analogWrite(D3, 100);
+        digitalWrite(D4, LOW);
+        delay(100 + i);
+        i += 10;
+      }
+
       digitalWrite(D1, LOW);
       digitalWrite(D2, LOW);
       digitalWrite(D3, LOW);
       digitalWrite(D4, LOW);
       delay(500);
-
-      i += 10;
       break;
     default:
       digitalWrite(D1, LOW);