From a63e50f70f1ab99b036346dd6d635544c062d0d8 Mon Sep 17 00:00:00 2001
From: Jeff Chen <jeffrey@cmu.edu>
Date: Mon, 17 Jun 2013 21:39:59 -0400
Subject: [PATCH 01/10] Add LeakDetector class in analog.cpp/analog.h

---
 src/analog.cpp | 15 +++++++++++++++
 src/analog.h   | 13 +++++++++++++
 2 files changed, 28 insertions(+)

diff --git a/src/analog.cpp b/src/analog.cpp
index 6cf0966..b67fed3 100644
--- a/src/analog.cpp
+++ b/src/analog.cpp
@@ -81,6 +81,21 @@ bool AnalogKillSwitch::getValueThresh() {
 	return (getValueRaw() > threshold);
 }
 
+
+LeakDetector::LeakDetector(PinName pin, float thresh) :
+AnalogInput(pin),
+threshold(thresh)
+{
+
+}
+
+LeakDetector::~LeakDetector() {}
+
+bool LeakDetector::getValueThresh()
+{
+	return (getValueRaw() > threshold);
+}
+
 void updateKill() {
 	isAlive = kill.getValueThresh();
 }
diff --git a/src/analog.h b/src/analog.h
index cb25a6b..588b52a 100644
--- a/src/analog.h
+++ b/src/analog.h
@@ -52,6 +52,19 @@ class AnalogKillSwitch : public AnalogInput {
 	float value, threshold;
 };
 
+class LeakDetector : AnalogInput
+{
+public:
+	LeakDetector(PinName pin, float thresh);
+	~LeakDetector();
+
+	bool getValueThresh();
+
+private:
+	float threshold;
+	
+};
+
 
 void updateKill();
 

From 0d2631cdbeeacb1248f9cb8371b6942134d70af9 Mon Sep 17 00:00:00 2001
From: Jeff Chen <jeffrey@cmu.edu>
Date: Mon, 17 Jun 2013 21:47:17 -0400
Subject: [PATCH 02/10] Add leak detection to main loop. TODO: check compiling

---
 src/analog.cpp       | 5 +++++
 src/analog.h         | 6 +++++-
 src/avnavcontrol.cpp | 5 +++--
 src/global.cpp       | 4 ++++
 4 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/src/analog.cpp b/src/analog.cpp
index b67fed3..8b676a2 100644
--- a/src/analog.cpp
+++ b/src/analog.cpp
@@ -91,6 +91,11 @@ threshold(thresh)
 
 LeakDetector::~LeakDetector() {}
 
+void updateLeak()
+{
+	hasLeak = leakDetector.getValueThresh();
+}
+
 bool LeakDetector::getValueThresh()
 {
 	return (getValueRaw() > threshold);
diff --git a/src/analog.h b/src/analog.h
index 588b52a..8b51793 100644
--- a/src/analog.h
+++ b/src/analog.h
@@ -5,7 +5,7 @@
 
 /*
  * Generic class for devices that output analog to the mbed.
- * Currently, these are the pressure sensor and the killswitch.
+ * Currently, these are the pressure sensor, leak detector, and killswitch.
  */
 class AnalogInput {
 public:
@@ -65,12 +65,16 @@ class LeakDetector : AnalogInput
 	
 };
 
+extern volatile bool hasLeak;
+void updateLeak();
+
 
 void updateKill();
 
 //The two analog devices are the killswitch and pressure sensor.
 extern AnalogKillSwitch kill;
 extern AnalogPressureSensor pressure;
+extern LeakDetector leakDetector;
 extern volatile bool isAlive;
 
 #endif
diff --git a/src/avnavcontrol.cpp b/src/avnavcontrol.cpp
index 65a0dbc..89c4581 100644
--- a/src/avnavcontrol.cpp
+++ b/src/avnavcontrol.cpp
@@ -30,7 +30,7 @@ int main() {
 	imu.p_device->attach(&rx_interrupt_imu, Serial::RxIrq);
 	
 	//create the tickers here
-	Ticker tick[5];
+	Ticker tick[6];
 	if (!debug) {
 		tick[0].attach(&send_status_pc, 0.1);
 	}
@@ -38,8 +38,9 @@ int main() {
 	tick[2].attach(&motor_send_wrapper, DT/2);
 	tick[3].attach(&updateKill, .01);
 	tick[4].attach(&updatePressure, 0.1);
+	tick[5].attach(&updateLeak, .01);
 	
-	while (true) {
+	while (!hasLeak) {
 		if (!motor.isTxEmpty()) {
 			tx_interrupt_motor();
 		}
diff --git a/src/global.cpp b/src/global.cpp
index 60bf5da..626adb0 100644
--- a/src/global.cpp
+++ b/src/global.cpp
@@ -14,6 +14,9 @@
 // TODO: check the magic threshold 0.85f. Too high/low?
 AnalogKillSwitch kill(p16, p18, 0.85f); // Vin, Vout
 
+//TODO: I picked 0.85f because that's what kill used. Check this.
+LeakDetector leakDetector(p20, 0.85f); //pin 20
+
 /* 
  * Daniel's was depth = adc_buffer[ADC_PRESS] * 0.361904762 - 101.33333;
  * That was based on the dsPIC33FJ family's ADC converter.
@@ -47,3 +50,4 @@ bool debug = false;
 
 volatile bool isAlive = false;
 volatile int depth(0);
+volatile bool hasLeak = false;

From 9c430c19912e473fc8d53c04d7540309e9cda8ef Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Mon, 17 Jun 2013 23:03:23 -0700
Subject: [PATCH 03/10] Split safety features into their own safety.cpp file

---
 Makefile       |  6 ++++--
 src/analog.h   |  3 +--
 src/global.cpp |  1 -
 src/safety.cpp |  9 +++++++++
 src/safety.h   | 10 ++++++++++
 5 files changed, 24 insertions(+), 5 deletions(-)
 create mode 100644 src/safety.cpp
 create mode 100644 src/safety.h

diff --git a/Makefile b/Makefile
index 08bc2ec..592e51b 100644
--- a/Makefile
+++ b/Makefile
@@ -17,7 +17,8 @@ $(SRC_PATH)pid.o \
 $(SRC_PATH)pid_library.o \
 $(SRC_PATH)global.o \
 $(SRC_PATH)Kalman.o \
-$(SRC_PATH)buffer.o
+$(SRC_PATH)buffer.o \
+$(SRC_PATH)safety.o
 
 ASSEMBLY = $(SRC_PATH)imu.s \
 $(SRC_PATH)pc.s \
@@ -28,7 +29,8 @@ $(SRC_PATH)pid.s \
 $(SRC_PATH)pid_library.s \
 $(SRC_PATH)global.s \
 $(SRC_PATH)Kalman.s \
-$(SRC_PATH)buffer.s
+$(SRC_PATH)buffer.s \
+$(SRC_PATH)safety.s
 
 #checks if we're on Windows or *nix
 #based on whether the SystemRoot variable
diff --git a/src/analog.h b/src/analog.h
index 8b51793..e7ea19d 100644
--- a/src/analog.h
+++ b/src/analog.h
@@ -65,7 +65,6 @@ class LeakDetector : AnalogInput
 	
 };
 
-extern volatile bool hasLeak;
 void updateLeak();
 
 
@@ -75,6 +74,6 @@ void updateKill();
 extern AnalogKillSwitch kill;
 extern AnalogPressureSensor pressure;
 extern LeakDetector leakDetector;
-extern volatile bool isAlive;
+extern volatile bool isAlive, hasLeak, isLowVoltage;
 
 #endif
diff --git a/src/global.cpp b/src/global.cpp
index 626adb0..abbfc02 100644
--- a/src/global.cpp
+++ b/src/global.cpp
@@ -50,4 +50,3 @@ bool debug = false;
 
 volatile bool isAlive = false;
 volatile int depth(0);
-volatile bool hasLeak = false;
diff --git a/src/safety.cpp b/src/safety.cpp
new file mode 100644
index 0000000..bd1f8e9
--- /dev/null
+++ b/src/safety.cpp
@@ -0,0 +1,9 @@
+#include "safety.h"
+
+volatile bool hasLeak = false, isLowVoltage = false;
+extern volatile bool isAlive;
+
+bool needStop()
+{
+	return (!isAlive | hasLeak | isLowVoltage);
+}
\ No newline at end of file
diff --git a/src/safety.h b/src/safety.h
new file mode 100644
index 0000000..e6bb874
--- /dev/null
+++ b/src/safety.h
@@ -0,0 +1,10 @@
+#ifndef SAFETY_H
+#define SAFETY_H
+
+#include "mbed.h"
+
+#include "analog.h"
+
+bool needStop();
+
+#endif

From 9497b63a87308b408386661f86fc2758bc2c5090 Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Mon, 17 Jun 2013 23:04:35 -0700
Subject: [PATCH 04/10] Fix coding style near analog and safety stuff

---
 src/analog.cpp       | 6 ++++--
 src/analog.h         | 9 ++++++---
 src/avnavcontrol.cpp | 2 +-
 3 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/analog.cpp b/src/analog.cpp
index 8b676a2..db32d5d 100644
--- a/src/analog.cpp
+++ b/src/analog.cpp
@@ -86,10 +86,12 @@ LeakDetector::LeakDetector(PinName pin, float thresh) :
 AnalogInput(pin),
 threshold(thresh)
 {
-
+	
 }
 
-LeakDetector::~LeakDetector() {}
+LeakDetector::~LeakDetector() {
+	
+}
 
 void updateLeak()
 {
diff --git a/src/analog.h b/src/analog.h
index e7ea19d..5aca13a 100644
--- a/src/analog.h
+++ b/src/analog.h
@@ -7,7 +7,8 @@
  * Generic class for devices that output analog to the mbed.
  * Currently, these are the pressure sensor, leak detector, and killswitch.
  */
-class AnalogInput {
+class AnalogInput
+{
 public:
 	AnalogInput(PinName pin);
 	~AnalogInput();
@@ -20,7 +21,8 @@ class AnalogInput {
  * The pressure sensor outputs voltage linearly with depth.
  * The variables must be manually calibrated.
  */
-class AnalogPressureSensor : public AnalogInput {
+class AnalogPressureSensor : public AnalogInput
+{
 public:
 	// Pass in calibration data in the form of y = mx + b equation.
 	AnalogPressureSensor(PinName pin, float calibration_m, float calibration_b);
@@ -38,7 +40,8 @@ void updatePressure();
  * The kill switch is either killed or unkilled, determined by whether
  * the voltage is greater than some specified threshold.
  */
-class AnalogKillSwitch : public AnalogInput {
+class AnalogKillSwitch : public AnalogInput
+{
 public:
 	AnalogKillSwitch(PinName Vin, PinName Vout, float thresh);
 	~AnalogKillSwitch();
diff --git a/src/avnavcontrol.cpp b/src/avnavcontrol.cpp
index 89c4581..1fccc52 100644
--- a/src/avnavcontrol.cpp
+++ b/src/avnavcontrol.cpp
@@ -38,7 +38,7 @@ int main() {
 	tick[2].attach(&motor_send_wrapper, DT/2);
 	tick[3].attach(&updateKill, .01);
 	tick[4].attach(&updatePressure, 0.1);
-	tick[5].attach(&updateLeak, .01);
+	tick[5].attach(&updateLeak, 0.01);
 	
 	while (!hasLeak) {
 		if (!motor.isTxEmpty()) {

From 85a6f4a1132bb3d58dd740accdbe58ddb87e5605 Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Mon, 17 Jun 2013 23:05:54 -0700
Subject: [PATCH 05/10] Integrate safety features with motor control

---
 src/avnavcontrol.cpp |  2 +-
 src/pid.cpp          | 13 +++++++------
 src/pid.h            |  3 +--
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/avnavcontrol.cpp b/src/avnavcontrol.cpp
index 1fccc52..907b1b2 100644
--- a/src/avnavcontrol.cpp
+++ b/src/avnavcontrol.cpp
@@ -40,7 +40,7 @@ int main() {
 	tick[4].attach(&updatePressure, 0.1);
 	tick[5].attach(&updateLeak, 0.01);
 	
-	while (!hasLeak) {
+	while (true) {
 		if (!motor.isTxEmpty()) {
 			tx_interrupt_motor();
 		}
diff --git a/src/pid.cpp b/src/pid.cpp
index c9084c0..47fe67e 100644
--- a/src/pid.cpp
+++ b/src/pid.cpp
@@ -222,15 +222,16 @@ void update_motors(float hpid, float dpid, float ppid) {
 		}
 	}
 	
-	// If the sub is dead, then turn the motors off. Otherwise, set them to
-	// the values that came out of PID.
+	// If the sub is dead or has an emergency, then turn the motors off.
+	// Otherwise, set them to the values that came out of PID.
 	// Note: When the sub is dead, the kill switch actually cuts power to the
 	// motors, so they stop moving. This is here so that the motors don't start
 	// moving when the sub is unkilled (alive) until we want them to.
-	motorArray[LEFT] = isAlive ? powerNum[LEFT] : 127;
-	motorArray[RIGHT] = isAlive ? powerNum[RIGHT] : 127;
-	motorArray[FRONT] = isAlive ? powerNum[FRONT] : 127;
-	motorArray[BACK] = isAlive ? powerNum[BACK] : 127;
+	bool isRun = !needStop();
+	motorArray[LEFT ] = isRun ? powerNum[LEFT ] : 127;
+	motorArray[RIGHT] = isRun ? powerNum[RIGHT] : 127;
+	motorArray[FRONT] = isRun ? powerNum[FRONT] : 127;
+	motorArray[BACK ] = isRun ? powerNum[BACK ] : 127;
 
 }
 
diff --git a/src/pid.h b/src/pid.h
index 6def76c..40d0c69 100644
--- a/src/pid.h
+++ b/src/pid.h
@@ -11,6 +11,7 @@
 #include "motor.h"
 #include "defs.h"
 #include "pc.h"
+#include "safety.h"
 
 extern Kalman pitchK, rollK;
 
@@ -26,8 +27,6 @@ extern volatile int desHead, desDepth, desPower;
 extern volatile int depth;
 extern bool debug;
 
-extern volatile bool isAlive;
-
 extern unsigned char motorArray[4];
 extern AnalogKillSwitch kill;
 

From 516de3240ec4226e44aed3c68e92fdec86111081 Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Mon, 17 Jun 2013 23:06:13 -0700
Subject: [PATCH 06/10] Calibrate leak detection with tap water

This may have to be re-calibrated with TRANSDEC water, which I expect will have
more ions dissolved.
---
 src/global.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/global.cpp b/src/global.cpp
index abbfc02..ec73163 100644
--- a/src/global.cpp
+++ b/src/global.cpp
@@ -14,8 +14,8 @@
 // TODO: check the magic threshold 0.85f. Too high/low?
 AnalogKillSwitch kill(p16, p18, 0.85f); // Vin, Vout
 
-//TODO: I picked 0.85f because that's what kill used. Check this.
-LeakDetector leakDetector(p20, 0.85f); //pin 20
+// Threshold determined experimentally with tap water
+LeakDetector leakDetector(p20, 0.5f); //pin 20
 
 /* 
  * Daniel's was depth = adc_buffer[ADC_PRESS] * 0.361904762 - 101.33333;

From 9f31c2722d6a2f3ec8b92c6a46ac92f3b4d3b5ef Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Mon, 17 Jun 2013 23:37:01 -0700
Subject: [PATCH 07/10] Send emergency type to PC

It's good to know why your sub randomly stops running
---
 src/pc.cpp     | 11 +++++++++--
 src/pc.h       |  1 +
 src/safety.cpp | 25 ++++++++++++++++++++++++-
 src/safety.h   |  3 ++-
 4 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/src/pc.cpp b/src/pc.cpp
index 5c05c46..c6afc3c 100644
--- a/src/pc.cpp
+++ b/src/pc.cpp
@@ -18,14 +18,15 @@ PC::PC(PinName tx, PinName rx, int baud)
  *         'd', AVNav-encoded data for depth (pressure sensor),
  *         'p', AVNav-encoded data for power (motors),
  *         kill switch status (l is dead and k is alive),
+ *         'e', AVNav-encoded code for type of emergency stop
  *         '\n' to terminate the message.
  */
-	mes = new char[12];
+	mes = new char[15];
 	// Copy an initial message to the buffer. The pipe characters (|) will be
 	// replaced with AVNav-encoded numbers when this is actually used. We chose
 	// the pipe character because it cannot be confused with AVNav-encoded
 	// data -- see comments in encode_avnav().
-	strcpy(mes, "h||d||p|||\n");
+	strcpy(mes, "h||d||p|||e||\n");
 }
 
 // The avnav functions encode data into AVNav format: 
@@ -79,6 +80,12 @@ void send_status_pc()
 	// get kill switch value
 	// TODO: is this correct? I assume voltage on kill pin means alive.
 	pc.mes[9] = isAlive ? 'k' : 'l';
+	
+	// Are we in an emergency stop? What kind?
+	avnav_temp = pc.encode_avnav(getEmergencyType());
+	pc.mes[11] = avnav_temp.byte1;
+	pc.mes[12] = avnav_temp.byte2;
+	
 	/*
 				char* tmp;
 			sprintf(tmp, "%d", pressure.getValueCalibrated());
diff --git a/src/pc.h b/src/pc.h
index 6fc2648..475dbb8 100644
--- a/src/pc.h
+++ b/src/pc.h
@@ -6,6 +6,7 @@
 #include "analog.h"
 #include "pid.h"
 #include "defs.h"
+#include "safety.h"
 
 struct avnav {
 	char byte1;
diff --git a/src/safety.cpp b/src/safety.cpp
index bd1f8e9..1502c04 100644
--- a/src/safety.cpp
+++ b/src/safety.cpp
@@ -3,7 +3,30 @@
 volatile bool hasLeak = false, isLowVoltage = false;
 extern volatile bool isAlive;
 
-bool needStop()
+enum EmergencyTypes {
+	NO_EMERGENCY,
+	LEAK,
+	VOLTAGE_LOW,
+	VOLTAGE_HIGH	// not used
+}
+
+inline bool needStop()
 {
 	return (!isAlive | hasLeak | isLowVoltage);
+}
+
+inline int getEmergencyType()
+{
+	if (hasLeak)
+	{
+		return LEAK;
+	}
+	else if (isLowVoltage)
+	{
+		return VOLTAGE_LOW;
+	}
+	else
+	{
+		return NO_EMERGENCY;
+	}
 }
\ No newline at end of file
diff --git a/src/safety.h b/src/safety.h
index e6bb874..65bdbec 100644
--- a/src/safety.h
+++ b/src/safety.h
@@ -5,6 +5,7 @@
 
 #include "analog.h"
 
-bool needStop();
+inline bool needStop();
+inline int getEmergencyType();
 
 #endif

From 0bb309ea5cea100692c81cb926c75ff11658b17a Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Mon, 17 Jun 2013 23:42:10 -0700
Subject: [PATCH 08/10] Make leak detection trip a software "circuit breaker"

If there is a leak, won't turn the sub back on until mbed reset.
---
 src/analog.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/analog.cpp b/src/analog.cpp
index db32d5d..0758ab8 100644
--- a/src/analog.cpp
+++ b/src/analog.cpp
@@ -95,7 +95,12 @@ LeakDetector::~LeakDetector() {
 
 void updateLeak()
 {
-	hasLeak = leakDetector.getValueThresh();
+	// If we already detected water, we don't want to enable again even if the
+	// water has moved to a different part of the sub.
+	if (!hasLeak)
+	{
+		hasLeak = leakDetector.getValueThresh();
+	}
 }
 
 bool LeakDetector::getValueThresh()

From 50eed276b58f65ff56823b13035158000e04b478 Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Mon, 17 Jun 2013 23:52:59 -0700
Subject: [PATCH 09/10] Ignore this commit.

I am bad at programming.
---
 src/safety.cpp | 6 +++---
 src/safety.h   | 4 ++--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/safety.cpp b/src/safety.cpp
index 1502c04..4ae0b56 100644
--- a/src/safety.cpp
+++ b/src/safety.cpp
@@ -8,14 +8,14 @@ enum EmergencyTypes {
 	LEAK,
 	VOLTAGE_LOW,
 	VOLTAGE_HIGH	// not used
-}
+};
 
-inline bool needStop()
+bool needStop()
 {
 	return (!isAlive | hasLeak | isLowVoltage);
 }
 
-inline int getEmergencyType()
+int getEmergencyType()
 {
 	if (hasLeak)
 	{
diff --git a/src/safety.h b/src/safety.h
index 65bdbec..d90e32d 100644
--- a/src/safety.h
+++ b/src/safety.h
@@ -5,7 +5,7 @@
 
 #include "analog.h"
 
-inline bool needStop();
-inline int getEmergencyType();
+bool needStop();
+int getEmergencyType();
 
 #endif

From 09aa19fb324a6b0f0d656203497ef2c1efa72237 Mon Sep 17 00:00:00 2001
From: Kevin Chen <kevinchen2003@gmail.com>
Date: Tue, 18 Jun 2013 16:18:56 -0700
Subject: [PATCH 10/10] Change leak threshold back to 0.85

This almost eliminates false positives when the analogin pin is floating
---
 src/global.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/global.cpp b/src/global.cpp
index ec73163..158339c 100644
--- a/src/global.cpp
+++ b/src/global.cpp
@@ -11,11 +11,11 @@
 #include "pc.h"
 
 //analoginput.h
-// TODO: check the magic threshold 0.85f. Too high/low?
+// Analog inputs' thresholds need to be around 0.85 because when the mbed's
+// AnalogIn pins are floating, the noise may make them oscillate between 0.0 and
+// 0.6. This avoids false positives/negatives when we're testing out of the sub.
 AnalogKillSwitch kill(p16, p18, 0.85f); // Vin, Vout
-
-// Threshold determined experimentally with tap water
-LeakDetector leakDetector(p20, 0.5f); //pin 20
+LeakDetector leakDetector(p20, 0.85f); //pin 20
 
 /* 
  * Daniel's was depth = adc_buffer[ADC_PRESS] * 0.361904762 - 101.33333;