diff --git a/Pressure.h b/Pressure.h
new file mode 100644
index 0000000..3afff6f
--- /dev/null
+++ b/Pressure.h
@@ -0,0 +1,65 @@
+#ifndef Pressure_h
+#define Pressure_h
+
+#include "Arduino.h"
+
+class Pressure {
+public:
+  Pressure(int pin): 
+    sense_pin_(pin),
+    current_(0.0),
+    current_peak_(0.0),
+    peak_(0.0),
+    plateau_(0.0),
+    peep_(0.0) {}
+
+  //Get pressure reading
+  void read() {
+    // read the voltage
+    int V = analogRead(sense_pin_); 
+
+    float Pmin = -100.0;   // pressure max in mbar
+    float Pmax = 100.0;    // pressure min in mbar
+    float Vmax = 1024;     // max voltage in range from analogRead
+    float R = 32./37;      // Internal 32K resistor and external 5K resistor ratio
+    // convert to pressure
+    float pres = (10 * V/Vmax * R - 1) * (Pmax-Pmin)/8. + Pmin; //mmHg
+
+    // convert to cmH20
+    pres *= 1.01972;
+
+    // update peak
+    current_peak_ = max(current_peak_, pres);
+
+    current_ = pres;
+  }
+
+  float get() {
+    return current_;
+  }
+
+  void set_peak_and_reset() {
+    peak_ = current_peak_;
+    current_peak_ = 0.0;
+  }
+
+  void set_plateau() {
+    plateau_ = get();
+  }
+
+  void set_peep() {
+    peep_ = get();
+  }
+
+  int peak() { return round(peak_); }
+  int plateau() { return round(plateau_); }
+  int peep() { return round(peep_); }
+
+private:
+  int sense_pin_;
+  float current_;
+  float current_peak_;
+  float peak_, plateau_, peep_;
+};
+
+#endif
diff --git a/e-vent.ino b/e-vent.ino
index b9a7435..46f364b 100644
--- a/e-vent.ino
+++ b/e-vent.ino
@@ -1,6 +1,7 @@
 #include <LiquidCrystal.h>
 #include <RoboClaw.h>
 #include "Display.h"
+#include "Pressure.h"
 
 // Settings
 ////////////
@@ -60,11 +61,13 @@ RoboClaw roboclaw(&serial,10000);
 int motorPosition = 0;
 
 // LCD Screen
-double pressOffset = 0;
 const int rs = 12, en = 11, d4 = 10, d5 = 9, d6 = 8, d7 = 7;
 LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
 Display displ(&lcd);
 
+// Pressure
+Pressure pressure(PRESS_SENSE_PIN);
+
 // Functions
 ////////////
 
@@ -107,28 +110,13 @@ void readPots(){
     Serial.print("\tVin:");
     Serial.print(Vin);
     Serial.print("\tVex:");
-    Serial.println(Vex);
+    Serial.print(Vex);
+    Serial.print("\tPressure:");
+    Serial.print(pressure.get());
+    Serial.println();
   }
 }
 
-//Get pressure reading
-float readPressure() {
-  // read the voltage
-  int V = analogRead(PRESS_SENSE_PIN); 
-
-  float Pmin = -100.0;        // pressure max in mbar
-  float Pmax = 100.0;         // pressure min in mbar
-  float Vmax = 1024;            // max voltage in range from analogRead
-  float R = 32./37;      // Internal 32K resistor and external 5K resistor ratio
-  // convert to pressure
-  float pres = (10 * V/Vmax * R - 1) * (Pmax-Pmin)/8. + Pmin; //mmHg
-
-  //convert to cmH20
-  pres *= 1.01972;
-  
-  return pres - pressOffset;
-}
-
 int readEncoder() {
   uint8_t robot_status;
   bool valid;
@@ -166,16 +154,13 @@ void setup() {
   delay(1000);
   
   //Initialize
-  analogReference(EXTERNAL); // For the pressure reading
+  analogReference(EXTERNAL); // For the pressure and pots reading
   displ.begin();
   setState(IN_STATE); // Initial state
   roboclaw.begin(38400); // Roboclaw
   roboclaw.SetM1MaxCurrent(address, 10000); // Current limit is 10A
   roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps); // Set PID Coefficients
   roboclaw.SetEncM1(address, 0); // Zero the encoder
-
-  // Calibrate pressure sensor
-  pressOffset = readPressure();
   
   if(DEBUG){
     // setup serial coms
@@ -200,6 +185,9 @@ void loop() {
   // Update display header
   displ.writeHeader();
   
+  // read pressure every cycle to keep track of peak
+  pressure.read();
+  
   if(state == DEBUG_STATE){
     // Stop motor
     roboclaw.ForwardM1(address,0);
@@ -213,21 +201,18 @@ void loop() {
       goToPosition(Volume, Vin);
     }
     
-    if(millis()-stateTimer > Tin*1000 || abs(motorPosition - Volume) < goalTol)
+    if(millis()-stateTimer > Tin*1000 || abs(motorPosition - Volume) < goalTol){
       setState(PAUSE_STATE);
+    }
   }
   
   else if(state == PAUSE_STATE){
     // Entering
     if(enteringState){
-      // Start the pressure averaging
       enteringState = false;
     }
     if(millis()-stateTimer > pauseTime){
-      //Finish the pressure averaging
-      displ.writePeakP(0);
-      displ.writePlateauP(round(readPressure()));
-      displ.writePEEP(0);
+      pressure.set_plateau();
       
       setState(EX_STATE);
     }
@@ -244,7 +229,12 @@ void loop() {
     if(abs(motorPosition) < goalTol)
       roboclaw.ForwardM1(address,0);
       
-    if(millis()-stateTimer > Tex*1000)
+    if(millis()-stateTimer > Tex*1000){
+      pressure.set_peak_and_reset();
+      displ.writePeakP(pressure.peak());
+      displ.writePEEP(pressure.peep());
+      displ.writePlateauP(pressure.plateau());
       setState(IN_STATE);
+    }
   }
 }