fixed last turretSequence and counterTurret error

1 files changed, 9 insertions, 1 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index f188795..269bc34 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -197,6 +197,14 @@ void initTurretSequence(int array[], int numOfPositions) {
 	}
 }
 
+int findInArray(int n, int array[], int length) {
+	for (int g = 0; g < length; g++) {
+		if (array[g] == n) {
+			return g;
+		}
+	}
+	return -1;
+}
 
 
 // **********************
@@ -726,7 +734,7 @@ int eliminateDirections(int directionArray[]) {
 			// for all turret directions relative to this robot direction, check obstacle sensor data
 			for (int k = kinit; k <= (kinit + 2*kinc); k++) {
 				if (k < 0) k = 0;
-				if (data.obstacle_turret_distances[k] > OBSTACLE_TOO_CLOSE) {
+				if (data.obstacle_turret_distances[findInArray(k, turretSequence, NUM_TURRET_DIRECTIONS)] < OBSTACLE_TOO_CLOSE) {
 					directionArray[i] = 0;
 				}
 			}