diff options
Diffstat (limited to 'Venus_Skeleton/Venus_Skeleton.ino')
| -rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 10 |
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; } } |