From ff5b85846ec67012c43dda1c92b032f255c1baef Mon Sep 17 00:00:00 2001
From: Teddy Ort <teddy@mit.edu>
Date: Thu, 26 Mar 2020 02:19:36 -0400
Subject: [PATCH 1/2] Add homing routine

---
 e-vent.ino | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 55 insertions(+), 2 deletions(-)

diff --git a/e-vent.ino b/e-vent.ino
index d55448d..27320b2 100644
--- a/e-vent.ino
+++ b/e-vent.ino
@@ -21,6 +21,7 @@ int BPM_PIN = A1;
 int IE_PIN = A2;
 int PRESS_POT_PIN = A3;
 int PRESS_SENSE_PIN = A4;
+int HOME_PIN = 4;
 int ROBO_D0 = 2;
 int ROBO_D1 = 3;
 
@@ -39,7 +40,7 @@ float VOL_MIN = 100;
 float VOL_MAX = 600; // 900; // For full 
 
 //Setup States
-enum States {DEBUG_STATE, IN_STATE, PAUSE_STATE, EX_STATE};
+enum States {DEBUG_STATE, IN_STATE, PAUSE_STATE, EX_STATE, PREHOME_STATE, HOMING_STATE, POSTHOME_STATE};
 States state;
 bool enteringState;
 unsigned long stateTimer;
@@ -165,8 +166,9 @@ void setup() {
   
   //Initialize
   analogReference(EXTERNAL); // For the pressure reading
+  pinMode(HOME_PIN, INPUT_PULLUP); // Pull up the limit switch
   displ.begin();
-  setState(IN_STATE); // Initial state
+  setState(PREHOME_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
@@ -242,4 +244,55 @@ void loop() {
     if(millis()-stateTimer > Tex*1000)
       setState(IN_STATE);
   }
+
+  else if(state == PREHOME_STATE){
+    //Entering
+    if(enteringState){
+      enteringState = false;
+      //Consider displaying homing status on the screen
+      roboclaw.BackwardM1(address, Vhome);
+    }
+
+    // Check status of limit switch
+    if(digitalRead(HOME_PIN) == LOW) {
+      setState(HOMING_STATE); 
+    }
+
+    // Consider a timeout to give up on homing
+  }
+
+  else if(state == HOMING_STATE){
+    //Entering
+    if(enteringState){
+      enteringState = false;
+      //Consider displaying homing status on the screen
+      roboclaw.ForwardM1(address, Vhome);
+    }
+    
+    if(digitalRead(HOME_PIN) == HIGH) {
+      // Stop motor
+      roboclaw.ForwardM1(address,0);
+      delay(pauseHome);
+      roboclaw.SetEncM1(address, 0); // Zero the encoder
+      setState(POSTHOME_STATE);
+      
+    }
+    // Consider a timeout to give up on homing
+  }
+  
+  else if(state == POSTHOME_STATE){
+    //Entering
+    if(enteringState){
+      enteringState = false;
+      roboclaw.ForwardM1(address,Vhome);
+    }
+
+    if(abs(motorPosition - bagHome) < goalTol){
+      // Stop the motor and home encoder
+      roboclaw.ForwardM1(address,0);
+      delay(pauseHome);
+      roboclaw.SetEncM1(address, 0); // Zero the encoder
+      setState(IN_STATE);
+    }
+  }
 }

From 5e96ba10092a79e2817ba2e1566385bdb704c4ea Mon Sep 17 00:00:00 2001
From: Teddy Ort <teddy@mit.edu>
Date: Thu, 26 Mar 2020 02:22:10 -0400
Subject: [PATCH 2/2] Add homing settings

---
 e-vent.ino | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/e-vent.ino b/e-vent.ino
index 27320b2..775cc31 100644
--- a/e-vent.ino
+++ b/e-vent.ino
@@ -9,9 +9,11 @@ bool DEBUG = false; // For logging
 int maxPwm = 255; // Maximum for PWM is 255 but this can be set lower
 int loopPeriod = 25; // The period (ms) of the control loop delay
 int pauseTime = 250; // Time in ms to pause after inhalation
-double Vex = 600; //1000;  // Velocity to exhale
-double rampTime = 0.5; // The time (s) the velocity profile takes to ramp up and down
-double goalTol = 20;
+double Vex = 600; // Velocity to exhale
+double Vhome = 30; //The speed to use during homing
+int goalTol = 20; // The tolerance to start stopping on reaching goal
+int bagHome = 100; // The bag-specific position of the bag edge
+int pauseHome = 250; // The pause time (ms) during homing to ensure stability
 
 // Pins
 ////////////
@@ -87,7 +89,6 @@ void readPots(){
   Tex = period - Tin;
   Vin = Volume/Tin; // Velocity in clicks/s
 
-  displ.writeVolume(100 * Volume/VOL_MAX);
   displ.writeBPM(bpm);
   displ.writeIEratio(ie);
   if(DEBUG){