From 98f5cc947880fa0f05b76d1d3861f973184b12cb Mon Sep 17 00:00:00 2001
From: Peter de Kok <p.j.s.d.kok@gmail.com>
Date: Wed, 17 Jun 2015 16:09:18 +0200
Subject: fixed last turretSequence and counterTurret error

---
 Venus_Skeleton/Venus_Skeleton.ino | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

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;
 				}
 			}
-- 
cgit v1.2.1