Working code from Lab-3 (as discussed in Lab-4)

Lab_3_Working_Code.cpp

/*
 * Design of Autonomous Systems Lab 3 Code (Spr'26)
 * Sam Ul Haque
*/


#include <iostream>
#include <iomanip>
#include <cstdio>
#include <cstring>
#include <array>
#include <chrono>
#include <thread>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/gpio.h>
#include <csignal>
#include <atomic>

std::atomic<bool> running(true);

// GPIO pin definitions for the RGB LED
#define GPIO_RED   16
#define GPIO_GREEN 19
#define GPIO_BLUE  20

// Ultrasonic pins
#define ULTRASONIC_TRIG 23
#define ULTRASONIC_ECHO 24

static int out_fd = -1, in_fd = -1;
static gpiohandle_data out_state{};          // persistent output state
static std::array<int, 64> out_idx, in_idx;  // pin -> index (or -1)

enum State {
    NOT_CALLED,
    INITIALIZING,
    SUCCESS,
    FAILURE
};

State init_state = NOT_CALLED;

std::atomic<float> distance(0.0); // Shared variable for distance measurement

static bool init() {

    if (init_state == SUCCESS) return true; // 100ns
    if (init_state == FAILURE) return false;
    if (init_state == INITIALIZING) return false; // prevent recursion
    // Else NOT_CALLED, so we start initializing
    init_state = INITIALIZING;

    if (out_fd >= 0 && in_fd >= 0) return true;

    out_idx.fill(-1);
    in_idx.fill(-1);
    memset(&out_state, 0, sizeof(out_state));

    int OUT_PINS[] = {GPIO_RED, GPIO_GREEN, GPIO_BLUE, ULTRASONIC_TRIG};
    int IN_PINS[]  = {ULTRASONIC_ECHO};

    int chip_fd = open("/dev/gpiochip0", O_RDONLY);
    if (chip_fd < 0) { perror("open gpiochip0"); return false; }

    int tmp_out_fd = -1, tmp_in_fd = -1;
    gpiohandle_data tmp_out_state{};

    // outputs
    {
        gpiohandle_request req{};
        req.lines = (int)(sizeof(OUT_PINS)/sizeof(OUT_PINS[0]));
        req.flags = GPIOHANDLE_REQUEST_OUTPUT;
        strcpy(req.consumer_label, "outs");
        for (int i = 0; i < req.lines; i++) {
            int pin = OUT_PINS[i];
            req.lineoffsets[i] = pin;
            req.default_values[i] = 0;
            if (pin >= 0 && pin < (int)out_idx.size()) out_idx[pin] = i;
        }
        if (ioctl(chip_fd, GPIO_GET_LINEHANDLE_IOCTL, &req) < 0) {
            perror("GET_LINEHANDLE outs (This can be ignored!)");
            close(chip_fd);
            return false;
        }
        tmp_out_fd = req.fd;
        tmp_out_state = gpiohandle_data{}; // start all low
    }

    // inputs
    {
        gpiohandle_request req{};
        req.lines = (int)(sizeof(IN_PINS)/sizeof(IN_PINS[0]));
        req.flags = GPIOHANDLE_REQUEST_INPUT;
        strcpy(req.consumer_label, "ins");
        for (int i = 0; i < req.lines; i++) {
            int pin = IN_PINS[i];
            req.lineoffsets[i] = pin;
            if (pin >= 0 && pin < (int)in_idx.size()) in_idx[pin] = i;
        }
        if (ioctl(chip_fd, GPIO_GET_LINEHANDLE_IOCTL, &req) < 0) {
            perror("GET_LINEHANDLE ins");
            // important: release outputs if inputs fail
            if (tmp_out_fd >= 0) close(tmp_out_fd);
            close(chip_fd);
            return false;
        }
        tmp_in_fd = req.fd;
    }

    close(chip_fd);

    // commit only when both succeeded
    out_fd = tmp_out_fd;
    in_fd  = tmp_in_fd;
    out_state = tmp_out_state;
    init_state = SUCCESS;
    return true;
}

static void write_pin(int pin, int v) {

    while (!init() && running.load()) usleep(1000); // wait and retry if initialization failed
    

    int idx = (pin >= 0 && pin < (int)out_idx.size()) ? out_idx[pin] : -1;
    if (idx < 0) { fprintf(stderr, "pin %d not in OUT_PINS\n", pin); return; }

    out_state.values[idx] = v ? 1 : 0;
    if (ioctl(out_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &out_state) < 0)
        perror("SET_LINE_VALUES");
}

static int read_pin(int pin) {
    
    while (!init() && running.load()) usleep(1000); // wait and retry if initialization failed
    
    int idx = (pin >= 0 && pin < (int)in_idx.size()) ? in_idx[pin] : -1;
    if (idx < 0) { fprintf(stderr, "pin %d not in IN_PINS\n", pin); return -1; }

    gpiohandle_data in_data{};
    if (ioctl(in_fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &in_data) < 0) {
        perror("GET_LINE_VALUES"); return -1;
    }
    return in_data.values[idx];
}

void readSensor() {
      while (running.load()) {

        std::this_thread::sleep_for(std::chrono::milliseconds(500)); // Wait for 500ms before the next reading
        
        using clock = std::chrono::steady_clock;
 

        // Trigger pulse (HC-SR04 expects ~10us HIGH)
        write_pin(ULTRASONIC_TRIG, 0);
        std::this_thread::sleep_for(std::chrono::microseconds(2));
        write_pin(ULTRASONIC_TRIG, 1);
        std::this_thread::sleep_for(std::chrono::microseconds(10));
        write_pin(ULTRASONIC_TRIG, 0);

        // Optional: add timeouts so you don't hang forever (lab 4)
        auto t0 = clock::now();
        while (read_pin(ULTRASONIC_ECHO) == 0 && running.load()) {
            if (clock::now() - t0 > std::chrono::milliseconds(50)) {
                // std::cout << "Echo never went HIGH (timeout)\n";
                break; // Skip to the next iteration to try again
            }
        }
        auto start = clock::now();  // timestamp ONCE when echo becomes HIGH

        while (read_pin(ULTRASONIC_ECHO) == 1 && running.load()) {
            if (clock::now() - start > std::chrono::milliseconds(50)) {
                // std::cout << "Echo stayed HIGH too long (timeout)\n";
                break; // Skip to the next iteration to try again
            }
        }
         auto end = clock::now();    // timestamp ONCE when echo becomes LOW

        auto pulse_us = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();

        // Convert to distance (speed of sound ~343 m/s => 0.0343 cm/us), divide by 2 for round trip
        double distance_cm = (pulse_us * 0.0343) / 2.0;

        distance.store(distance_cm); // Update the shared atomic variable

        // Print distance with specific number of digits
        std::cout << "Distance measured: " << std::fixed << std::setprecision(2) << distance_cm << " cm\r";
        std::cout.flush();
      }
      std::cout << "Sensor reading thread exiting...\n";
}

void flashLED() {
      const float threshold = 30.0; // Distance threshold in cm
      while (running.load()) {
         float currentDistance = distance.load(); // Read the shared atomic variable
         if (currentDistance < threshold) {
            // Flash the LED
            write_pin(GPIO_RED, 1); // Turn on red LED
            usleep(50000); // Wait for 500ms
            write_pin(GPIO_RED, 0); // Turn off red LED
            usleep(50000); // Wait for 500ms
            write_pin(GPIO_RED, 1); // Turn on red LED
            usleep(50000); // Wait for 500ms
            write_pin(GPIO_RED, 0); // Turn off red LED
            usleep(500000); // Wait for 500ms
         }
      }
        std::cout << "LED flashing thread exiting...\n";
}

void cleanup() {
    std::cout << "\nCleaning up GPIO...\n";

    // Turn everything OFF
    if (out_fd >= 0) {
        for (int i = 0; i < 64; i++) {
            if (out_idx[i] >= 0)
                out_state.values[out_idx[i]] = 0;
        }
        ioctl(out_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &out_state);
        close(out_fd);
        out_fd = -1;
    }

    if (in_fd >= 0) {
        close(in_fd);
        in_fd = -1;
    }

    std::cout << "GPIO released. Bye!\n";
}

void signal_handler(int sig) {
    if (sig == SIGINT) {
        running.store(false);
    }
}

int main() {
    signal(SIGINT, signal_handler);
   // Start the sensor reading thread
   std::thread sensorThread(readSensor);


   // Start the LED flashing thread
   std::thread ledThread(flashLED);

   // Join threads (optional, as they run indefinitely)
   sensorThread.join();
   ledThread.join();

   cleanup();

   return 0;
}