diff --git a/TaskScheduler/PriorityQueue.c b/TaskScheduler/PriorityQueue.c
new file mode 100644
index 0000000..50930b3
--- /dev/null
+++ b/TaskScheduler/PriorityQueue.c
@@ -0,0 +1,105 @@
+#include "PriorityQueue.h"
+
+#ifndef NULL
+    #define NULL 0
+#endif
+
+//forward declarations
+void Heapify(PriorityQueue *queue, int index);
+void SwapItems(PriorityQueue *queue, int indexA, int indexB);
+int ParentIndex(int index);
+
+void InitializeQueue(PriorityQueue *queue){
+    uint8_t i = 0;
+    queue->nTasks = 0;
+    for(i = 0; i < MAX_TASKS; i++){
+        queue->pendingTasks[i] = NULL;
+    }
+}
+
+void AddTaskToQueue(PriorityQueue *queue, Task* task){
+    if(!IsQueueFull(queue)){
+        uint8_t i, parent;
+
+        //Add task to queue
+        queue->pendingTasks[queue->nTasks] = task;
+        queue->nTasks++;
+
+        //Fix min heap property of queue
+        i = queue->nTasks - 1;
+        parent = ParentIndex(i);
+        while(i != 0 && queue->pendingTasks[parent]->priority > queue->pendingTasks[i]->priority){
+            SwapItems(queue, i, parent);
+            i = parent;
+            parent = ParentIndex(i);
+        }
+    }
+    else{
+        //Do nothing, queue is full
+    }
+}
+
+void RunNextTask(PriorityQueue *queue){
+    if(queue->nTasks > 0){
+        queue->pendingTasks[0]->pCallback();
+
+        //Remove the task
+        SwapItems(queue, 0, queue->nTasks - 1);
+        queue->pendingTasks[queue->nTasks - 1] = NULL;
+        queue->nTasks--;
+
+        //Reorder the queue
+        Heapify(queue, 0);
+    }
+    else{
+        //No tasks to run
+    }
+}
+
+void RunAllTasks(PriorityQueue *queue){
+    while(queue->nTasks > 0){
+        RunNextTask(queue);
+    }
+}
+
+bool IsQueueFull(PriorityQueue *queue){
+    if(queue->nTasks >= MAX_TASKS){
+        return true;
+    }
+    else{
+        return false;
+    }
+}
+
+//Fixes the min heap when extracting a node
+void Heapify(PriorityQueue *queue, int index){
+    uint8_t left = 2*index + 1;
+    uint8_t right = 2*index + 2;
+    uint8_t smallest = index;
+
+    //If the left task has lower priority, set it as the lowest
+    if(left < queue->nTasks && queue->pendingTasks[left]->priority < queue->pendingTasks[smallest]->priority){
+        smallest = left;
+    }
+
+    //If the right task has lower priority, set it as the lowest
+    if(right < queue->nTasks && queue->pendingTasks[right]->priority < queue->pendingTasks[smallest]->priority){
+        smallest = right;
+    }
+
+    //If the passed index was not the lowest, swap the two and repeat
+    if(smallest != index){
+        SwapItems(queue, smallest, index);
+        Heapify(queue, smallest);
+    }
+}
+
+void SwapItems(PriorityQueue *queue, int indexA, int indexB){
+    Task* tempTaskPointer = queue->pendingTasks[indexB];
+    queue->pendingTasks[indexB] = queue->pendingTasks[indexA];
+    queue->pendingTasks[indexA] = tempTaskPointer;
+}
+
+int ParentIndex(int index){
+    return (index - 1) / 2;
+}
diff --git a/TaskScheduler/PriorityQueue.h b/TaskScheduler/PriorityQueue.h
new file mode 100644
index 0000000..cd997a1
--- /dev/null
+++ b/TaskScheduler/PriorityQueue.h
@@ -0,0 +1,26 @@
+#ifndef TIVA_TASK_SCHEDULER_PRIORITY_QUEUE_H
+#define TIVA_TASK_SCHEDULER_PRIORITY_QUEUE_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "TaskScheduler.h"
+
+#define MAX_TASKS   15   //Must be an order of 2 - 1 (i.e 1, 3, 7, 15, 31)
+
+
+struct PriorityQueue_tag;
+typedef struct PriorityQueue_tag PriorityQueue;
+
+struct PriorityQueue_tag{
+    int nTasks;
+    Task* pendingTasks[MAX_TASKS];
+};
+
+void InitializeQueue(PriorityQueue *queue);
+void AddTaskToQueue(PriorityQueue *queue, Task* task);
+void RunNextTask(PriorityQueue *queue);
+void RunAllTasks(PriorityQueue *queue);
+bool IsQueueFull(PriorityQueue *queue);
+
+#endif
diff --git a/TaskScheduler/TaskScheduler.c b/TaskScheduler/TaskScheduler.c
index 4ced95f..9698296 100644
--- a/TaskScheduler/TaskScheduler.c
+++ b/TaskScheduler/TaskScheduler.c
@@ -1,5 +1,6 @@
 //Project includes
 #include "TaskScheduler.h"
+#include "PriorityQueue.h"
 
 //Tivaware includes
 #include <driverlib/sysctl.h>
@@ -13,7 +14,11 @@
     #define NULL 0
 #endif
 
-static volatile TaskScheduler scheduler;
+static volatile TaskScheduler   scheduler;
+
+//Does not need to be volatile because it is only used in the ISR
+static PriorityQueue   queue;
+static PriorityQueue*           pqueue;
 
 /*
  * Timer ISR for the task scheduler
@@ -34,19 +39,21 @@ void TaskSchedulerTimer_ISR(void){
             if(pTask->ticks >= pTask->maxTicks){
                 pTask->ticks = 0;
 
-                //Add task to priority queue
-                //TODO: Implement priority queue
-                pTask->pCallback();
+                AddTaskToQueue(pqueue, pTask);
             }
         }
         else{
-            //Do nothing
+            //Do nothing, but reset the ticks
+            pTask->ticks = 0;
         }
 
         //Move to next task
         pTask = pTask->pNextTask;
     }
 
+    //Run all pending tasks
+    RunAllTasks(pqueue);
+
     TimerIntClear(scheduler.timerBase, TIMER_TIMA_TIMEOUT);
 }
 
@@ -55,6 +62,10 @@ void InitializeTaskScheduler(uint32_t timerBase, uint32_t sysCtlTimerPeriph, uin
     scheduler.timerBase = timerBase;
     scheduler.pTaskListRoot = NULL;
 
+    //Initialize the priority queue
+    pqueue = &queue;
+    InitializeQueue(pqueue);
+
     //Initialize the timer
     SysCtlPeripheralEnable(sysCtlTimerPeriph);
 
diff --git a/main.c b/main.c
index 31afc71..88fad01 100644
--- a/main.c
+++ b/main.c
@@ -1,5 +1,6 @@
 //Project includes
 #include "TaskScheduler/TaskScheduler.h"
+#include "TaskScheduler/PriorityQueue.h"
 
 //Standard includes
 #include <stdbool.h>
@@ -63,17 +64,20 @@ int main(void)
 
     InitializeTaskScheduler(TIMER0_BASE, SYSCTL_PERIPH_TIMER0, SYS_CLK, INT_TIMER0A);
 
-    tasks[0].period = 3;
+    tasks[0].period = 5;
     tasks[0].enabled = 1;
     tasks[0].pCallback = print;
+    tasks[0].priority = 2;
 
-    tasks[1].period = 1.5;
+    tasks[1].period = 30;
     tasks[1].enabled = 1;
     tasks[1].pCallback = print2;
+    tasks[1].priority = 1;
 
     tasks[2].period = 1;
     tasks[2].enabled = 1;
     tasks[2].pCallback = print3;
+    tasks[2].priority = 0;
 
     AddTask(&tasks[0]);
     AddTask(&tasks[1]);