/*

 * RTOS Elevator Controller - 4 Floors (STM32)

 * Hardware: STM32, 4 Floor LEDs, 4 Floor Buttons, 1 Door LED

 * Demonstrates: Tasks, Queue, Semaphore, State Machine

 */



#include <Arduino.h>

#include <STM32FreeRTOS.h>



// Pin Definitions for STM32

#define FLOOR_0_LED PC13

#define FLOOR_1_LED PC14

#define FLOOR_2_LED PC15

#define FLOOR_3_LED PA0

#define DOOR_LED PA1



#define BUTTON_0 PB0

#define BUTTON_1 PB1

#define BUTTON_2 PB10

#define BUTTON_3 PB11



// Timing constants (milliseconds)

#define FLOOR_TRAVEL_TIME 2000  // Time to move between floors

#define DOOR_OPEN_TIME 3000     // How long door stays open

#define BUFFER_DELAY 3000

// Global variables

QueueHandle_t floorRequestQueue;

SemaphoreHandle_t doorSemaphore;

volatile int currentFloor = 0;



enum ElevatorState {

  IDLE,

  MOVING,

  DOOR_OPENING,

  DOOR_OPEN,

  DOOR_CLOSING

};



ElevatorState elevatorState = IDLE;



// Function to update floor LEDs

void updateFloorLED(int floor) {

  digitalWrite(FLOOR_0_LED, floor == 0 ? HIGH : LOW);

  digitalWrite(FLOOR_1_LED, floor == 1 ? HIGH : LOW);

  digitalWrite(FLOOR_2_LED, floor == 2 ? HIGH : LOW);

  digitalWrite(FLOOR_3_LED, floor == 3 ? HIGH : LOW);

}



// Button Interrupt Service Routines

void button0ISR() {

  int floor = 0;

  BaseType_t xHigherPriorityTaskWoken = pdFALSE;

  xQueueSendFromISR(floorRequestQueue, &floor, &xHigherPriorityTaskWoken);

  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);

}



void button1ISR() {

  int floor = 1;

  BaseType_t xHigherPriorityTaskWoken = pdFALSE;

  xQueueSendFromISR(floorRequestQueue, &floor, &xHigherPriorityTaskWoken);

  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);

}



void button2ISR() {

  int floor = 2;

  BaseType_t xHigherPriorityTaskWoken = pdFALSE;

  xQueueSendFromISR(floorRequestQueue, &floor, &xHigherPriorityTaskWoken);

  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);

}



void button3ISR() {

  int floor = 3;

  BaseType_t xHigherPriorityTaskWoken = pdFALSE;

  xQueueSendFromISR(floorRequestQueue, &floor, &xHigherPriorityTaskWoken);

  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);

}



// Task 1: Elevator Controller (Main logic)

void elevatorTask(void *pvParameters) {

  int targetFloor;

 

  while (1) {

    // Wait for floor request from queue

    if (xQueueReceive(floorRequestQueue, &targetFloor, portMAX_DELAY) == pdTRUE) {

     

      // Ignore if already on requested floor

      if (targetFloor == currentFloor) {

        Serial.print("Already on floor ");

        Serial.println(targetFloor);

        continue;

      }

     

      Serial.print("Request received: Floor ");

      Serial.println(targetFloor);

      elevatorState = MOVING;

     

      // Move elevator floor by floor

      if (targetFloor > currentFloor) {

        // Moving UP

        for (int i = currentFloor + 1; i <= targetFloor; i++) {

          Serial.print("Moving to Floor ");

          Serial.println(i);

          currentFloor = i;

          updateFloorLED(currentFloor);

          vTaskDelay(pdMS_TO_TICKS(FLOOR_TRAVEL_TIME));

        }

      } else {

        // Moving DOWN

        for (int i = currentFloor - 1; i >= targetFloor; i--) {

          Serial.print("Moving to Floor ");

          Serial.println(i);

          currentFloor = i;

          updateFloorLED(currentFloor);

          vTaskDelay(pdMS_TO_TICKS(FLOOR_TRAVEL_TIME));

        }

      }

     

      Serial.print("Arrived at Floor ");

      Serial.println(currentFloor);

     

      // Signal door task to open door

      xSemaphoreGive(doorSemaphore);

     

      elevatorState = IDLE;

    }

  }

}



// Task 2: Door Controller

void doorTask(void *pvParameters) {

  while (1) {

    // Wait for signal from elevator task

    if (xSemaphoreTake(doorSemaphore, portMAX_DELAY) == pdTRUE) {

     

      // Open door

      elevatorState = DOOR_OPENING;

      Serial.println("Door opening...");

      digitalWrite(DOOR_LED, HIGH);

      vTaskDelay(pdMS_TO_TICKS(500)); // Opening animation

     

      // Door open

      elevatorState = DOOR_OPEN;

      Serial.println("Door open - waiting for passengers");

      vTaskDelay(pdMS_TO_TICKS(DOOR_OPEN_TIME));

     

      // Close door

      elevatorState = DOOR_CLOSING;

      Serial.println("Door closing...");

      vTaskDelay(pdMS_TO_TICKS(500)); // Closing animation

      digitalWrite(DOOR_LED, LOW);

     

      Serial.println("Door closed - ready for next request");

      Serial.println("------------------------");

      elevatorState = IDLE;

    }

  }

}



// Task 3: Status Display

void statusTask(void *pvParameters) {

  while (1) {

    Serial.print("Current Floor: ");

    Serial.print(currentFloor);

    Serial.print(" | State: ");

   

    switch (elevatorState) {

      case IDLE:

        Serial.println("IDLE");

        break;

      case MOVING:

        Serial.println("MOVING");

        break;

      case DOOR_OPENING:

        Serial.println("DOOR OPENING");

        break;

      case DOOR_OPEN:

        Serial.println("DOOR OPEN");

        break;

      case DOOR_CLOSING:

        Serial.println("DOOR CLOSING");

        break;

    }

   

    vTaskDelay(pdMS_TO_TICKS(5000)); // Update every 5 seconds

  }

}



void setup() {

  Serial.begin(115200);

  delay(1000); // Give serial time to initialize

 

  Serial.println("\n=== RTOS Elevator Controller ===");

  Serial.println("STM32 with FreeRTOS");

  Serial.println("4 Floors | FIFO Queue System");

  Serial.println("================================\n");

 

  // Initialize LED pins

  pinMode(FLOOR_0_LED, OUTPUT);

  pinMode(FLOOR_1_LED, OUTPUT);

  pinMode(FLOOR_2_LED, OUTPUT);

  pinMode(FLOOR_3_LED, OUTPUT);

  pinMode(DOOR_LED, OUTPUT);

 

  // Initialize button pins

  pinMode(BUTTON_0, INPUT_PULLUP);

  pinMode(BUTTON_1, INPUT_PULLUP);

  pinMode(BUTTON_2, INPUT_PULLUP);

  pinMode(BUTTON_3, INPUT_PULLUP);

 

  // Set initial floor

  updateFloorLED(currentFloor);

 

  // Attach interrupts to buttons

  attachInterrupt(digitalPinToInterrupt(BUTTON_0), button0ISR, FALLING);

  attachInterrupt(digitalPinToInterrupt(BUTTON_1), button1ISR, FALLING);

  attachInterrupt(digitalPinToInterrupt(BUTTON_2), button2ISR, FALLING);

  attachInterrupt(digitalPinToInterrupt(BUTTON_3), button3ISR, FALLING);

 

  // Create RTOS primitives

  floorRequestQueue = xQueueCreate(10, sizeof(int)); // Queue holds 10 requests

  doorSemaphore = xSemaphoreCreateBinary();

 

  if (floorRequestQueue == NULL || doorSemaphore == NULL) {

    Serial.println("ERROR: Failed to create RTOS objects!");

    while(1);

  }

 

  // Create tasks

  xTaskCreate(

    elevatorTask,      // Task function

    "ElevatorControl", // Task name

    128,               // Stack size (words, not bytes)

    NULL,              // Parameters

    2,                 // Priority (higher = more important)

    NULL               // Task handle

  );

 

  xTaskCreate(

    doorTask,

    "DoorControl",

    128,

    NULL,

    2,                 // Same priority as elevator

    NULL

  );

 

  xTaskCreate(

    statusTask,

    "StatusDisplay",

    128,

    NULL,

    1,                 // Lower priority

    NULL

  );

 

  Serial.println("System initialized!");

  Serial.println("Press buttons to call elevator\n");

 

  // Start the scheduler

  vTaskStartScheduler();

 

  // Should never reach here

  Serial.println("ERROR: Scheduler failed to start!");

  while(1);

}



void loop() {

  // Empty - FreeRTOS tasks handle everything

  // This will never be called once scheduler starts

}