diff --git a/CONTRIBUTING b/CONTRIBUTING
index 1fa2a8f..2912965 100644
--- a/CONTRIBUTING
+++ b/CONTRIBUTING
@@ -26,3 +26,26 @@ Bug fixes
 - Urgent bug fixes may be done from ‘master’ while development has continued on ‘develop’
 - In this case, check out a bug fix branch from ‘master’
 - Merge the fix back into ‘master’ AND ‘develop’
+
+
+Plotting
+
+- if you #define src/util/debug.h:DEBUG_PLOTTING to "control"
+  (real) or "sensors" (simulated values), the controller will print
+  sampled data to the debug port in the following format::
+
+    ms,FIELDA,FIELDB,...\r
+    MS1,VALA1,VALB1,...\r
+    MS2,VALA2,VALB2,...\r
+    MS3,VALA3,VALB3,...\r
+
+  Where the first line are the header names with ms being timestamps
+  in milliseconds.
+
+  This can be visualized with the arduino plotter or with tools such
+  as kst2 (eg: in Linux -- note you might have to edit the beggining
+  of the file to remove any leftover output before flashing until the
+  headers so kst2 can parse it correctly)::
+
+    $ cat /dev/ttyACM0 > file.txt
+    $ kst2 file.txt
diff --git a/src/hal/motor.cpp b/src/hal/motor.cpp
index 0ffff0f..94177a5 100644
--- a/src/hal/motor.cpp
+++ b/src/hal/motor.cpp
@@ -1,9 +1,7 @@
-
 #include "../hal/hal.h"
 #include "../hal/motor.h"
 
 #include <assert.h>
-#include <stdint.h>
 #include <util/atomic.h>
 
 #include "Arduino.h"
@@ -11,7 +9,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
-// #define DEBUG
+#define DEBUG
 #define DEBUG_MODULE "motor"
 #include "../util/debug.h"
 
@@ -24,6 +22,9 @@
 #define PIN_MOTOR_ENABLE    8 // Enable locks the motor in place when it is not moving, which consumes power.
 #define PIN_MOTOR_DIRECTION 9
 
+#define PIN_MOTOR_ENABLE_FALSE LOW
+#define PIN_MOTOR_ENABLE_TRUE  HIGH
+
 #define PIN_MOTOR_DIRECTION_OPEN  LOW
 #define PIN_MOTOR_DIRECTION_CLOSE HIGH
 
@@ -40,6 +41,8 @@
 // Motor Definitions
 //**************************************
 
+#define MOTOR_STEPS_PER_REVOLUTION 200
+
 // millidegrees of shaft rotation (including the gearbox) per motor step
 
 // stepperonline 23HS22-2804S-HG50
@@ -74,42 +77,55 @@
 #define MC_MICROSTEP_RESOLUTION 400
 #define MC_MICROSTEPS_PER_STEP (MC_MICROSTEP_RESOLUTION/100)
 
+// stepperonline DM332T
+// Pulse/rev
+// The DM332T supports the following settings:
+//  400 (half steps)
+//  800 (quarter steps)
+// 1600 (eigth steps)
+// 3200 (sixteenth steps)
+// 4000
+// 6400
+// 8000
+// 12800
+// #define MC_MICROSTEPS_PER_REVOLUTION 800
+// #define MC_MICROSTEPS_PER_STEP (MC_MICROSTEPS_PER_REVOLUTION/MOTOR_STEPS_PER_REVOLUTION)
+
 //**************************************
-// Motor Control Definitions
+// Motor State Machine Definitions
 //**************************************
 #define MOTOR_STATE_OFF   0
 #define MOTOR_STATE_HOLD  1
 #define MOTOR_STATE_OPEN  2
 #define MOTOR_STATE_CLOSE 3
+#define MOTOR_STATE_NONE  UINT8_MAX
 
-#define MOTOR_DIRECTION_OPEN   -1
-#define MOTOR_DIRECTION_CLOSE   1
+//**************************************
+// Motor Control Definitions
+//**************************************
 
-// TODO
-// This value should be derived from Timer 3 and Timer 4 frequencies.
+// TODO: Update this based on maximum PWM frequency and control loop frequency.
 #define MOTOR_CONTROL_STEP_ERROR 5
 
 //**************************************
 // Motor Control Variables
 //**************************************
-
-// Staging area for motor move commands.
-static struct {
-  bool    valid = false;
-  uint8_t direction = 0;
-  uint8_t distance = 0;
-} motor_move_command;
-
 static volatile struct {
-  int16_t step_command = 0;
-  int16_t step_position = 0;
-  int8_t  microstep_position = 0;
+  // Precomputed value so that we can minimize the amount of math operations
+  // inside of an ISR.
+  bool     counter_update;
+  uint16_t counter_TOP;
+
+  int16_t  step_position_command;
+  int16_t  step_position;
+  int8_t   microstep_position;
 } motor_control;
 
-// Motor State Machine
+//**************************************
+// Motor State Machine Variables
+//**************************************
 static volatile uint8_t motor_state = MOTOR_STATE_OFF;
-
-static volatile bool motor_busy = false; 
+static volatile uint8_t motor_state_pending = MOTOR_STATE_NONE;
 
 //*****************************************************************************
 // Timer 3 Configuration: Motor State Control
@@ -124,7 +140,7 @@ void timer3_setup() {
   TCNT3 = 0;
   // OCR3A = 20000; // 50 Hz (1,000,000 / 20,000 = 50)
   OCR3A = 10000; // 100 Hz (1,000,000 / 10,000 = 100)
-  TIMSK3 = (1<<OCIE3A);
+  TIMSK3 = (1<<TOIE3);
   TCCR3B |= (0<<CS32) | (1<<CS31) | (0<<CS30);
 }
 
@@ -138,15 +154,15 @@ void timer3_setup() {
 // motor position.
 //
 // WGM4[3:0] = 0b1001 (PWM, Phase and Frequency Correct Mode, TOP: OCR4A)
-// COM4B[1:0] = 0b10 (Enable PWM output on OC4B -> ATMega 2560 PH4 -> Arduino D7)
+// COM4B[1:0] = 0b10 (Enable non-inverting PWM output on OC4B -> ATMega 2560 PH4 -> Arduino D7)
 // CS3[2:0] = 0b010 (clk/8 prescaler)
 //
 // Create values for the control registers such that the timer can be enabled
 // and disabled dynamically.
 //*****************************************************************************
 static uint8_t TCCR4A_value = (1<<COM4B1) | (0<<COM4B0) | (0<<WGM41) | (1<<WGM40);
-static uint8_t TCCR4B_value_disabled = (1<<WGM43)  | (0<<WGM42);
-static uint8_t TCCR4B_value_enabled = TCCR4B_value_disabled | (0<<CS42)   | (1<<CS41)  | (0<<CS40);
+static uint8_t TCCR4B_value_disabled = (1<<WGM43) | (0<<WGM42);
+static uint8_t TCCR4B_value_enabled = TCCR4B_value_disabled | (0<<CS42) | (1<<CS41) | (0<<CS40);
 
 void timer4_setup()
 {
@@ -154,84 +170,198 @@ void timer4_setup()
   TCCR4B = TCCR4B_value_disabled;
   TCNT4 = 0;
   
-  // TODO
+  // TODO: Set defaults.
   OCR4A = 625;
   OCR4B = OCR4A/2; // 50% duty cycle
 
-  TIMSK4 = (1<<OCIE4A);
+  TIMSK4 = (1<<OCIE4A); // Enable interrupt at TOP.
 }
 
-void timer4_enable() {
+static inline void timer4_enable() {
   TCCR4B = TCCR4B_value_enabled;
 }
 
-void timer4_disable() {
+static inline void timer4_disable() {
   TCCR4B = TCCR4B_value_disabled;
+  TCNT4  = 0; // TODO: Does TCNT need to be written to zero after OCR4A/OCR4B get written in order to latch in their values?
 }
 
+static inline void timer4_interrupt_BOTTOM_enable()
+{
+  TIFR4 = _BV(TOV4); // Clear the Timer 4 Overflow Flag.
+  TIMSK4 |= _BV(TOIE4); // Set the Timer 4 Overflow Interrupt Enable.
+}
+
+static inline void timer4_interrupt_BOTTOM_disable()
+{
+  TIMSK4 &= ~_BV(TOIE4); // Clear the Timer 4 Overflow Interrupt Enable.
+}
+
+static inline void timer4_set_TOP(uint16_t counter_value)
+{
+  OCR4A = counter_value;
+  OCR4B = counter_value/2; // 50% duty cycle
+}
 
 //*****************************************************************************
-// Motor State Control
+// Motor PWM Control
 //*****************************************************************************
 
-void motor_pwm_enable()
+static inline void motor_pwm_enable()
 {
+  timer4_set_TOP(motor_control.counter_TOP);
+  motor_control.counter_update = false;
   timer4_enable();
 }
 
-void motor_pwm_disable()
+static inline void motor_pwm_disable()
 {
   timer4_disable();
 }
 
-// Manages the motor state machine.
-void set_motor_state(uint8_t next_state)
+static inline void motor_pwm_phase_interrupt_enable()
+{
+  timer4_interrupt_BOTTOM_enable();
+}
+
+static inline void motor_pwm_phase_interrupt_disable()
+{
+  timer4_interrupt_BOTTOM_disable();
+}
+
+//*****************************************************************************
+// Motor State Machine
+//*****************************************************************************
+
+// Sets the motor state machine to the MOTOR_STATE_OFF state, and configures
+// its outputs accordingly.
+static void motor_state_set_OFF()
+{
+  DEBUG_PRINT("MOTOR_STATE_OFF");
+  motor_pwm_disable();
+  digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_FALSE);
+
+  motor_state = MOTOR_STATE_OFF;
+}
+
+// Sets the motor state machine to the MOTOR_STATE_HOLD state, and configures
+// its outputs accordingly.
+static void motor_state_set_HOLD()
+{
+  DEBUG_PRINT("MOTOR_STATE_HOLD");
+  motor_pwm_disable();
+  digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_TRUE);
+
+  motor_state = MOTOR_STATE_HOLD;
+}
+
+// Sets the motor state machine to the MOTOR_STATE_OPEN state, and configures
+// its outputs accordingly.
+static inline void motor_state_set_OPEN()
+{
+  DEBUG_PRINT("MOTOR_STATE_OPEN");
+  digitalWrite(PIN_MOTOR_DIRECTION, PIN_MOTOR_DIRECTION_OPEN);
+  digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_TRUE);
+  motor_pwm_enable();
+  
+  motor_state = MOTOR_STATE_OPEN;
+}
+
+// Sets the motor state machine to the MOTOR_STATE_CLOSE state, and configures
+// its outputs accordingly.
+static inline void motor_state_set_CLOSE()
+{
+  DEBUG_PRINT("MOTOR_STATE_CLOSE");
+  digitalWrite(PIN_MOTOR_DIRECTION, PIN_MOTOR_DIRECTION_CLOSE);
+  digitalWrite(PIN_MOTOR_ENABLE, PIN_MOTOR_ENABLE_TRUE);
+  motor_pwm_enable();
+  
+  motor_state = MOTOR_STATE_CLOSE;
+}
+
+// Registers a provided motor state as pending to be installed in the next
+// available phase window of the PWM signal.
+static void motor_state_register_pending(uint8_t next_state)
+{
+  motor_state_pending = next_state;
+  motor_pwm_phase_interrupt_enable();
+}
+
+// Installs a pending motor state update. It is expected that this function
+// will only be called within the correct phase window of the PWM signal.
+static void motor_state_install_pending()
+{
+  if (motor_state_pending == MOTOR_STATE_OFF) {
+    motor_state_set_OFF();
+  } else if (motor_state_pending == MOTOR_STATE_HOLD) {
+    motor_state_set_HOLD();
+  } else {
+    // TODO: error
+  }
+
+  motor_pwm_phase_interrupt_disable();
+  motor_state_pending = MOTOR_STATE_NONE;
+}
+
+// Determines whether the current motor state is a moving state.
+static bool motor_state_moving()
+{
+  return ((motor_state == MOTOR_STATE_OPEN) ||
+          (motor_state == MOTOR_STATE_CLOSE));
+}
+
+// Manages motor state machine transitions.
+//
+// A transition from a non-moving state to any other state is made immediately
+// via motor_state_set().
+//
+// A transition from a moving state to a non-moving state is registered as
+// pending to be installed in the next available phase window of the PWM signal
+// via motor_state_register_pending().
+//
+// A transition from a moving state directly to another moving state is treated
+// as an error.
+void motor_state_transition(uint8_t next_state)
 {
   if (motor_state == next_state) {
     return;
   }
+ 
+  if (motor_state_pending != MOTOR_STATE_NONE) {
+    if (next_state != motor_state_pending) {
+      // TODO: error
+    } else {
+      return;
+    }
+  }
 
   if (next_state == MOTOR_STATE_OFF) {
-    motor_pwm_disable();
-    digitalWrite(PIN_MOTOR_ENABLE, LOW);
-    motor_busy = false;
+    if (motor_state_moving()) {
+      motor_state_register_pending(MOTOR_STATE_OFF);
+    } else {
+      motor_state_set_OFF();
+    }
   } else if (next_state == MOTOR_STATE_HOLD) {
-    motor_pwm_disable();
-    digitalWrite(PIN_MOTOR_ENABLE, HIGH);
-    motor_busy = false;
+    if (motor_state_moving()) {
+      motor_state_register_pending(MOTOR_STATE_HOLD);
+    } else {
+      motor_state_set_HOLD();
+    }
   } else if (next_state == MOTOR_STATE_OPEN) {
-    // TODO
-    /*
     if (motor_state == MOTOR_STATE_CLOSE) {
-      // Should going directly from the CLOSE state to the OPEN state with no
-      // intermediate state transition constitute an error?
-      //
-      // Not sure if restoring interrupt state is necessary here.
-      assert(motor_state != MOTOR_STATE_CLOSE);
+      // assert(motor_state != MOTOR_STATE_CLOSE);
+      // TODO: error
     }
-    */
-
-    digitalWrite(PIN_MOTOR_DIRECTION, PIN_MOTOR_DIRECTION_OPEN);
-    digitalWrite(PIN_MOTOR_ENABLE, HIGH);
-    motor_pwm_enable();
+    motor_state_set_OPEN();
   } else if (next_state == MOTOR_STATE_CLOSE) {
-    // TODO
-    /*
     if (motor_state == MOTOR_STATE_OPEN) {
-      // Should going directly from the OPEN state to the CLOSE state with no
-      // intermediate state transition be an error?
-
-      // Not sure if restoring interrupt state is necessary here.
-      assert(motor_state != MOTOR_STATE_OPEN);
+      // assert(motor_state != MOTOR_STATE_OPEN);
+      // TODO: error
     }
-    */
-
-    digitalWrite(PIN_MOTOR_DIRECTION, PIN_MOTOR_DIRECTION_CLOSE);
-    digitalWrite(PIN_MOTOR_ENABLE, HIGH);
-    motor_pwm_enable();
+    motor_state_set_CLOSE();
+  } else {
+    // TODO: error
   }
-
-  motor_state = next_state;
 }
 
 //*****************************************************************************
@@ -239,34 +369,23 @@ void set_motor_state(uint8_t next_state)
 //*****************************************************************************
 
 // Set the current position as the zero point.
-void motor_position_zero()
+void motor_position_set_zero()
 {
   assert((motor_state != MOTOR_STATE_OPEN) && (motor_state != MOTOR_STATE_CLOSE));
 
-  motor_control.microstep_position = 0;
+  motor_control.step_position_command = 0;
   motor_control.step_position = 0;
-  motor_control.step_command = 0;
+  motor_control.microstep_position = 0;
 }
 
-// Moves the motor to an angle relative to its current position.
+// Sets the motor command positon.
 //
-// Parameters
-// ----------
-// uint8_t angle      The angle (in degrees) to move the motor.
-// int8_t  direction  The direction to move the motor. One of:
-//                      * MOTOR_DIRECTION_OPEN
-//                      * MOTOR_DIRECTION_CLOSE
-void motor_move(uint8_t angle, int8_t direction)
+// Parameter
+// ---------
+// uint8_t position   The absolute position to which to move the motor.
+//                    units: degrees
+void motor_position_set(int8_t position)
 {
-  // TODO: Do we want to have assertions? How are they implemented in this
-  // system? (i.e. Do they cause a watchdog reset, print a debug message to the
-  // console, etc.)
-  assert((direction == MOTOR_DIRECTION_OPEN) ||
-         (direction == MOTOR_DIRECTION_CLOSE));
-
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-    motor_busy = true;
-    
 
     // Deriving motor_control.step_command from motor_control.step_position can
     // cause error to accumulate from movement to movement. If
@@ -275,8 +394,29 @@ void motor_move(uint8_t angle, int8_t direction)
     // motor_control.step_command = motor_control.step_position + direction*((angle*1000)/MOTOR_STEP_ANGLE);
 
     // Convert to millidegrees and account for direction.
-    motor_control.step_command += (int16_t) (direction*((((int32_t) angle)*1000)/MOTOR_STEP_ANGLE));
-  }
+    motor_control.step_position_command = (int16_t) ((((int32_t) position)*1000)/MOTOR_STEP_ANGLE);
+
+}
+
+//*****************************************************************************
+// Motor Speed Control
+//*****************************************************************************
+
+// Sets the motor command speed.
+//
+// Parameter
+// ---------
+// uint8_t  speed   The speed at which to move the motor.
+//                  units: MPRM (milli-RPM)
+void motor_speed_set(uint16_t speed)
+{
+  // Convert from MRPM to frequency.
+  uint32_t frequency = (((((uint32_t) speed)*360U)/60U)/((uint8_t) MOTOR_STEP_ANGLE))*((uint8_t) MC_MICROSTEPS_PER_STEP);
+
+  // Convert from frequency to counter value.
+  motor_control.counter_TOP = (uint16_t) (1000000UL/frequency);
+  
+  motor_control.counter_update = true;
 }
 
 // Moves the motor to the home position (fully open).
@@ -285,53 +425,43 @@ void motor_home()
   // Move the motor far enough in the OPEN direction (more than the total
   // possible excursion) in order to guarantee that the top limit switch will
   // be tripped.
-  motor_move(180, MOTOR_DIRECTION_OPEN);
+  motor_position_set(-90);
+  motor_speed_set(5000UL);
 }
 
-// Moves the motor to the away position (fully closed).
-void motor_away() {
-  // Move the motor far enough in the CLOSE direction (more than the total
-  // possible excursion) in order to guarantee that the bottom limit switch
-  // will be tripped.
-  motor_move(180, MOTOR_DIRECTION_CLOSE);
-}
-
-//*****************************************************************************
-// Interrupt Service Routines
-//*****************************************************************************
-
-// Motor State Control ISR
-static inline void motor_state_control_ISR()
+static void motor_state_update()
 {
-  int32_t motor_step_delta = motor_control.step_command - motor_control.step_position;
+  int32_t motor_step_delta = motor_control.step_position_command - motor_control.step_position;
 
   if (motor_step_delta > MOTOR_CONTROL_STEP_ERROR) {
     if (digitalRead(PIN_LIMIT_BOTTOM) == PIN_LIMIT_TRIPPED) {
-      // Do not move the motor in the CLOSE direction if the bottom limit switch is tripped.
-      set_motor_state(MOTOR_STATE_HOLD);
+      // Do not move the motor in the CLOSE direction if the bottom limit
+      // switch is tripped.
+      motor_state_transition(MOTOR_STATE_HOLD);
     } else {
-      set_motor_state(MOTOR_STATE_CLOSE);
+      motor_state_transition(MOTOR_STATE_CLOSE);
     }
   } else if (motor_step_delta < -MOTOR_CONTROL_STEP_ERROR) {
     if (digitalRead(PIN_LIMIT_TOP) == PIN_LIMIT_TRIPPED) {
-      // Do not move the motor in the OPEN direction if the top limit switch is tripped.
-      set_motor_state(MOTOR_STATE_HOLD);
-      motor_position_zero();
+      // Do not move the motor in the OPEN direction if the top limit switch is
+      // tripped. There is no need to go through motor_state_transition()
+      // because the zero reference point is set here.
+      motor_state_set_HOLD();
+      motor_position_set_zero();
     } else {
-      set_motor_state(MOTOR_STATE_OPEN);
+      motor_state_transition(MOTOR_STATE_OPEN);
     }
   } else {
     if ((motor_state == MOTOR_STATE_OPEN) ||
         (motor_state == MOTOR_STATE_CLOSE)) {
       // Hold the motor in place in order to maintain the correct position
       // without having to zero the motor again.
-      set_motor_state(MOTOR_STATE_HOLD);
+      motor_state_transition(MOTOR_STATE_HOLD);
     }
   }
 }
 
-// Motor Position Tracking ISR
-static inline void motor_position_tracking_ISR()
+static inline void motor_position_update()
 {
   if (motor_state == MOTOR_STATE_OPEN) {
     if (--motor_control.microstep_position <= -MC_MICROSTEPS_PER_STEP) {
@@ -343,26 +473,49 @@ static inline void motor_position_tracking_ISR()
       motor_control.step_position++;
       motor_control.microstep_position = 0;
     }
+  } else {
+    static uint16_t error_count = 0;
+    DEBUG_PRINT("motor_position_update ERROR %d", ++error_count);
+    // TODO: error
   }
 }
 
-ISR(TIMER3_COMPA_vect)
+//*****************************************************************************
+// Interrupt Service Routines
+//*****************************************************************************
+
+// Timer 3 BOTTOM
+ISR(TIMER3_OVF_vect)
 {
-  motor_state_control_ISR();
+  // motor_state_update();
+}
+
+// Timer 4 BOTTOM
+ISR(TIMER4_OVF_vect) {
+  motor_state_install_pending();
 }
 
+// Timer 4 TOP
 ISR(TIMER4_COMPA_vect)
 {
-  motor_position_tracking_ISR();
+  if (motor_control.counter_update) {
+    timer4_set_TOP(motor_control.counter_TOP);
+    motor_control.counter_update = false;
+  }
+
+  motor_position_update();
 }
 
+
 //*****************************************************************************
 // motorHal Interface Implementation
 //*****************************************************************************
-int motorHalInit(void)
+int8_t motorHalInit(void)
 {
   ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
-    timer3_setup();
+    // TODO: Remove/relocate Timer 3
+    // timer3_setup();
+    
     timer4_setup();
 
     // Motor Pin Configuration
@@ -379,47 +532,42 @@ int motorHalInit(void)
   // avoided during WDT reset.
 
   // Move the motor to the home position to zero the motor position.
-  motor_home();
-  while (motor_busy) {}
+  motorHalCommand(INT8_MIN, 5000U);
+  while(motorHalStatus() == HAL_IN_PROGRESS) {}
   delay(1000);
 
   // Move to the top of the bag.
   // TODO: This value will vary depending upon the installation location of the
   // top limit switch, as well as the type of bag. Ultimately, the sensors
   // should be used to find the top of the bag.
-  motor_move(15, MOTOR_DIRECTION_CLOSE);
-  while (motor_busy) {} 
+  motorHalCommand(15, 5000U);
+  while (motorHalStatus() == HAL_IN_PROGRESS) {}
   delay(1000);
 
   return HAL_OK;
 }
 
-// TODO: Implement duration.
-int motorHalBegin(unsigned int direction, unsigned int distance, unsigned int duration)
+// Absolute position, relative to the motor zero point.
+// Speed given in MRPM (millirevolutions per second).
+int8_t motorHalCommand(uint8_t position, uint16_t speed)
 {
-  motor_move_command.distance = distance;
+  motor_position_set(position);
+  motor_speed_set(speed);
+  
+  motor_state_update();
 
-  if (direction == MOTOR_HAL_DIRECTION_INHALATION) {
-    motor_move_command.direction = MOTOR_DIRECTION_CLOSE;
-  } else if (direction == MOTOR_HAL_DIRECTION_EXHALATION) {
-    motor_move_command.direction = MOTOR_DIRECTION_OPEN;
+  if (motor_state_moving()) {
+    return HAL_IN_PROGRESS;
   } else {
-    return HAL_FAIL;
+    return HAL_OK;
   }
-
-  motor_move_command.valid = true;
-  
-  return HAL_OK;
 }
 
-int motorHalRun(void)
+int8_t motorHalStatus(void)
 {
-  if (motor_move_command.valid) {
-    motor_move(motor_move_command.distance, motor_move_command.direction);
-    motor_move_command.valid = false;
-  }
+  motor_state_update();
 
-  if (motor_busy) {
+  if (motor_state_moving()) {
     return HAL_IN_PROGRESS;
   } else {
     return HAL_OK;
diff --git a/src/hal/motor.h b/src/hal/motor.h
index 11eea3a..85cdcfc 100644
--- a/src/hal/motor.h
+++ b/src/hal/motor.h
@@ -1,19 +1,18 @@
-
 #ifndef __MOTOR_HAL_H__
 #define __MOTOR_HAL_H__
 
 #include "../hal/hal.h"
 
-#define MOTOR_HAL_DIRECTION_INHALATION 0x55
-#define MOTOR_HAL_DIRECTION_EXHALATION 0xAF
+#include <stdint.h>
 
 // TODO: Doc
-int motorHalInit(void);
+int8_t motorHalInit(void);
 
 // TODO: Doc
-int motorHalBegin(unsigned int direction, unsigned int distance, unsigned int duration);
+int8_t motorHalCommand(uint8_t position, uint16_t speed);
 
 // TODO: Doc
-int motorHalRun(void);
+int8_t motorHalStatus(void);
+
+#endif /* __MOTOR_HAL_H__ */
 
-#endif /* __MOTOR_HAL_H__ */
\ No newline at end of file
diff --git a/src/hal/sensor/adc.h b/src/hal/sensor/adc.h
new file mode 100644
index 0000000..24a8584
--- /dev/null
+++ b/src/hal/sensor/adc.h
@@ -0,0 +1,49 @@
+
+#ifndef __ADC_SENSOR_HAL_H__
+#define __ADC_SENSOR_HAL_H__
+
+#include <avr/io.h>
+
+#include <Arduino.h>
+
+// As multiple sensors use the ADC, add several macros to
+// define common functions for the ADC with writing a separate
+// driver for it
+
+// Check if ADC is enabled/initialized
+#define adcHalEnabled() (bit_is_set(ADCSRA, ADEN))
+
+// Check if ADC is currently doing a conversion or has data ready that hasnt been claimed
+#define adcHalBusy() (bit_is_set(ADCSRA, ADSC) || bit_is_set(ADCSRA, ADIF))
+
+// Check if ADC is currently doing a conversion, so data can be retreived once complete
+#define adcHalInProgress() (bit_is_set(ADCSRA, ADSC))
+
+// Initialized ADC to use AVcc Reference
+#define adcHalInit()                                                \
+  do {                                                              \
+    ADMUX = bit(REFS0);                                             \
+    ADCSRA = bit(ADEN) | bit(ADPS0) |  bit(ADPS1) | bit(ADPS2);     \
+  } while (0);
+  
+// Begin an ADC conversion; first set the correct mux bits, then set the start bit
+#define adcHalBegin(pin)                                            \
+  do {                                                              \
+    ADMUX = (ADMUX & (~0x7)) | (pin & 0x7);                         \
+    ADCSRB = (ADCSRB & (~bit(MUX5))) | ((pin > 7) ? bit(MUX5) : 0); \
+    ADCSRA |= bit(ADSC);                                            \
+  } while (0);
+
+// Get the value from an ADC conversion
+#define adcHalGetValue() ADC
+
+// Get the current pin being converted
+#define adcHalGetCurrentPin() ((ADMUX & 0x7) | ((ADCSRB & bit(MUX5)) ? 0x80 : 0x00))
+
+// Complete an ADC conversion by clearing the conversion complete flag
+#define adcHalComplete()                                            \
+  do {                                                              \
+    ADCSRA |= bit(ADIF);                                            \
+  } while (0);
+  
+#endif /* __ADC_SENSOR_HAL_H__ */
\ No newline at end of file
diff --git a/src/hal/sensor/airflow.cpp b/src/hal/sensor/airflow.cpp
new file mode 100644
index 0000000..365d4df
--- /dev/null
+++ b/src/hal/sensor/airflow.cpp
@@ -0,0 +1,46 @@
+
+#include <stdint.h>
+
+#include "../../hal/hal.h"
+#include "../../hal/sensor/adc.h"
+#include "../../hal/sensor/airflow.h"
+
+#define AIRFLOW_SENSOR_PIN 0
+
+static int16_t reading;
+
+int airflowSensorHalInit(void)
+{
+  adcHalInit();
+
+  return HAL_OK;
+}
+
+int airflowSensorHalFetch(void)
+{
+  // If the ADC is free, kick off our conversion; otherwise if its no longer
+  // converting our pin, take the value and free the ADC
+  if (!adcHalBusy()) {
+    adcHalBegin(AIRFLOW_SENSOR_PIN);
+  } else if ((adcHalGetCurrentPin() == AIRFLOW_SENSOR_PIN) && !adcHalInProgress()) {
+    reading = adcHalGetValue();
+    adcHalComplete();
+    return HAL_OK;
+  }
+    
+  return HAL_IN_PROGRESS;
+}
+
+int airflowSensorHalGetValue(int16_t* value)
+{
+  // reading is ADC values, from PMF4003V data sheet:
+  // flow = (Vout - 1[V]) / 4[V] * Range = (Vout - 1[V]) / 4[V] * 20000[0.01SLM]
+  // Vout = ADCVal / 2^10 * 5[V] = ADCVal * 5[V] / 1024
+  // So:
+  // flow = (ADCVal / 1024 * 5[V] - 1[V]) / 4[V] * 20000[0.01SLM]
+  // flow = (ADCVal * 5 - 1 * 1024) / 1024 / 4 * 20000
+  // flow = (ADCVal * 5 * 20000 - 1024 * 20000) / 4096
+  // flow = (ADCVal * 100000 - 20480000) / 4096
+  *value = (int16_t) (((((int32_t) reading) * 100000L) - 20480000L) >> 12);
+  return HAL_OK;
+}
\ No newline at end of file
diff --git a/src/hal/sensor/airflow.h b/src/hal/sensor/airflow.h
new file mode 100644
index 0000000..d6df453
--- /dev/null
+++ b/src/hal/sensor/airflow.h
@@ -0,0 +1,18 @@
+
+#ifndef __AIRFLOW_SENSOR_HAL_H__
+#define __AIRFLOW_SENSOR_HAL_H__
+
+#include <stdint.h>
+
+#include "../../hal/hal.h"
+
+// TODO: Doc
+int airflowSensorHalInit(void);
+
+// TODO: Doc
+int airflowSensorHalFetch(void);
+
+// TODO: Doc
+int airflowSensorHalGetValue(int16_t* value);
+
+#endif /* __AIRFLOW_SENSOR_HAL_H__ */
\ No newline at end of file
diff --git a/src/hal/sensor/pressure.cpp b/src/hal/sensor/pressure.cpp
new file mode 100644
index 0000000..548e04d
--- /dev/null
+++ b/src/hal/sensor/pressure.cpp
@@ -0,0 +1,41 @@
+
+#include <stdint.h>
+
+#include "../../hal/hal.h"
+#include "../../hal/sensor/adc.h"
+#include "../../hal/sensor/pressure.h"
+
+#define PRESSURE_SENSOR_PIN 1
+
+static int16_t reading;
+
+int pressureSensorHalInit(void)
+{
+  adcHalInit();
+
+  return HAL_OK;
+}
+
+int pressureSensorHalFetch(void)
+{
+  // If the ADC is free, kick off our conversion; otherwise if its no longer
+  // converting our pin, take the value and free the ADC
+  if (!adcHalBusy()) {
+    adcHalBegin(PRESSURE_SENSOR_PIN);
+  } else if ((adcHalGetCurrentPin() == PRESSURE_SENSOR_PIN) && !adcHalInProgress()) {
+    reading = adcHalGetValue();
+    adcHalComplete();
+    return HAL_OK;
+  }
+    
+  return HAL_IN_PROGRESS;
+}
+
+int pressureSensorHalGetValue(int16_t* value)
+{
+  // For the MPXV7025 pressure sensor: (reading / 1024 - 0.5) / 0.018 = P in kPa
+  int32_t pascals = (((int32_t)reading * 217L) - 110995L)>>2; // convert to Pascals
+  int32_t u100umH2O = (pascals * 4177L)>>12;                  // convert Pascals to 0.1mmH2O
+  *value = (int16_t)u100umH2O;                                // return as 16 bit signed int
+  return HAL_OK;
+}
\ No newline at end of file
diff --git a/src/hal/sensor/pressure.h b/src/hal/sensor/pressure.h
new file mode 100644
index 0000000..c56be7e
--- /dev/null
+++ b/src/hal/sensor/pressure.h
@@ -0,0 +1,18 @@
+
+#ifndef __PRESSURE_SENSOR_HAL_H__
+#define __PRESSURE_SENSOR_HAL_H__
+
+#include <stdint.h>
+
+#include "../../hal/hal.h"
+
+// TODO: Doc
+int pressureSensorHalInit(void);
+
+// TODO: Doc
+int pressureSensorHalFetch(void);
+
+// TODO: Doc
+int pressureSensorHalGetValue(int16_t* value);
+
+#endif /* __PRESSURE_SENSOR_HAL_H__ */
\ No newline at end of file
diff --git a/src/hal/sensor/sensor.cpp b/src/hal/sensor/sensor.cpp
deleted file mode 100644
index a703cf1..0000000
--- a/src/hal/sensor/sensor.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-
-#include "../../hal/hal.h"
-#include "../../hal/sensor/sensor.h"
-#include <Arduino.h>
-#include <util/atomic.h>
-
-// Sensor pin defines
-#define FLOW_SENSE_PIN     A0
-#define PRESSURE_SENSE_PIN A1
-
-
-// The following function initializes Timer 5 to do pressure and flow sensor data collection
-int sensorHalInit(void)
-{
-  // Sensor timer
-  TCCR5A=0; // set timer control registers
-  TCCR5B=0x0A;  // set timer to clear timer on compare (CTC), and prescaler to divide by 8
-  TCNT5=0;  // initialize counter to 0
-  OCR5A= 10000;  // Set interrupt to 2khz
-  TIMSK5 |= 0x02; // enable timer compare interrupt
-
-  return HAL_OK;
-}
-
-
-//--------------------------------------------------------------------------------------------------
-int16_t pSum; // pressure data accumulator
-// This routine reads the pressure value as a voltage off the analog pin and inserts it into the filter
-void pressureSensorHalFetch(){
-  int16_t pIn;  // pressure value from sensor.
-  pIn=analogRead(PRESSURE_SENSE_PIN);
-  pSum = pIn; // Filter Implementation: pSum-(pSum>>4)+pIn; // filter
-}
-
-// This routine returns the pressure value out of the filter
-int pressureSensorHalGetValue(int16_t *value){
-  int32_t temp;
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
-	temp = pSum; // Filter implementation: pSum>>4;
-  }
-  int32_t pascals = ((temp * 217L) - 110995L)>>2;	// convert to Pascals
-  int32_t u100umH2O = (pascals * 4177L)>>12;		// convert Pascals to 0.1mmH2O
-  *value = (int16_t)u100umH2O;						// return as 16 bit signed int
-  return HAL_OK;
-}
-
-
-//--------------------------------------------------------------------------------------------------
-int16_t fSum;    // flow sensor data accumulator
-int32_t fVolSum; // volume accumulator, units of 0.01[mL]
-// This routine reads the flow sensor value as a voltage off the analog pin and inserts it into the filter
-// The flow gets integrated into the Volume accumulator
-void airflowSensorHalFetch(){
-  int16_t fIn;  // pressure value from sensor.
-  fIn=analogRead(FLOW_SENSE_PIN);
-  fSum = fIn; // Filter Implementation: fSum-(fSum>>3)+fIn; // filter
-  
-  // get precise volume : integrate flow at every sample
-  int16_t f; // flow in 0.01 SLM
-  airflowSensorHalGetFlow(&f);
-  // In order to preserve precision of the volume, the acculumator should be in
-  // units on the order of 0.01mL, since the longest breath cycle is 6 seconds 
-  // (5[bpm], 1:1I:E = 12[sec/breath] / 2 = 6[sec/breath] for inhalation/exhalation stages)
-  // Over 6 seconds, a sampling rate of 100[Hz] means 600 samples and if the calculation
-  // error is about 1 unit off every time, units of 0.01[mL] means a difference of only
-  // 6[mL] over the course of a breath (600[samples] * 0.01[mL] = 6[mL]).
-  // From 0.01[SLM] flow to 0.01[mL] volume:
-  //  Vol = Flow * dt
-  //      = (Flow[SLM] / (60[sec/min]) * (1000[0.01mL/0.01L])) * (0.01[sec])
-  //      = Flow * 1000 / 100 / 60
-  //      = Flow / 6
-  // TODO: Consider using units that avoids division entirely
-  // TODO: floor flow at zero before using it volume?
-  fVolSum += f / 6;
-}
-
-// This routine returns the flow sensor value out of the filter
-int airflowSensorHalGetFlow(int16_t *value){
-  int32_t temp;
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
-    temp = fSum; // Filter Implementation: (fSum>>3);
-  }  
-  // fSum (now temp) is filtered ADC values, from PMF4003V data sheet:
-  // flow = (Vout - 1[V]) / 4[V] * Range = (Vout - 1[V]) / 4[V] * 20000[0.01SLM]
-  // Vout = ADCVal / 2^10 * 5[V] = ADCVal * 5[V] / 1024
-  // So:
-  // flow = (ADCVal / 1024 * 5[V] - 1[V]) / 4[V] * 20000[0.01SLM]
-  // flow = (ADCVal * 5 - 1 * 1024) / 1024 / 4 * 20000
-  // flow = (ADCVal * 5 * 20000 - 1024 * 20000) / 4096
-  // flow = (ADCVal * 100000 - 20480000
-  *value = (int16_t) (((temp * 100000L) - 20480000L) >> 12);
-  return HAL_OK;
-}
-
-// This routine returns the integrated volume value
-int airflowSensorHalGetVolume(int16_t *value){
-  ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
-    // Since volume is stored in units [0.01mL], convert to [mL]
-    *value = fVolSum / 100;
-  }
-  return HAL_OK;
-}
-
-int airflowSensorHalResetVolume(void) {
-  fVolSum = 0;
-  return HAL_OK;
-}
-
-
-//--------------------------------------------------------------------------------------------------
-// This interrupt service routine is called every 0.5ms in order to handle timed
-// events like sensor reads
-// Note:
-//  The serial comms triggers every 100ms.
-//  The MPXV7025 pressure sensor has a response time of 1ms, and a warm-up time of 20ms.
-//    Currently sampling every 5ms and running through a /16 filter.
-//  The Posifa PMF4103V flow sensor has a response time of 5ms. Currently sampling every 10ms and
-//    running through a /8 filter
-int16_t timer5Count=0;
-ISR(TIMER5_COMPA_vect){
-  timer5Count++;
-  pressureSensorHalFetch();
-  if(timer5Count==2){
-    airflowSensorHalFetch();
-	timer5Count=0;
-  }
-}
\ No newline at end of file
diff --git a/src/hal/sensor/sensor.h b/src/hal/sensor/sensor.h
deleted file mode 100644
index 932761f..0000000
--- a/src/hal/sensor/sensor.h
+++ /dev/null
@@ -1,24 +0,0 @@
-
-#ifndef __SENSOR_HAL_H__
-#define __SENSOR_HAL_H__
-
-#include <stdint.h>
-#include "../../hal/hal.h"
-
-// Initializes the pressure and airflow sensors
-int sensorHalInit(void);
-
-// Gets the airflow sensor value in 0.01 SLM
-int airflowSensorHalGetFlow(int16_t *value);
-
-// Gets the pressor sensor value (measured in units of 0.1mmH2O)
-int pressureSensorHalGetValue(int16_t *value);
-
-// Gets the volume value in ml
-int airflowSensorHalGetVolume(int16_t *value);
-
-// Resets volume integrator
-int airflowSensorHalResetVolume(void);
-
-
-#endif /* __SENSOR_HAL_H__ */
\ No newline at end of file
diff --git a/src/hal/timer.cpp b/src/hal/timer.cpp
index 524bc55..7edf796 100644
--- a/src/hal/timer.cpp
+++ b/src/hal/timer.cpp
@@ -1,5 +1,6 @@
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #include "../hal/hal.h"
 #include "../hal/timer.h"
@@ -12,11 +13,12 @@ int timerHalInit(void)
   return HAL_OK;
 }
 
-int timerHalBegin(struct timer* timer, uint32_t duration)
+int timerHalBegin(struct timer* timer, uint32_t duration, bool periodic)
 {
   if (timer) {
     timer->start = (uint32_t) micros();
     timer->duration = duration;
+    timer->periodic = periodic;
     return HAL_OK;
   }
   return HAL_FAIL;
@@ -25,9 +27,15 @@ int timerHalBegin(struct timer* timer, uint32_t duration)
 int timerHalRun(struct timer* timer)
 {
   if (timer) {
+    // Check to see if the time has expired
     if (((uint32_t) micros()) - timer->start < timer->duration) {
       return HAL_IN_PROGRESS;
     } else {
+      // In order to keep more precise periodic timers, add the duration
+      // to the start time once the timer has timed out
+      if (timer->periodic) {
+        timer->start += timer->duration;
+      }
       return HAL_TIMEOUT;
     }
   }
diff --git a/src/hal/timer.h b/src/hal/timer.h
index 16a6085..ceec626 100644
--- a/src/hal/timer.h
+++ b/src/hal/timer.h
@@ -3,6 +3,7 @@
 #define __TIMER_HAL_H__
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #include "../hal/hal.h"
 
@@ -14,13 +15,14 @@
 struct timer {
   uint32_t start;
   uint32_t duration;
+  bool     periodic;
 };
 
 // TODO: Doc
 int timerHalInit(void);
 
 // TODO: Doc
-int timerHalBegin(struct timer* timer, uint32_t duration);
+int timerHalBegin(struct timer* timer, uint32_t duration, bool periodic);
 
 // TODO: Doc
 int timerHalRun(struct timer* timer);
diff --git a/src/modules/control.cpp b/src/modules/control.cpp
index ceea1df..4b1d6b9 100644
--- a/src/modules/control.cpp
+++ b/src/modules/control.cpp
@@ -2,6 +2,8 @@
 // Control Module
 //
 
+#include <stdint.h>
+
 #include "../pt/pt.h"
 
 #include "../hal/motor.h"
@@ -9,12 +11,27 @@
 
 #include "../modules/module.h"
 #include "../modules/control.h"
+#include "../modules/sensors.h"
 #include "../modules/parameters.h"
 
-// #define DEBUG
+#include "../util/metrics.h"
+
+#define DEBUG
 #define DEBUG_MODULE "control"
 #include "../util/debug.h"
 
+#define CONTROL_LOOP_PERIOD (10 MSEC)
+
+#define HOLD_TIME           (200 MSEC)
+
+#define INHALATION_OVERTIME(t) ((t) * 4 / 3)
+
+#define MIN_VELOCITY  2 // TODO: Fix this
+
+// Uncomment the following to enable the current closed loop control
+// #define CONTROL_CLOSED_LOOP
+
+
 // Public Variables
 struct control control = {
   .state = CONTROL_IDLE
@@ -22,15 +39,243 @@ struct control control = {
 
 // Private Variables
 static struct pt controlThread;
+static struct timer breathTimer;
 static struct timer controlTimer;
 
-static unsigned int motorCompressionDistance;
-static unsigned int motorCompressionDuration;
+static uint16_t targetVolume;
+static uint32_t totalBreathTime;
+static uint32_t targetInhalationTime;
+
+static uint32_t measuredInhalationTime;
+static uint32_t measuredExhalationTime;
+
+static bool controlComplete;
+
+// *****************
+// Control variables
+// *****************
+// Position uint8_t
+// Velocity millirev/min
+// Velocity max = 20 rpm
+static unsigned int currentPosition = 0;
+static unsigned int targetPosition = 0;
+static float targetAirFlow = 0.0f;
+static unsigned int tolerancePosition = 0;
+static unsigned int trapezoidRatio = 2;             // TEMPORARILY SET
+static uint32_t rampTime = 0;
+static uint32_t platTime = 0;
+static uint32_t elapsedTime = 0;
+static float velocityScale = 0.0f;
+static float nomVelocity = 0.0f;
+static uint32_t breathTimerStateStart = 0;
+
+static struct metrics midControlTiming;
+static float controlI=0.0f;
+static float controlOutputFiltered=0.0f;
+
+
+//this is our initial guess scaling factor to match the trajectory to flow sensor values
+#define SCALING_FACTOR 1.89f
+
+// Pre-compute the trajectory based on known set points and timing requirements
+static void computeTrajectory()
+{
+  // Debug
+  DEBUG_PRINT("COMPUTING TRAJECTORY");
+
+  // Update known measurements (position, volume estimation, etc.)
+
+  // Calculate time sections of trajectory (CURRENTLY SAME FOR I/E PHASES DUE TO SENSOR DRIVER HOLD)
+  if (control.state == CONTROL_BEGIN_EXHALATION) {
+
+    // Position information needs to be integrated to allow for adaptive positioning on exhalation
+    targetPosition = 20;
+    rampTime = (totalBreathTime - targetInhalationTime) / (2.0f + trapezoidRatio);
+    platTime = trapezoidRatio * rampTime;
+    nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0f * 1000000.0f * 60.0f / ((float)(rampTime + platTime) * 360.0f);
+    nomVelocity/=SCALING_FACTOR;
+
+  } else if (control.state == CONTROL_BEGIN_INHALATION) {
+
+    // EVENTUALLY REFACTORING FOR VOLUME TRAJECTORY, temporarily position tracking for testing
+    targetPosition = 40;
+    rampTime = targetInhalationTime / (2.0f + trapezoidRatio);
+    platTime = trapezoidRatio * rampTime;
+    nomVelocity = abs((float)targetPosition - (float)currentPosition) * 1000.0f * 1000000.0f * 60.0f / ((float)(rampTime + platTime) * 360.0f);
+    nomVelocity/=SCALING_FACTOR;
+
+  } else {
+    // Blank
+  }
+}
+
+// Track timing and scale output based on location in trajectory
+static int updateTrajectory()
+{
+
+  // Determine elapsed time and place along trajectory
+  elapsedTime = timerHalCurrent(&breathTimer) - breathTimerStateStart;
+
+  if (elapsedTime < rampTime) {
+    // Ramp up
+    velocityScale = ((float)elapsedTime / (float)rampTime);
+
+  } else if ((elapsedTime > rampTime) && (elapsedTime < (platTime + rampTime))) {
+    // Plateau
+    velocityScale = 1.0;
+
+  } else if (elapsedTime<(platTime + 2*rampTime)) {
+    // Ramp down
+    velocityScale = (float)(2 * rampTime + platTime - elapsedTime) / (float)rampTime;
+  }else
+  {
+        velocityScale=0.0f;
+	targetAirFlow=0.0f;
+        return 1;
+  }
+
+ targetAirFlow = (nomVelocity * velocityScale);
+ // Debug Output Section
+  DEBUG_PRINT_EVERY(10, "Target Position: %lu ; Nominal Velocity: %lu ; Target Velocity: %lu", 
+    (uint32_t)targetPosition, (uint32_t)nomVelocity, (uint32_t)targetAirFlow);
+  // DEBUG_PRINT_EVERY(1000, "Elapsed Time: %lu ; Ramp Time: %lu ; Velocity Scale: %lu", elapsedTime, rampTime, (uint32_t)(1000.0 * velocityScale));
+  // DEBUG_PRINT_EVERY(1000, "Proportion of Phase: %lu", velocityScale);
+ return 0;
+}
+
+// Update control commands and handle ISR/control flags
+static int updateControl(void)
+{
+
+    float flowSensorInput;
+    static float lastFlowSensorInput=0.0f;
+    float controlP,controlD;
+    float controlOut;
+    float controlOutLimited;
+
+    //TEMPORARY ASSIGNMENT. SHOULD BE STORED IN PARAMETERS
+    // TODO: Currently, enable open loop control if needed, disabling closed loop while in development
+#ifdef CONTROL_CLOSED_LOOP
+    float Kf=0.9f;
+    float Kp=1.0f;
+    float Kd=0.8f;
+    float Ki=.25f;
+#else
+    float Kf=1.0f;
+    float Kp=0.0f;
+    float Kd=0.0f;
+    float Ki=0.0f;
+#endif
+    float KiMax=80.0f; //max speed adjustment because of I-part in % of nominalVelocity
+    float controlMax=100.0f; //max speed adjustment of current target velocity
+    
+
+   if (control.state == CONTROL_EXHALATION)
+    {
+	//deactivate controller for exhilation
+	Kf=1.0f;
+        Kp=.0f;
+        Ki=.0f;
+        Kd=.0f;
+    }
+
+    metricsStart(&midControlTiming);
+
+    // Update trajectory to get instantaneous target air flow
+    if (updateTrajectory()>0)
+    {
+	motorHalCommand(targetPosition,0);
+	DEBUG_PRINT("Target Reached");
+	return 1;
+    }
+
+    // **************************************************************
+    // Here is where closed loop control structure will be developed
+    // **************************************************************
+    // Depending on whether we are in inhalation or exhalation, we will
+    // switch between closed loop air flow tracking and closed loop position
+    // tracking. The updateControl() function will accept a flag set by an ISR.
+
+
+    //read sensor inputs
+    flowSensorInput=sensors.currentFlow;//needs to be getFlowSensorInput
+
+    //calculate proportional part
+    controlP=targetAirFlow-flowSensorInput;
+
+    //calculate differential part
+    controlD=lastFlowSensorInput-flowSensorInput;
+
+    //calculate integral part
+    controlI+=controlP;
+
+    //limit integral part
+    if ((Ki*controlI)>(nomVelocity*KiMax/100.0f))
+        controlI=(nomVelocity*KiMax/100.0f)/Ki;
+
+    if ((Ki*controlI)<(-nomVelocity*KiMax/100.0f))
+       controlI=-(nomVelocity*KiMax/100.0f)/Ki;
+
+    //limit and output
+
+    controlOut=Kp*controlP+Kd*controlD+Ki*controlI;
+
+
+    if (controlOut>(controlMax/100.0f*nomVelocity))
+        controlOut=(controlMax/100.0f*nomVelocity);
+
+    if (controlOut<-(controlMax/100.0f*nomVelocity))
+        controlOut=-(controlMax/100.0f*nomVelocity);
+
+    //final control output is feed forward + limited controlOut
+    controlOutLimited=Kf*targetAirFlow+controlOut;
+
+    controlOutLimited*=SCALING_FACTOR;
+
+    //IIR filter
+    controlOutputFiltered=0.9f*controlOutputFiltered+0.1f*controlOutLimited; 
+
+
+    //TODO: Remove this diry fix here. It needs to be replace with a better trajectory planning.
+    if (control.state == CONTROL_EXHALATION)
+    {
+      // Send motor commands
+      motorHalCommand(targetPosition, controlOutputFiltered);
+    }else
+    {
+      // Send motor commands
+      motorHalCommand(targetPosition*3, controlOutputFiltered);
+    }
+    
+    lastFlowSensorInput=flowSensorInput;
+
+    metricsStop(&midControlTiming);
+
+    DEBUG_PRINT_EVERY(100,"Control Stats: Avg us: %u\n",midControlTiming.average);
+
+    DEBUG_PLOT((int32_t)flowSensorInput, (int32_t)targetAirFlow, (int32_t)controlOutLimited, (int32_t)(Kp*controlP), (int32_t)controlOutputFiltered, (int32_t)(Ki*controlD));
+
+    // Return unfinished
+    return 0;
+}
+
+static bool checkInhalationTimeout(void)
+{
+  return ((timerHalCurrent(&breathTimer) - breathTimerStateStart) >=
+          INHALATION_OVERTIME(targetInhalationTime));
+}
+
+static bool checkExhalationTimeout(void)
+{
+  return (timerHalRun(&breathTimer) != HAL_IN_PROGRESS);
+}
 
 static PT_THREAD(controlThreadMain(struct pt* pt))
 {
   PT_BEGIN(pt);
 
+  metricsReset(&midControlTiming);
+
   // Current Settings
   // 15 BPM
   // I:E 1:2
@@ -40,41 +285,78 @@ static PT_THREAD(controlThreadMain(struct pt* pt))
     if (control.state == CONTROL_IDLE) {
       DEBUG_PRINT_EVERY(10000, "state: CONTROL_IDLE");
 
+      // Reset any parameters
+      control.breathCount = 0;
+      control.ieRatioMeasured = 0;
+      control.respirationRateMeasured = 0;
+      
       // Wait for the parameters to enter the run state before
-      if (parameters.startVentilation) {
-        control.state = CONTROL_BEGIN_INHALATION;
-      }
+      PT_WAIT_UNTIL(pt, parameters.startVentilation);
+      
+      control.state = CONTROL_BEGIN_INHALATION;
       
     } else if (control.state == CONTROL_BEGIN_INHALATION) {
       DEBUG_PRINT("state: CONTROL_BEGIN_INHALATION");
       
-      // if (parameters.ventilationMode == VENTILATOR_MODE_VC) {
-      if (true) {
-        // TODO: Calculate breath parameters and motor control.
-        motorCompressionDistance = 40; // Fixed to 40 degrees excursion for now.
-        motorCompressionDuration = 0;
-        // TODO: Error check
-        motorHalBegin(MOTOR_HAL_DIRECTION_INHALATION, motorCompressionDistance, motorCompressionDuration);
-      } else {
-        // TODO: Implement Assist mode setup
-      }
+      // TODO: Actually sync up with how ie is supposed to be represented
+      //       for next, fix IE at 1:1.5 (1 / 2.5 = 0x0066)
+      parameters.ieRatioRequested = 0x0066;
+      parameters.respirationRateRequested = 15;
+
+      // Collect all set points from parameters
+      totalBreathTime = (60 SEC) / parameters.respirationRateRequested;
+      targetInhalationTime = (parameters.ieRatioRequested * totalBreathTime) >> 8; // TODO: address fixed point math
+      targetVolume = parameters.volumeRequested;
+      
+      // Initialize all 
+      measuredInhalationTime = 0;
+      measuredExhalationTime = 0;
+      
+      // Kickoff timer for monitoring breath of breathing
+      timerHalBegin(&breathTimer, totalBreathTime, false);
+      
+      breathTimerStateStart = timerHalCurrent(&breathTimer);
+      controlComplete = false;
+
+      // Compute trajectory
+      computeTrajectory();
+
+      controlI=0.0f; //reset I-part
+      controlOutputFiltered=0.0f;
       control.state = CONTROL_INHALATION;
       
     } else if (control.state == CONTROL_INHALATION) {
       DEBUG_PRINT("state: CONTROL_INHALATION");
+      
+      // Begin control loop timer when control starts
+      timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD, true);
 
-      // if (parameters.ventilationMode == VENTILATOR_MODE_VC) {
-      if (true) {
-        PT_WAIT_UNTIL(pt, motorHalRun() != HAL_IN_PROGRESS);
-      } else {
-        // TODO: Implement Assist mode run
+      while (1) {
+        // TODO: Different modes?
+        controlComplete = updateControl();
+      
+        // If the control still hasnt reached its destination and the timeout
+        // condition hasnt been met, continue the control loop, waiting for the
+        // next control cycle; otherwise exit this state
+        if (!controlComplete && !checkInhalationTimeout()) {
+          PT_WAIT_UNTIL(pt, timerHalRun(&controlTimer) != HAL_IN_PROGRESS);
+        } else {
+          break;
+        }
       }
+
+      // Update some things on state exit
+      currentPosition = targetPosition;
+      measuredInhalationTime = timerHalCurrent(&breathTimer);
+        
       control.state = CONTROL_BEGIN_HOLD_IN;
       
     } else if (control.state == CONTROL_BEGIN_HOLD_IN) {
       DEBUG_PRINT("state: CONTROL_BEGIN_HOLD_IN");
 
-      timerHalBegin(&controlTimer, 500 MSEC);
+      // Setup the hold timer
+      timerHalBegin(&controlTimer, HOLD_TIME, false);
+
       control.state = CONTROL_HOLD_IN;
       
     } else if (control.state == CONTROL_HOLD_IN) {
@@ -86,18 +368,52 @@ static PT_THREAD(controlThreadMain(struct pt* pt))
     } else if (control.state == CONTROL_BEGIN_EXHALATION) {
       DEBUG_PRINT("state: CONTROL_BEGIN_EXHALATION");
 
-      // Since exhalation is not dependent on the bag, allow the bag to decompress with the same parameters as compression
-      // In order to time exhalation, set a timer to time the exhalation cycle
-      // TODO: consider if patient takes breathe before motor has completely moved back
-      motorHalBegin(MOTOR_HAL_DIRECTION_EXHALATION, motorCompressionDistance, motorCompressionDuration);
-      timerHalBegin(&controlTimer, 2666 MSEC); // TODO: calculate this value from the breath parameters.
-      control.state = CONTROL_EXHALATION;
+      // Compute trajectory
+      computeTrajectory();
+      
+      breathTimerStateStart = timerHalCurrent(&breathTimer);
+      controlComplete = false;
       
+      control.state = CONTROL_EXHALATION;
+
     } else if (control.state == CONTROL_EXHALATION) {
       DEBUG_PRINT("state: CONTROL_EXHALATION");
+      
+      // Begin control loop timer when control starts
+      timerHalBegin(&controlTimer, CONTROL_LOOP_PERIOD, true);
+      
+      while (1) {
+        // TODO: Different modes?
+        controlComplete = updateControl();
+      
+        // If the control still hasnt reached its destination and the timeout
+        // condition hasnt been met, continue the control loop, waiting for the
+        // next control cycle; otherwise move on to the next steps in exhalation
+        // TODO: Consider what a timeout here means, it means the motor wasn't
+        //       able to get all the way back, should we really keep going?
+        if (!controlComplete && !checkExhalationTimeout()) {
+          PT_WAIT_UNTIL(pt, timerHalRun(&controlTimer) != HAL_IN_PROGRESS);
+        } else {
+          break;
+        }
+      }
+      
+      // TODO: look into this
+      currentPosition = targetPosition;
+      
+      // At this point, either wait for the breath timer to expire or find a new
+      // breath to sync with
+      PT_WAIT_UNTIL(pt, ((timerHalRun(&breathTimer) != HAL_IN_PROGRESS) ||
+                         (sensors.inhalationDetected)));
+      
+      // Calculate and update the measured times
+      uint32_t currentBreathTime = timerHalCurrent(&breathTimer);
+      measuredExhalationTime = currentBreathTime - HOLD_TIME - measuredInhalationTime;
+      control.ieRatioMeasured = (measuredExhalationTime << 8) / measuredInhalationTime; // TODO: address fixed point math
+      control.respirationRateMeasured = (60 SEC) / (currentBreathTime USEC);
+      control.breathCount++;
 
-      // TODO: Fix this condition for assisted modes
-      PT_WAIT_UNTIL(pt, motorHalRun() != HAL_IN_PROGRESS && timerHalRun(&controlTimer) != HAL_IN_PROGRESS);
+      // Check if we need to continue onto another breath or if ventilation has stopped
       control.state = (parameters.startVentilation) ? CONTROL_BEGIN_INHALATION : CONTROL_IDLE;
       
     } else {
diff --git a/src/modules/control.h b/src/modules/control.h
index cf6c6f4..d6af484 100644
--- a/src/modules/control.h
+++ b/src/modules/control.h
@@ -18,7 +18,7 @@
 struct control {
   // Variables
   uint8_t  state;
-  uint32_t respirationRateRequested;
+  uint32_t respirationRateMeasured;
   uint32_t ieRatioMeasured;
   uint32_t breathCount;
   
diff --git a/src/modules/link.cpp b/src/modules/link.cpp
index 77d5dfe..d6ddff9 100644
--- a/src/modules/link.cpp
+++ b/src/modules/link.cpp
@@ -118,15 +118,12 @@ void updateDataPacket()
   //
   public_data_packet.tidal_volume_set = parameters.volumeRequested;
   public_data_packet.tidal_volume_measured = sensors.currentVolume;
-  public_data_packet.respiratory_rate_set = parameters.respirationRateRequested;  // same field on control structure
+  public_data_packet.respiratory_rate_set = parameters.respirationRateRequested;
   public_data_packet.ie_ratio_set = parameters.ieRatioRequested; // comm.ieRatioRequested; 
   
   public_data_packet.control_state = control.state;
-  // should we use the one from parameters or this one
-  public_data_packet.respiratory_rate_set = control.respirationRateRequested; // // should we use the one from parameters or this one
-  
   public_data_packet.ie_ratio_measured = control.ieRatioMeasured;
-  //public_data_packet.respiratory_rate_measured = control.  // could not find the field for this
+  public_data_packet.respiratory_rate_measured = control.respirationRateMeasured;
   
   // readings from sensor module
   public_data_packet.plateau_value_measurement = sensors.plateauPressure;
diff --git a/src/modules/sensors.cpp b/src/modules/sensors.cpp
index c2d7884..ffb4e64 100644
--- a/src/modules/sensors.cpp
+++ b/src/modules/sensors.cpp
@@ -8,9 +8,10 @@
 
 #include "../pt/pt.h"
 
-#include "../hal/sensor/battery.h"
-#include "../hal/sensor/sensor.h"
 #include "../hal/timer.h"
+#include "../hal/sensor/battery.h"
+#include "../hal/sensor/airflow.h"
+#include "../hal/sensor/pressure.h"
 
 #include "../modules/module.h"
 #include "../modules/control.h"
@@ -39,7 +40,9 @@
 //
 // Airflow Sensor Parameters
 //
-#define AIRFLOW_SAMPLING_PERIOD                  (10 MSEC)
+#define AIRFLOW_SAMPLING_PERIOD                  (5 MSEC)
+
+#define AIRFLOW_BIAS_SAMPLES                      32
 
 #define VOLUME_MINUTE_PERIOD                      (5 SEC)
 #define VOLUME_MINUTE_WINDOW                     (60 SEC) / VOLUME_MINUTE_PERIOD
@@ -68,155 +71,217 @@ static PT_THREAD(sensorsPressureThreadMain(struct pt* pt))
   PT_BEGIN(pt);
 
   // Kick off sampling timer
-  timerHalBegin(&pressureTimer, PRESSURE_SAMPLING_PERIOD);
+  timerHalBegin(&pressureTimer, PRESSURE_SAMPLING_PERIOD, true);
   
-  int16_t pressure;
-  pressureSensorHalGetValue(&pressure);	// get pressure, in [0.1mmH2O]
+  while (1) {
+    // Fetch the latest sample from the sensor
+    PT_WAIT_UNTIL(pt, pressureSensorHalFetch() != HAL_IN_PROGRESS);
+    
+    int16_t pressure;
+    pressureSensorHalGetValue(&pressure); // get pressure, in [0.1mmH2O]
   
-  // Update public interface with the pressure value
-  sensors.currentPressure = pressure;
+    // Update public interface with the pressure value
+    sensors.currentPressure = pressure;
 
-  // Shift in new pressure
-  for (int i = PRESSURE_WINDOW - 1; i > 0; i--) {
-    previousPressure[i] = previousPressure[i - 1];
-  }
-  previousPressure[0] = pressure;
-  
-  // Derive Peak Pressure from pressure readings; updating the public value upon
-  // entry into CONTROL_HOLD_IN state
-  if ((control.state == CONTROL_HOLD_IN) && !setPeakPressure) {
-    sensors.peakPressure = currentMaxPressure;
-    currentMaxPressure = INT16_MIN;
-    setPeakPressure = true;
-  } else {
-    currentMaxPressure = max(currentMaxPressure, pressure);
-    if (control.state == CONTROL_EXHALATION) {
-      setPeakPressure = false;
+    // Shift in new pressure
+    for (int i = PRESSURE_WINDOW - 1; i > 0; i--) {
+      previousPressure[i] = previousPressure[i - 1];
     }
-  }
-  
-  // Derive Plateau Pressure from pressure readings during hold in; updating
-  // public value upon entry into CONTROL_EXHALATION state
-  if (control.state == CONTROL_HOLD_IN) {
-    plateauPressureSum += pressure;
-    plateauPressureSampleCount++;
-  } else if ((control.state == CONTROL_EXHALATION) &&
-             (plateauPressureSampleCount > 0)) {
-    sensors.plateauPressure = plateauPressureSum / plateauPressureSampleCount;
-    plateauPressureSum = 0;
-    plateauPressureSampleCount = 0;
-  }
-  
-  // Derive PEEP Pressure from pressure readings during exhalation after values
-  // have "stabilized" (ie, the difference of any one point from their average
-  // is less than some threshold)
-  if (control.state == CONTROL_EXHALATION) {
-    int32_t sum = 0;
-    for (int i = 0; i < PRESSURE_WINDOW; i++) {
-      sum += previousPressure[i];
+    previousPressure[0] = pressure;
+    
+    // Derive Peak Pressure from pressure readings; updating the public value upon
+    // entry into CONTROL_HOLD_IN state
+    if ((control.state == CONTROL_HOLD_IN) && !setPeakPressure) {
+      sensors.peakPressure = currentMaxPressure;
+      currentMaxPressure = INT16_MIN;
+      setPeakPressure = true;
+    } else {
+      currentMaxPressure = max(currentMaxPressure, pressure);
+      if (control.state == CONTROL_EXHALATION) {
+        setPeakPressure = false;
+      }
     }
     
-    int16_t average = sum / PRESSURE_WINDOW;
+    // Derive Plateau Pressure from pressure readings during hold in; updating
+    // public value upon entry into CONTROL_EXHALATION state
+    if (control.state == CONTROL_HOLD_IN) {
+      plateauPressureSum += pressure;
+      plateauPressureSampleCount++;
+    } else if ((control.state == CONTROL_EXHALATION) &&
+               (plateauPressureSampleCount > 0)) {
+      sensors.plateauPressure = plateauPressureSum / plateauPressureSampleCount;
+      plateauPressureSum = 0;
+      plateauPressureSampleCount = 0;
+    }
     
-    bool flat = true;
-    for (int i = 0; i < PRESSURE_WINDOW; i++) {
-      if (abs(average - previousPressure[i]) > PEEP_PRESSURE_FLAT_THRESHOLD) {
-        flat = false;
+    // Derive PEEP Pressure from pressure readings during exhalation after values
+    // have "stabilized" (ie, the difference of any one point from their average
+    // is less than some threshold)
+    if (control.state == CONTROL_EXHALATION) {
+      int32_t sum = 0;
+      for (int i = 0; i < PRESSURE_WINDOW; i++) {
+        sum += previousPressure[i];
+      }
+      
+      int16_t average = sum / PRESSURE_WINDOW;
+      
+      bool flat = true;
+      for (int i = 0; i < PRESSURE_WINDOW; i++) {
+        if (abs(average - previousPressure[i]) > PEEP_PRESSURE_FLAT_THRESHOLD) {
+          flat = false;
+        }
+      }
+      
+      // TODO: determine if we want this reading to update so long as it can, or
+      // measure once per breath, like other derived pressures
+      if (flat) {
+        sensors.peepPressure = pressure;
       }
     }
     
-    // TODO: determine if we want this reading to update so long as it can, or
-    // measure once per breath, like other derived pressures
-    if (flat) {
-      sensors.peepPressure = pressure;
+    // Derive inhalation detection from pressure readings during exhalation by
+    // looking for a dip in pressure below the PEEP threshold (or below an
+    // absolute pressure threshold)
+    if ((control.state == CONTROL_EXHALATION) && !sensors.inhalationDetected) {
+      if ((pressure < INHALATION_DETECTION_ABSOLUTE_THRESHOLD) ||
+          (sensors.peepPressure - pressure > INHALATION_DETECTION_PEEP_THRESHOLD)) {
+        sensors.inhalationDetected = true;
+        inhalationTimeout = 0;
+      }
     }
-  }
-  
-  // Derive inhalation detection from pressure readings during exhalation by
-  // looking for a dip in pressure below the PEEP threshold (or below an
-  // absolute pressure threshold)
-  if ((control.state == CONTROL_EXHALATION) && !sensors.inhalationDetected) {
-    if ((pressure < INHALATION_DETECTION_ABSOLUTE_THRESHOLD) ||
-        (sensors.peepPressure - pressure > INHALATION_DETECTION_PEEP_THRESHOLD)) {
-      sensors.inhalationDetected = true;
-      inhalationTimeout = 0;
+    if (sensors.inhalationDetected &&
+        (inhalationTimeout++ == (uint8_t) (INHALATION_TIMEOUT / PRESSURE_SAMPLING_PERIOD))) {
+      sensors.inhalationDetected = false;
     }
-  }
-  if (sensors.inhalationDetected &&
-      (inhalationTimeout++ == (uint8_t) (INHALATION_TIMEOUT / PRESSURE_SAMPLING_PERIOD))) {
-    sensors.inhalationDetected = false;
+    
+    DEBUG_PRINT_EVERY(100, "Pressure  = %c%u.%01u mmH2O",
+                      (pressure < 0) ? '-' : ' ', abs(pressure)/10, abs(pressure)%10);
+
+    // Ensure this threads cannot block if it somehow elapses the timer too fast
+    PT_YIELD(pt);
+
+    PT_WAIT_UNTIL(pt, timerHalRun(&pressureTimer) != HAL_IN_PROGRESS);
   }
   
-  DEBUG_PRINT_EVERY(200, "Pressure  = %c%u.%01u mmH2O",
-                    (pressure < 0) ? '-' : ' ', abs(pressure)/10, abs(pressure)%10);
-
-  PT_WAIT_UNTIL(pt, timerHalRun(&pressureTimer) != HAL_IN_PROGRESS);
-  PT_RESTART(pt);
+  // Should never reach here
   PT_END(pt);
 }
 
 static PT_THREAD(sensorsAirFlowThreadMain(struct pt* pt))
 {
+  static int32_t airflowSum = 0;
+  static uint32_t previousSampleTime = 0;
   static bool setVolumeIn = false;
   static int16_t volumeMinuteWindow[VOLUME_MINUTE_WINDOW] = {0};
   static uint8_t volumeMinuteTimeout = 0;
   static int16_t maxVolume = INT16_MIN;
   static bool volumeReset = false;
-  
+  static int16_t airflowBias = 0;
+  static int airflowBiasCounter = AIRFLOW_BIAS_SAMPLES;
+
   PT_BEGIN(pt);
   
-  // Kick off sampling timer
-  timerHalBegin(&airflowTimer, AIRFLOW_SAMPLING_PERIOD);
-  
-  int16_t airflow;
-  int16_t airvolume;
-  airflowSensorHalGetFlow(&airflow); // get airflow, in [0.1SLM]
-  airflowSensorHalGetVolume(&airvolume); // get airvolume, in [mL] (derived from volume)
-  
-  // Update public interface with the flow and volume values
-  sensors.currentFlow = airflow;
-  sensors.currentVolume = airvolume;
-  
-  // Derive Volume IN from current volume; updating the public value upon entry
-  // into CONTROL_HOLD_IN state
-  if ((control.state == CONTROL_HOLD_IN) && !setVolumeIn) {
-    sensors.volumeIn = airvolume;
-    setVolumeIn = true;
-  } else if (control.state == CONTROL_EXHALATION) {
-    setVolumeIn = false;
+  // Find the bias of the sensor
+  while (airflowBiasCounter--) {
+    PT_WAIT_UNTIL(pt, airflowSensorHalFetch() != HAL_IN_PROGRESS);
+    int16_t airflow;
+    airflowSensorHalGetValue(&airflow);
+    airflowSum += airflow;
   }
   
-  // Volume OUT cannot be derived from flow sensor due to position and direction
+  airflowBias = airflowSum / AIRFLOW_BIAS_SAMPLES;
+  airflowSum = 0;
   
-  // Derive Volume/min from current volume by remembering the max volume over
-  // last minute volume period and shifting it into the window, updating the
-  // minute volume by subtracting out the old value and adding in the new one
-  maxVolume = max(maxVolume, airvolume);
-  if (volumeMinuteTimeout++ == (uint8_t) (VOLUME_MINUTE_PERIOD / AIRFLOW_SAMPLING_PERIOD)) {
-    sensors.volumePerMinute -= volumeMinuteWindow[VOLUME_MINUTE_WINDOW - 1];
-    sensors.volumePerMinute += maxVolume;
-    for (int i = VOLUME_MINUTE_WINDOW - 1; i > 0; i--) {
-      volumeMinuteWindow[i] = volumeMinuteWindow[i - 1];
-    }
-    volumeMinuteWindow[0] = maxVolume;
-    maxVolume = INT16_MIN;
-  }
+  DEBUG_PRINT("Airflow bias = %c%u.%02u SLM",
+              (airflowBias < 0) ? '-' : ' ', abs(airflowBias)/100, abs(airflowBias)%100);
+
+  // Kick off sampling timer
+  timerHalBegin(&airflowTimer, AIRFLOW_SAMPLING_PERIOD, true);
   
-  // Determine if its time to reset the volume integrator, do it once in exhalation
-  if ((control.state == CONTROL_EXHALATION) && !volumeReset) {
-    DEBUG_PRINT("*** Reset Volume ***");
-    airflowSensorHalResetVolume();
-    volumeReset = true;
-  } else if (control.state == CONTROL_INHALATION) {
-    volumeReset = false;
+  while (1) {
+    // Fetch the latest sample from the sensor
+    PT_WAIT_UNTIL(pt, airflowSensorHalFetch() != HAL_IN_PROGRESS);
+    
+    // Calculate dt from the current time from the ideal sample time against
+    // the previous amount off of the ideal sample time
+    uint32_t currentSampleTime;
+    uint32_t dt;
+    
+    currentSampleTime = timerHalCurrent(&airflowTimer);
+    dt = AIRFLOW_SAMPLING_PERIOD + currentSampleTime - previousSampleTime;
+    previousSampleTime = currentSampleTime;
+    
+    int16_t airflow;
+    int16_t airvolume;
+    airflowSensorHalGetValue(&airflow); // get airflow, in [0.1SLM]
+    
+    // Apply the bias to the flow reading
+    airflow -= airflowBias;
+
+    // Integrate the airflow to get the volume
+    // In order to preserve precision of the volume, the acculumator should be in
+    // units on the order of 0.001mL, since the longest breath cycle is 6 seconds 
+    // (5[bpm], 1:1I:E = 12[sec/breath] / 2 = 6[sec/breath] for inhalation/exhalation stages)
+    // Over 6 seconds, a sampling rate of 200[Hz] means 1200 samples and if the calculation
+    // error is about 1 unit off every time, units of 0.001[mL] means a difference of only
+    // 1.2[mL] over the course of a breath (1200[samples] * 0.001[mL] = 1.2[mL]).
+    // From 0.01[SLM] flow to 0.001[mL] volume:
+    //  Vol = Flow * dt
+    //      = (Flow[SLM] / (60[sec/min]) * (10000[0.001mL/0.01L])) * ((dt[usec]) / (1000000[usec/sec]))
+    //      = Flow * dt * 10000 / 1000000 / 60
+    //      = Flow * dt / 6000
+    airflowSum += ((int32_t) airflow) * ((int32_t) dt) / 6000L; // airflowSum in [0.001mL]
+    airvolume = airflowSum / 1000L; // airflow in [mL]
+    
+    // Update public interface with the flow and volume values
+    sensors.currentFlow = airflow;
+    sensors.currentVolume = airvolume;
+    
+    // Derive Volume IN from current volume; updating the public value upon entry
+    // into CONTROL_HOLD_IN state
+    if ((control.state == CONTROL_HOLD_IN) && !setVolumeIn) {
+      sensors.volumeIn = airvolume;
+      setVolumeIn = true;
+    } else if (control.state == CONTROL_EXHALATION) {
+      setVolumeIn = false;
+    }
+    
+    // Volume OUT cannot be derived from flow sensor due to position and direction
+    
+    // Derive Volume/min from current volume by remembering the max volume over
+    // last minute volume period and shifting it into the window, updating the
+    // minute volume by subtracting out the old value and adding in the new one
+    maxVolume = max(maxVolume, airvolume);
+    if (volumeMinuteTimeout++ == (uint8_t) (VOLUME_MINUTE_PERIOD / AIRFLOW_SAMPLING_PERIOD)) {
+      sensors.volumePerMinute -= volumeMinuteWindow[VOLUME_MINUTE_WINDOW - 1];
+      sensors.volumePerMinute += maxVolume;
+      for (int i = VOLUME_MINUTE_WINDOW - 1; i > 0; i--) {
+        volumeMinuteWindow[i] = volumeMinuteWindow[i - 1];
+      }
+      volumeMinuteWindow[0] = maxVolume;
+      maxVolume = INT16_MIN;
+    }
+
+    // Determine if its time to reset the volume integrator, do it once in exhalation
+    if ((control.state == CONTROL_EXHALATION) && !volumeReset) {
+      DEBUG_PRINT("*** Reset Volume ***");
+      airflowSum = 0;
+      volumeReset = true;
+    } else if (control.state == CONTROL_INHALATION) {
+      volumeReset = false;
+    }
+    
+    DEBUG_PRINT_EVERY(200, "Airflow   = %c%u.%02u SLM",
+                      (airflow < 0) ? '-' : ' ', abs(airflow)/100, abs(airflow)%100);
+    DEBUG_PRINT_EVERY(200, "AirVolume = %d mL", airvolume);
+    
+    // Ensure this threads cannot block if it somehow elapses the timer too fast
+    PT_YIELD(pt);
+    
+    PT_WAIT_UNTIL(pt, timerHalRun(&airflowTimer) != HAL_IN_PROGRESS);
   }
-  
-  DEBUG_PRINT_EVERY(200, "Airflow   = %c%u.%02u SLM",
-                    (airflow < 0) ? '-' : ' ', abs(airflow)/100, abs(airflow)%100);
-  DEBUG_PRINT_EVERY(200, "AirVolume = %d mL", airvolume);
-  
-  PT_WAIT_UNTIL(pt, timerHalRun(&airflowTimer) != HAL_IN_PROGRESS);
-  PT_RESTART(pt);
+
+  // Should never reach here
   PT_END(pt);
 }
 
@@ -245,6 +310,9 @@ PT_THREAD(sensorsThreadMain(struct pt* pt))
   if (!PT_SCHEDULE(sensorsBatteryThreadMain(&sensorsBatteryThread))) {
     PT_EXIT(pt);
   }
+  
+  // TODO: Mess with the units to make the graph scale nicely?
+  DEBUG_PLOT(sensors.currentFlow, sensors.currentVolume, sensors.currentPressure);
 
   PT_RESTART(pt);
   PT_END(pt);
@@ -253,7 +321,10 @@ PT_THREAD(sensorsThreadMain(struct pt* pt))
 int sensorsModuleInit(void)
 {
   // TODO: Improve error propagation for all hal init failures
-  if (sensorHalInit() != HAL_OK) {
+  if (pressureSensorHalInit() != HAL_OK) {
+    return MODULE_FAIL;
+  }
+  if (airflowSensorHalInit() != HAL_OK) {
     return MODULE_FAIL;
   }
   if (batterySensorHalInit() != HAL_OK) {
diff --git a/src/util/debug.h b/src/util/debug.h
index fef2765..2c49e1b 100644
--- a/src/util/debug.h
+++ b/src/util/debug.h
@@ -1,7 +1,11 @@
 
 #include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdbool.h>
 
 #include <HardwareSerial.h>
+#include <Arduino.h>	/* millis() */
 
 #ifndef DEBUG_MODULE
 #error "Cannot include debug.h without defining DEBUG_MODULE"
@@ -15,10 +19,51 @@
 #define DEBUG_SERIAL_PORT Serial
 #endif
 
+// In order to enable the standard Arduino Serial Plotter, all other types of
+// messages must be disabled and only a single module can plot a single set
+// of variables at one time. As such, to use the DEBUG_PLOT macro, define
+// DEBUG_PLOTTING here with the module name (DEBUG_MODULE) to be plotted,
+// example commented out below:
+// #define DEBUG_PLOTTING "control"
+
 // Initial setup of debug; only to be used once
 #define DEBUG_BEGIN DEBUG_SERIAL_PORT.begin(230400)
 
 #ifdef DEBUG
+#ifdef DEBUG_PLOTTING
+
+// Use DEBUG_PLOT to plot a set of variables, can only be used once in a module
+// and requires DEBUG_PLOTTING to be defined and set the correct module's name,
+// see above
+// NOTE: Compiler seems pretty good at optimizing out the strcmp, would like a
+//       better way of doing this but the rest of the mechanism is so clean, I
+//       am relying on the compiler to do the optimization rather than via some
+//       macros somehow...
+#define DEBUG_PLOT(...)                                                 \
+  do {                                                                  \
+    static bool header = true;                                          \
+    if (strcmp(DEBUG_PLOTTING, DEBUG_MODULE) == 0) {                    \
+      if (header) {                                                     \
+        DEBUG_SERIAL_PORT.println("ms," #__VA_ARGS__);                  \
+        header = false;                                                 \
+      }                                                                 \
+      DEBUG_SERIAL_PORT.print(millis());               			\
+      DEBUG_SERIAL_PORT.print(',');               			\
+      int32_t _vars[] = { __VA_ARGS__ };                                \
+      for (int i = 0; i < sizeof(_vars) / sizeof(int32_t); i++) {       \
+        if (i > 0) DEBUG_SERIAL_PORT.print(",");                        \
+        DEBUG_SERIAL_PORT.print(_vars[i]);                              \
+      }                                                                 \
+      DEBUG_SERIAL_PORT.print("\n");                                    \
+    }                                                                   \
+  } while (0);                                                          \
+
+#define DEBUG_PRINT(...)
+#define DEBUG_PRINT_EVERY(...)
+
+#else
+
+#define DEBUG_PLOT(...)
 
 // Use DEBUG_PRINT to add print messages like printf
 #define DEBUG_PRINT(...)                                                \
@@ -41,7 +86,9 @@
     }                                                                   \
   } while (0);
 
+#endif
 #else
+#define DEBUG_PLOT(...)
 #define DEBUG_PRINT(...)
 #define DEBUG_PRINT_EVERY(...)
 #endif
\ No newline at end of file