diff --git a/frtos/line_sensor/CMakeLists.txt b/frtos/line_sensor/CMakeLists.txt
index 47ecca5..dcd82c9 100644
--- a/frtos/line_sensor/CMakeLists.txt
+++ b/frtos/line_sensor/CMakeLists.txt
@@ -4,9 +4,11 @@ add_library(line_sensor
         Config.h
 )
 
-target_link_libraries( line_sensor
+target_link_libraries(line_sensor
         hardware_adc
-        pico_stdlib)
+        pico_stdlib
+        FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
+)
 
 target_include_directories(line_sensor
         PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}")
\ No newline at end of file
diff --git a/frtos/line_sensor/Config.h b/frtos/line_sensor/Config.h
index 8a36d39..5eac01c 100644
--- a/frtos/line_sensor/Config.h
+++ b/frtos/line_sensor/Config.h
@@ -6,9 +6,9 @@
 
 #define ADC_READING_DELAY_MS                    ( 300 )
 
-#define ADC_PIN_ONE                             ( 26 )
-#define ADC_PIN_TWO                             ( 27 ) // Comment this line out
-                                                       // if you only have one
+#define LEFT_SENSOR_PIN                         ( 26 )
+#define RIGHT_SENSOR_PIN                        ( 27 ) // Comment this line out
+// if you only have one
 
 #define THRESHOLD                               ( VREF / 2 ) // 50% of VREF
 
diff --git a/frtos/line_sensor/FreeRTOSConfig.h b/frtos/line_sensor/FreeRTOSConfig.h
new file mode 100644
index 0000000..f715e60
--- /dev/null
+++ b/frtos/line_sensor/FreeRTOSConfig.h
@@ -0,0 +1,143 @@
+/*
+ * FreeRTOS V202111.00
+ * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+#ifndef FREERTOS_CONFIG_H
+#define FREERTOS_CONFIG_H
+
+/*-----------------------------------------------------------
+ * Application specific definitions.
+ *
+ * These definitions should be adjusted for your particular hardware and
+ * application requirements.
+ *
+ * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
+ * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
+ *
+ * See http://www.freertos.org/a00110.html
+ *----------------------------------------------------------*/
+
+/* Scheduler Related */
+#define configUSE_PREEMPTION                    1
+#define configUSE_TICKLESS_IDLE                 0
+#define configUSE_IDLE_HOOK                     0
+#define configUSE_TICK_HOOK                     0
+#define configTICK_RATE_HZ                      ( ( TickType_t ) 1000 )
+#define configMAX_PRIORITIES                    32
+#define configMINIMAL_STACK_SIZE                ( configSTACK_DEPTH_TYPE ) 256
+#define configUSE_16_BIT_TICKS                  0
+
+#define configIDLE_SHOULD_YIELD                 1
+
+/* Synchronization Related */
+#define configUSE_MUTEXES                       1
+#define configUSE_RECURSIVE_MUTEXES             1
+#define configUSE_APPLICATION_TASK_TAG          0
+#define configUSE_COUNTING_SEMAPHORES           1
+#define configQUEUE_REGISTRY_SIZE               8
+#define configUSE_QUEUE_SETS                    1
+#define configUSE_TIME_SLICING                  1
+#define configUSE_NEWLIB_REENTRANT              0
+// todo need this for lwip FreeRTOS sys_arch to compile
+#define configENABLE_BACKWARD_COMPATIBILITY     1
+#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5
+
+/* System */
+#define configSTACK_DEPTH_TYPE                  uint32_t
+#define configMESSAGE_BUFFER_LENGTH_TYPE        size_t
+
+/* Memory allocation related definitions. */
+#define configSUPPORT_STATIC_ALLOCATION         0
+#define configSUPPORT_DYNAMIC_ALLOCATION        1
+#define configTOTAL_HEAP_SIZE                   (128*1024)
+#define configAPPLICATION_ALLOCATED_HEAP        0
+
+/* Hook function related definitions. */
+#define configCHECK_FOR_STACK_OVERFLOW          0
+#define configUSE_MALLOC_FAILED_HOOK            0
+#define configUSE_DAEMON_TASK_STARTUP_HOOK      0
+
+/* Run time and task stats gathering related definitions. */
+#define configGENERATE_RUN_TIME_STATS           0
+#define configUSE_TRACE_FACILITY                1
+#define configUSE_STATS_FORMATTING_FUNCTIONS    0
+
+/* Co-routine related definitions. */
+#define configUSE_CO_ROUTINES                   0
+#define configMAX_CO_ROUTINE_PRIORITIES         1
+
+/* Software timer related definitions. */
+#define configUSE_TIMERS                        1
+#define configTIMER_TASK_PRIORITY               ( configMAX_PRIORITIES - 1 )
+#define configTIMER_QUEUE_LENGTH                10
+#define configTIMER_TASK_STACK_DEPTH            1024
+
+/* Interrupt nesting behaviour configuration. */
+/*
+#define configKERNEL_INTERRUPT_PRIORITY         [dependent of processor]
+#define configMAX_SYSCALL_INTERRUPT_PRIORITY    [dependent on processor and application]
+#define configMAX_API_CALL_INTERRUPT_PRIORITY   [dependent on processor and application]
+*/
+
+#if FREE_RTOS_KERNEL_SMP // set by the RP2040 SMP port of FreeRTOS
+/* SMP port only */
+#define configNUM_CORES                         1
+#define configTICK_CORE                         0
+#define configRUN_MULTIPLE_PRIORITIES           1
+#define configUSE_CORE_AFFINITY                 0
+#endif
+
+/* RP2040 specific */
+#define configSUPPORT_PICO_SYNC_INTEROP         1
+#define configSUPPORT_PICO_TIME_INTEROP         1
+
+#include <assert.h>
+/* Define to trap errors during development. */
+#define configASSERT(x)                         assert(x)
+
+/* Set the following definitions to 1 to include the API function, or zero
+to exclude the API function. */
+#define INCLUDE_vTaskPrioritySet                1
+#define INCLUDE_uxTaskPriorityGet               1
+#define INCLUDE_vTaskDelete                     1
+#define INCLUDE_vTaskSuspend                    1
+#define INCLUDE_vTaskDelayUntil                 1
+#define INCLUDE_vTaskDelay                      1
+#define INCLUDE_xTaskGetSchedulerState          1
+#define INCLUDE_xTaskGetCurrentTaskHandle       1
+#define INCLUDE_uxTaskGetStackHighWaterMark     1
+#define INCLUDE_xTaskGetIdleTaskHandle          1
+#define INCLUDE_eTaskGetState                   1
+#define INCLUDE_xTimerPendFunctionCall          1
+#define INCLUDE_xTaskAbortDelay                 1
+#define INCLUDE_xTaskGetHandle                  1
+#define INCLUDE_xTaskResumeFromISR              1
+#define INCLUDE_xQueueGetMutexHolder            1
+
+/* A header file that defines trace macro can be included here. */
+
+#endif /* FREERTOS_CONFIG_H */
+
diff --git a/frtos/line_sensor/line_sensor.c b/frtos/line_sensor/line_sensor.c
index 53dbcfd..91a2ba1 100644
--- a/frtos/line_sensor/line_sensor.c
+++ b/frtos/line_sensor/line_sensor.c
@@ -1,89 +1,20 @@
+#include <stdio.h>
+#include "pico/stdlib.h"
+
+#include "hardware/adc.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+#include "semphr.h"
+
 #include "line_sensor.h"
 #include "string.h"
 
-
-const float conversionFactor = 3.3f / (1 << 12);
+//const float conversionFactor = 3.3f / (1 << 12);
 
 volatile u_int8_t map[MAP_SIZE][MAP_SIZE] = {0};
 
-/**
- * @brief Get the status of the Line Sensor
- * @param gpio The GPIO pin to read from
- * @return state_t LINE_DETECTED or LINE_NOT_DETECTED
- */
-static inline state_t
-get_state(uint gpio) {
-    adc_select_input(gpio - 26); // 26 is the first ADC pin (ADC0)
-
-    // ADC Conversion, vref is 3.3V, 12 bit resolution
-    float adc = (float) adc_read() * conversionFactor;
-
-    return (adc > THRESHOLD)
-           ? LINE_DETECTED : LINE_NOT_DETECTED;
-}
-
-/**
- * @brief Get the Direction based on the Line Sensor
- *
- * This function will detect if the line sensor is detecting a line or not
- * It will read from both ADC channels and return if both are detecting a line
- *
- * @return state_t LINE_DETECTED or LINE_NOT_DETECTED
- */
-static inline
-direction_t get_current_dir() {
-#ifdef ADC_PIN_TWO
-    state_t state_two = get_state(ADC_PIN_TWO);
-#endif
-    state_t state_one = get_state(ADC_PIN_ONE);
-
-#ifdef ADC_PIN_TWO
-    direction_t direction = (state_one << 1) | state_two;
-#else
-    direction_t direction = state_one;
-#endif
-
-    return direction;
-}
-
-
-/**
- * @brief Setup the Line Sensor
- *
- * This function will setup the Line Sensor by initializing the ADC
- */
-void
-line_sensor_setup() {
-
-    adc_init();
-    adc_gpio_init(ADC_PIN_ONE);
-#ifdef ADC_PIN_TWO
-    adc_gpio_init(ADC_PIN_TWO);
-#endif
-
-#ifdef ADC_PIN_THREE            // Additional ADC Pin for Barcode Scanner
-    adc_gpio_init(ADC_PIN_THREE);
-#endif
-
-}
-
-/**
- * @brief Initialize the car's initial state
- *
- * @return The initialized car state
- */
-car_state_t
-initialize_car_state() {
-    car_state_t car_state;
-    car_state.x = MAP_SIZE >> 1;
-    car_state.y = MAP_SIZE >> 1;
-    car_state.current_direction = FORWARD;
-    car_state.orientation = NORTH;
-
-    map[car_state.x][car_state.y] = MAP_START_SYMBOL;
-
-    return car_state;
-}
 
 /**
  * @brief Update the map based on the car's state
@@ -167,43 +98,3 @@ print_map() {
     }
 }
 
-/**
- * @brief FreeRTOS Task to read from the line sensor
- *
- * This task will read from the line sensor and print the state to the console
- *
- * @param params Pointer to the task parameters
- */
-void
-line_sensor_task(__unused void *params) {
-
-    car_state_t car_state = initialize_car_state();
-
-    sleep_ms(2000);
-    printf("Starting Line Sensor Task\n");
-
-
-    for (u_int8_t i = 40; i != 0; i--) {
-        car_state.current_direction = get_current_dir();
-
-        switch (car_state.current_direction) {
-            case FORWARD:
-                handle_forward_movement(&car_state);
-                break;
-            case RIGHT:
-                handle_right_turn(&car_state);
-                break;
-            case LEFT:
-                handle_left_turn(&car_state);
-                break;
-            default:
-                break;
-        }
-
-        update_map(car_state);
-        sleep_ms(ADC_READING_DELAY_MS);
-    }
-
-    print_map();
-
-}
\ No newline at end of file
diff --git a/frtos/line_sensor/line_sensor.h b/frtos/line_sensor/line_sensor.h
index 36d7d6b..14bf3fb 100644
--- a/frtos/line_sensor/line_sensor.h
+++ b/frtos/line_sensor/line_sensor.h
@@ -1,47 +1,283 @@
-#ifndef LINE_SENSOR_H
-#define LINE_SENSOR_H
-
 #include <stdio.h>
 #include "pico/stdlib.h"
 
 #include "hardware/adc.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "message_buffer.h"
+#include "semphr.h"
+
 #include "Config.h"
 
-typedef enum
-{
-    LINE_DETECTED = 1,
-    LINE_NOT_DETECTED = 0,
+typedef enum { // Unused, useful for readability
+    LINE_DETECTED = 0,
+    LINE_NOT_DETECTED = 1,
 } state_t;
 
-typedef enum
-{
+typedef enum {
     ERROR = 0,
     RIGHT = 1,
     LEFT = 2,
     FORWARD = 3
 } direction_t;
 
-typedef enum
-{
+typedef enum {
     NORTH = 0,
     EAST = 1,
     SOUTH = 2,
     WEST = 3,
 } orientation_t;
 
-typedef struct
-{
+typedef struct {
     u_int8_t x;                     // Current x coordinate
     u_int8_t y;                     // Current y coordinate
     direction_t current_direction;  // Current direction (forward, left, right)
     orientation_t orientation;      // Current orientation (N, E, S, W)
 } car_state_t;
 
-void line_sensor_setup();
+// Semaphore
+SemaphoreHandle_t g_left_sensor_sem = NULL;
+SemaphoreHandle_t g_right_sensor_sem = NULL;
 
-static inline state_t get_state(uint gpio);
-static inline direction_t get_current_dir();
+// Queue
+static MessageBufferHandle_t left_sensor_msg_buffer;   // Left Sensor Buffer
+static MessageBufferHandle_t right_sensor_msg_buffer;   // Right Sensor Buffer
 
-void line_sensor_task(__unused void *params);
+// Car State Struct
+static car_state_t g_car_state;
 
-#endif
\ No newline at end of file
+static car_state_t initialize_car_state() {
+    g_car_state.x = MAP_SIZE >> 1;
+    g_car_state.y = MAP_SIZE >> 1;
+    g_car_state.current_direction = FORWARD;
+    g_car_state.orientation = NORTH;
+
+    return g_car_state;
+}
+
+/**
+ * @brief Setup the Line Sensor
+ *
+ * This function will setup the Line Sensor by initializing it as an input
+ */
+static inline void
+line_sensor_setup() {
+    g_left_sensor_sem   = xSemaphoreCreateBinary();
+    g_right_sensor_sem  = xSemaphoreCreateBinary();
+
+    // Setup GPIO Interrupts
+    uint mask           = (1 << LEFT_SENSOR_PIN) | (1 << RIGHT_SENSOR_PIN);
+
+    // Initialise 2 GPIO pins and set them to input
+    gpio_init_mask(mask);
+    gpio_set_dir_in_masked(mask);
+
+    left_sensor_msg_buffer  = xMessageBufferCreate(30);
+    right_sensor_msg_buffer = xMessageBufferCreate(30);
+
+}
+
+
+/**
+ * @brief Monitor the left sensor
+ *
+ * This function will monitor the left sensor and send the state to the
+ * left sensor message buffer, used to calculate the direction of the car
+ *
+ * @param params
+ */
+void
+monitor_left_sensor_task(__unused void *params) {
+    for (;;)
+    {
+        if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
+        {
+            // Get Current State
+            state_t state = gpio_get(LEFT_SENSOR_PIN);
+
+            xMessageBufferSend(left_sensor_msg_buffer,
+                               &state,
+                               sizeof(state_t),
+                               0);
+
+        }
+    }
+}
+
+/**
+ * @brief Monitor the right sensor
+ *
+ * This function will monitor the right sensor and send the state to the
+ * right sensor message buffer, used to calculate the direction of the car
+ *
+ * @param params
+ */
+void
+monitor_right_sensor_task(__unused void *params) {
+    for (;;)
+    {
+        if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE)
+        {
+            // Get Current State
+            state_t state = gpio_get(RIGHT_SENSOR_PIN);
+
+            xMessageBufferSend(right_sensor_msg_buffer,
+                               &state,
+                               sizeof(state_t),
+                               0);
+        }
+    }
+}
+
+/**
+ * @brief Monitor the direction and Oritentation of the car
+ *
+ * This function will monitor the direction and orientation of the car
+ * and update the car state accordingly
+ *
+ * @param params
+ */
+void
+monitor_direction_task(__unused void *params) {
+    state_t left_state;
+    state_t right_state;
+
+    for (;;)
+    {
+        // Receive from Buffer
+        xMessageBufferReceive(left_sensor_msg_buffer,
+                              &left_state,
+                              sizeof(state_t),
+                              portMAX_DELAY);
+
+        xMessageBufferReceive(right_sensor_msg_buffer,
+                              &right_state,
+                              sizeof(state_t),
+                              portMAX_DELAY);
+
+        g_car_state.current_direction = (left_state << 1) | right_state;
+
+        switch (g_car_state.current_direction)
+        {
+            case FORWARD:
+                break;
+            case RIGHT:
+                g_car_state.orientation = (g_car_state.orientation + 1) & 0x03;
+                break;
+            case LEFT:
+                g_car_state.orientation = (g_car_state.orientation - 1) & 0x03;
+                break;
+            default:
+                break;
+        }
+
+        switch (g_car_state.current_direction)
+        {
+            case FORWARD:
+                printf("Direction: Forward\n");
+                break;
+            case RIGHT:
+                printf("Direction: Right\n");
+                break;
+            case LEFT:
+                printf("Direction: Left\n");
+                break;
+            default:
+                printf("Direction: Error\n");
+                break;
+        }
+
+        switch (g_car_state.orientation)
+        {
+            case NORTH:
+                printf("Orientation: North\n");
+                break;
+            case EAST:
+                printf("Orientation: East\n");
+                break;
+            case SOUTH:
+                printf("Orientation: South\n");
+                break;
+            case WEST:
+                printf("Orientation: West\n");
+                break;
+            default:
+                printf("Orientation: Error\n");
+                break;
+        }
+    }
+}
+
+/**
+ * @brief Timer Interrupt Handler for the left sensor
+ * @param rt
+ * @return True (To keep the timer running)
+ */
+bool h_left_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
+
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    xSemaphoreGiveFromISR(g_left_sensor_sem,
+                          &xHigherPriorityTaskWoken);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    return true;
+}
+
+/**
+ * @brief Timer Interrupt Handler for the right sensor
+ *
+ * @param repeatingTimer
+ * @return True (To keep the timer running)
+ */
+bool h_right_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
+
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    xSemaphoreGiveFromISR(g_right_sensor_sem,
+                          &xHigherPriorityTaskWoken);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+
+    return true;
+}
+
+/**
+* Main Launch body
+ * This is to be in the main launch function for FreeRTOS
+ *
+ * // Repeating timer
+ *   struct repeating_timer g_left_sensor_timer;
+ *   add_repeating_timer_ms(100,
+ *                          h_left_sensor_timer_handler,
+ *                          NULL,
+ *                          &g_left_sensor_timer);
+
+ *   struct repeating_timer g_right_sensor_timer;
+ *   add_repeating_timer_ms(100,
+ *                          h_right_sensor_timer_handler,
+ *                          NULL,
+ *                          &g_right_sensor_timer);
+
+
+ *   TaskHandle_t h_monitor_left_sensor_task;
+ *   xTaskCreate(monitor_left_sensor_task,
+ *               "Monitor Left Sensor Task",
+ *               configMINIMAL_STACK_SIZE,
+ *               NULL,
+ *               LEFT_TASK_PRIORITY,
+ *               &h_monitor_left_sensor_task);
+
+ *   TaskHandle_t h_monitor_right_sensor_task;
+ *   xTaskCreate(monitor_right_sensor_task,
+ *               "Monitor Right Sensor Task",
+ *               configMINIMAL_STACK_SIZE,
+ *               NULL,
+ *               RIGHT_TASK_PRIORITY,
+ *               &h_monitor_right_sensor_task);
+
+ *   TaskHandle_t h_monitor_direction_task;
+ *   xTaskCreate(monitor_direction_task,
+ *               "Monitor Direction Task",
+ *               configMINIMAL_STACK_SIZE,
+ *               NULL,
+ *               DIRECTION_TASK_PRIORITY,
+ *               &h_monitor_direction_task);
+*/
\ No newline at end of file
diff --git a/frtos/line_sensor/lwipopts.h b/frtos/line_sensor/lwipopts.h
new file mode 100644
index 0000000..051c08a
--- /dev/null
+++ b/frtos/line_sensor/lwipopts.h
@@ -0,0 +1,25 @@
+#ifndef _LWIPOPTS_H
+#define _LWIPOPTS_H
+
+// Generally you would define your own explicit list of lwIP options
+// (see https://www.nongnu.org/lwip/2_1_x/group__lwip__opts.html)
+//
+// This example uses a common include to avoid repetition
+#include "lwipopts_examples_common.h"
+
+#if !NO_SYS
+#define TCPIP_THREAD_STACKSIZE 1024
+#define DEFAULT_THREAD_STACKSIZE 1024
+#define DEFAULT_RAW_RECVMBOX_SIZE 8
+#define TCPIP_MBOX_SIZE 8
+#define LWIP_TIMEVAL_PRIVATE 0
+
+// not necessary, can be done either way
+#define LWIP_TCPIP_CORE_LOCKING_INPUT 1
+
+// ping_thread sets socket receive timeout, so enable this feature
+#define LWIP_SO_RCVTIMEO 1
+#endif
+
+
+#endif