car7DirectionPathFinder-1-1.ino

#include <TFMini.h>
#include <SoftwareSerial.h>

// must be a 8x1 integer array of LED pins
const int DIRECTIONAL_LED_PINS[] = {
    13, // north status (id = 0)
    12, // northeast status (id = 1)
    11, // east status (id = 2)
    10, // southeast status (id = 3)
    9, // south status (id = 4)
    8, // southwest status (id = 5)
    7, // west status (id = 6)
    6 // decision status (no id)
};

// rx, tx
const int TFMINI_PINS[] = {4, 5};

// milliseconds
const int LED_BLINK_DELAY = 50;
const int PING_WAIT_TIME = 100 * 2;

SoftwareSerial tfMiniSerial(TFMINI_PINS[1], TFMINI_PINS[0]);
TFMini tfMini;

// dest should be a 2x1 integer array
// dest[0] = direction id
// dest[1] = distance (no units)
// will return the distance of the last least point, if there are two matching distances then the distance that comes later wins priority
void *getBestDirection(int n, int ne, int e, int se, int s, int sw, int w, int dest[]) {
    int distances[7] = {n, ne, e, se, s, sw, w};
    int indexOfLowestDistance = -1;
    int valueOfLowestDistance = distances[0];

    for (int i = 0; i < sizeof(distances) / sizeof(int); i++) {
        if (distances[i] <= valueOfLowestDistance) {
            indexOfLowestDistance = i;
            valueOfLowestDistance = distances[i];
            
            Serial.println(sizeof(distances));
        }
    }

    dest[0] = indexOfLowestDistance;
    dest[1] = valueOfLowestDistance;
}

void setup() {
    Serial.begin(115200);
    while(!Serial){}

    tfMiniSerial.begin(115200);
    tfMini.begin(&tfMiniSerial);

    delay(100);
    
    pinMode(LED_BUILTIN, OUTPUT);
    for (int i = 0; i < sizeof(DIRECTIONAL_LED_PINS) / sizeof(int); i++) {
        pinMode(DIRECTIONAL_LED_PINS[i], OUTPUT);
        digitalWrite(DIRECTIONAL_LED_PINS[i], HIGH);
        delay(LED_BLINK_DELAY);
        digitalWrite(DIRECTIONAL_LED_PINS[i], LOW);
    }

    // testing to make sure that everything is working fine, with hardcoded values
    int bestDirection[2];
    getBestDirection(2, 2, 3, 4, 1, 6, 7, bestDirection);
    if (bestDirection[0] == 4 && bestDirection[1] == 1) {
        Serial.println("[DEBUG] GETBESTDIRECTION CHECK PASSED");
        delay(LED_BLINK_DELAY * 2);
        digitalWrite(DIRECTIONAL_LED_PINS[7], HIGH);
        delay(LED_BLINK_DELAY * 2);
        digitalWrite(DIRECTIONAL_LED_PINS[7], LOW);
    } else {
        Serial.println("[DEBUG] GETBESTDIRECTION CHECK FAILED");
    }

    delay(PING_WAIT_TIME);

    Serial.println(tfMini.getDistance());
    Serial.println(tfMini.getRecentSignalStrength());
}

void loop() {
}