diff --git a/include/stallDetectionHeader.h b/include/stallDetectionHeader.h
new file mode 100644
index 0000000..c9a634c
--- /dev/null
+++ b/include/stallDetectionHeader.h
@@ -0,0 +1,218 @@
+#pragma once
+#include <ros/ros.h>
+//#include <chrono>
+#include <boost/chrono.hpp>
+#include <iostream>
+#include <deque>
+#include <queue>
+#include <math.h>
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Bool.h>
+#include <geometry_msgs/Pose2D.h>
+#include <sensor_msgs/PointCloud.h>
+#include "boost/date_time/posix_time/posix_time.hpp"
+#include <string.h>
+
+
+
+class StallDetection
+{
+private:
+	ros::NodeHandle n;
+	ros::Publisher stallVelocityPub;
+	ros::Publisher navigationPIDStatusPub;
+	ros::Subscriber robotCurrentVelocitySub;
+	ros::Subscriber expectedVelocitySub;
+	ros::Subscriber actualVelocitySub;
+	ros::Subscriber eStopStatusSub;
+	int queueSize;
+	deque<geometry_msgs::Pose2D> robotVelocityHistory;
+	bool disableNavigationPID;
+	bool eStopStatus;
+	geometry_msgs::Twist actualVelocity;
+	geometry_msgs::Twist expectedVelocity;
+	boost::posix_time::ptime stuckTime;
+	double maxSpeed;
+	bool stallDetected;
+	double overrideSpeed;
+	double reverseDurationInMilli;
+	//Initialize tolerance thresholds and time saving for Slip detection
+	double noMovementTolerance;// 10 cm
+	double noRotationTolerance; //5 degrees 
+	//DateTime lastStuckTime; //save last stuck time
+	//TimeSpan reverseDuration = new TimeSpan(0, 0, 0, 0, 800); // 0.5 Seconds
+	double reverseSpeed;
+	//Private Functions
+	double findMinElement(string returnType)
+	{
+		if (returnType == "x")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::min_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.x < b.x;
+			});
+			return (*it).x;
+		}
+		else if (returnType == "y")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::min_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.y < b.y;
+			});
+			return (*it).y;
+		}
+		else if (returnType == "theta")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.theta < b.theta;
+			});
+			return (*it).theta;
+		}
+		else
+			return 0.0;
+
+	}
+	double findMaxElement(string returnType)
+	{
+		if (returnType == "x")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.x < b.x;
+			});
+			return (*it).x;
+		}
+		else if (returnType == "y")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.y < b.y;
+			});
+			return (*it).y;
+		}
+		else if (returnType == "theta")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.theta < b.theta;
+			});
+			return (*it).theta;
+		}
+		else
+			return 0.0;
+	}
+
+	void robotPositionCallback(const geometry_msgs::Pose2D::ConstPtr& robotLocation)
+	{
+		addVelocityToHistory(*robotLocation);
+	}
+	void eStopStatusCallback(const std_msgs::Bool::ConstPtr& gpio)
+	{
+		//eStopStatusCallback = gpio->eStop;
+	}
+	void expectedRobotVelocityCallback(const geometry_msgs::Twist::ConstPtr& velocity)
+	{
+		expectedVelocity = *velocity;
+	}
+	void actualRobotVelocityCallback(const geometry_msgs::Twist::ConstPtr& velocity)
+	{
+		actualVelocity = *velocity;
+	}
+public:
+	StallDetection()
+	{
+		robotCurrentVelocitySub = n.subscribe("/localization/robot_location", 100, &StallDetection::robotPositionCallback, this);
+		actualVelocitySub = n.subscribe("/localization/velocity", 100, &StallDetection::expectedRobotVelocityCallback, this);
+		expectedVelocitySub = n.subscribe("/obstacle_reaction/velocity", 100, &StallDetection::expectedRobotVelocityCallback, this);
+		eStopStatusSub = n.subscribe("gpio/inputs", 1, &StallDetection::eStopStatusCallback, this);
+		n.param("robot_velocity_queue_size", queueSize, 100);
+		n.param("slip_detection_noMovementTolerance", noMovementTolerance, 0.1);// 10 cm
+		n.param("slip_detection_noRotationTolerance", noRotationTolerance, 5.0); //5 degrees
+		n.param("slip_detection_reverseSpeed", reverseSpeed, -0.5);
+		n.param("stall_detection_maxSpeed", maxSpeed, 0.7);
+		n.param("stall_detection_reverse_duration", reverseDurationInMilli, 0.7);
+
+		stallDetected = false;
+		robotVelocityHistory.resize(queueSize);
+	}
+	bool getStallStatus()
+	{
+		return stallDetected;
+	}
+	double getReverseDurationInMilli()
+	{
+		return reverseDurationInMilli;
+	}
+	double getExpectedVelocity()
+	{
+		return expectedVelocity.linear.x;
+	}
+	double getMaxSpeed()
+	{
+		return maxSpeed;
+	}
+	double getReverseSpeed()
+	{
+		return reverseSpeed;
+	}
+	boost::posix_time::ptime getLastStuckTime()
+	{
+		return stuckTime;
+	}
+	void addVelocityToHistory(geometry_msgs::Pose2D robotCurrentVelocity)
+	{
+		if (robotVelocityHistory.size() > queueSize)
+		{
+			robotVelocityHistory.pop_front();
+		}
+		robotVelocityHistory.push_back(robotCurrentVelocity);
+
+		if (robotVelocityHistory.size() >= queueSize)
+		{
+			activateStallDetection();
+		}
+
+	}
+	void activateStallDetection()
+	{
+		double minX = findMinElement("x");
+		double maxX = findMaxElement("x");
+		double minY = findMinElement("y");
+		double maxY = findMaxElement("y");
+		double minHeading = findMinElement("theta");
+		double maxHeading = findMaxElement("theta");
+
+		if ((pow(maxX - minX, 2) + pow(maxY - minY, 2) < noMovementTolerance)
+			&& maxHeading - minHeading < noRotationTolerance && eStopStatus &&
+			(expectedVelocity.linear.x != actualVelocity.linear.x) || (expectedVelocity.angular.z != actualVelocity.angular.z))
+		{
+			stallDetected = true;
+			ptime t(second_clock::local_time());//save the time which slipping was detected 
+			stuckTime = t;
+			robotVelocityHistory.clear();// clear the QUeue so that stall detection cannot occur again until the queue is full
+		}
+		else
+			stallDetected = false;
+	}
+
+	//to be called in drive_mode_switch
+	void doBackup(geometry_msgs::Twist &stallVelocity)
+	{
+		//This is the part saying to backup
+					//Send Reverse Speed
+		double reverseSpeed = getMaxSpeed() * getReverseSpeed();
+		stallVelocity.linear.x = reverseSpeed;
+		stallVelocity.angular.z = 0.0;
+
+		//TODO: try accelerating forward at different rates via PID, 
+	}
+};
+
+
diff --git a/include/stall_detection_header.h b/include/stall_detection_header.h
new file mode 100644
index 0000000..215b3be
--- /dev/null
+++ b/include/stall_detection_header.h
@@ -0,0 +1,218 @@
+#pragma once
+#include <ros/ros.h>
+//#include <chrono>
+#include <boost/chrono.hpp>
+#include <iostream>
+#include <deque>
+#include <queue>
+#include <math.h>
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Bool.h>
+#include <geometry_msgs/Pose2D.h>
+#include <sensor_msgs/PointCloud.h>
+#include "boost/date_time/posix_time/posix_time.hpp"
+#include <string.h>
+
+
+
+class StallDetection
+{
+private:
+	ros::NodeHandle n;
+	ros::Publisher stallVelocityPub;
+	ros::Publisher navigationPIDStatusPub;
+	ros::Subscriber robotCurrentVelocitySub;
+	ros::Subscriber expectedVelocitySub;
+	ros::Subscriber actualVelocitySub;
+	ros::Subscriber eStopStatusSub;
+	int queueSize;
+	deque<geometry_msgs::Pose2D> robotVelocityHistory;
+	bool disableNavigationPID;
+	bool eStopStatus;
+	geometry_msgs::Twist actualVelocity;
+	geometry_msgs::Twist expectedVelocity;
+	boost::posix_time::ptime stuckTime;
+	double maxSpeed;
+	bool stallDetected;
+	double overrideSpeed;
+	double reverseDurationInMilli;
+	//Initialize tolerance thresholds and time saving for Slip detection
+	double noMovementTolerance;// 10 cm
+	double noRotationTolerance; //5 degrees 
+	//DateTime lastStuckTime; //save last stuck time
+	//TimeSpan reverseDuration = new TimeSpan(0, 0, 0, 0, 800); // 0.5 Seconds
+	double reverseSpeed;
+	//Private Functions
+	double findMinElement(string returnType)
+	{
+		if (returnType == "x")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::min_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.x < b.x;
+			});
+			return (*it).x;
+		}
+		else if (returnType == "y")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::min_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.y < b.y;
+			});
+			return (*it).y;
+		}
+		else if (returnType == "theta")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.theta < b.theta;
+			});
+			return (*it).theta;
+		}
+		else
+			return 0.0;
+
+	}
+	double findMaxElement(string returnType)
+	{
+		if (returnType == "x")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.x < b.x;
+			});
+			return (*it).x;
+		}
+		else if (returnType == "y")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.y < b.y;
+			});
+			return (*it).y;
+		}
+		else if (returnType == "theta")
+		{
+			std::deque<geometry_msgs::Pose2D>::iterator it = std::max_element(robotVelocityHistory.begin(), robotVelocityHistory.end(),
+				[](geometry_msgs::Pose2D &a, geometry_msgs::Pose2D &b)
+			{
+				return a.theta < b.theta;
+			});
+			return (*it).theta;
+		}
+		else
+			return 0.0;
+	}
+
+	void robotPositionCallback(const geometry_msgs::Pose2D::ConstPtr& robotLocation)
+	{
+		addVelocityToHistory(*robotLocation);
+	}
+	void eStopStatusCallback(const std_msgs::Bool::ConstPtr& gpio)
+	{
+		//eStopStatusCallback = gpio->eStop;
+	}
+	void expectedRobotVelocityCallback(const geometry_msgs::Twist::ConstPtr& velocity)
+	{
+		expectedVelocity = *velocity;
+	}
+	void actualRobotVelocityCallback(const geometry_msgs::Twist::ConstPtr& velocity)
+	{
+		actualVelocity = *velocity;
+	}
+public:
+	StallDetection()
+	{
+		robotCurrentVelocitySub = n.subscribe("/localization/robot_location", 100, &StallDetection::robotPositionCallback, this);
+		actualVelocitySub = n.subscribe("/localization/velocity", 100, &StallDetection::expectedRobotVelocityCallback, this);
+		expectedVelocitySub = n.subscribe("/obstacle_reaction/velocity", 100, &StallDetection::expectedRobotVelocityCallback, this);
+		eStopStatusSub = n.subscribe("gpio/inputs", 1, &StallDetection::eStopStatusCallback, this);
+		n.param("robot_velocity_queue_size", queueSize, 100);
+		n.param("slip_detection_noMovementTolerance", noMovementTolerance, 0.1);// 10 cm
+		n.param("slip_detection_noRotationTolerance", noRotationTolerance, 5.0); //5 degrees
+		n.param("slip_detection_reverseSpeed", reverseSpeed, -0.5);
+		n.param("stall_detection_maxSpeed", maxSpeed, 0.7);
+		n.param("stall_detection_reverse_duration", reverseDurationInMilli, 0.7);
+
+		stallDetected = false;
+		robotVelocityHistory.resize(queueSize);
+	}
+	bool getStallStatus()
+	{
+		return stallDetected;
+	}
+	double getReverseDurationInMilli()
+	{
+		return reverseDurationInMilli;
+	}
+	double getExpectedVelocity()
+	{
+		return expectedVelocity.linear.x;
+	}
+	double getMaxSpeed()
+	{
+		return maxSpeed;
+	}
+	double getReverseSpeed()
+	{
+		return reverseSpeed;
+	}
+	boost::posix_time::ptime getLastStuckTime()
+	{
+		return stuckTime;
+	}
+	void addVelocityToHistory(geometry_msgs::Pose2D robotCurrentVelocity)
+	{
+		if (robotVelocityHistory.size() > queueSize)
+		{
+			robotVelocityHistory.pop_front();
+		}
+		robotVelocityHistory.push_back(robotCurrentVelocity);
+
+		if (robotVelocityHistory.size() >= queueSize)
+		{
+			activateStallDetection();
+		}
+
+	}
+	void activateStallDetection()
+	{
+		double minX = findMinElement("x");
+		double maxX = findMaxElement("x");
+		double minY = findMinElement("y");
+		double maxY = findMaxElement("y");
+		double minHeading = findMinElement("theta");
+		double maxHeading = findMaxElement("theta");
+
+		if ((pow(maxX - minX, 2) + pow(maxY - minY, 2) < noMovementTolerance)
+			&& maxHeading - minHeading < noRotationTolerance && eStopStatus &&
+			(expectedVelocity.linear.x != actualVelocity.linear.x) || (expectedVelocity.angular.z != actualVelocity.angular.z))
+		{
+			stallDetected = true;
+			ptime t(second_clock::local_time());//save the time which slipping was detected 
+			stuckTime = t;
+			robotVelocityHistory.clear();// clear the QUeue so that stall detection cannot occur again until the queue is full
+		}
+		else
+			stallDetected = false;
+	}
+
+	//to be called in drive_mode_switch
+	void doBackup(geometry_msgs::Twist &stallVelocity)
+	{
+		//This is the part saying to backup
+					//Send Reverse Speed
+		double reverseSpeed = getMaxSpeed() * getReverseSpeed();
+		stallVelocity.linear.x = reverseSpeed;
+		stallVelocity.angular.z = 0.0;
+
+		//TODO: try accelerating forward at different rates via PID, 
+	}
+};
+
+
diff --git a/src/drive_mode_switch.cpp b/src/drive_mode_switch.cpp
index a66706f..a345388 100644
--- a/src/drive_mode_switch.cpp
+++ b/src/drive_mode_switch.cpp
@@ -1,23 +1,44 @@
 #include <ros/ros.h>
-#include <std_msgs/String.h>
+#include <_msgs/String.h>
 #include <geometry_msgs/Twist.h>
 #include <isc_joy/xinput.h>
+#include "stallDetectionHeader.h"
 
 #include <string>
 
+//imports for stall detection
+#include <boost/chrono.hpp>
+#include <iostream>
+#include <deque>
+#include <queue>
+#include <math.h>
+#include <std_msgs/Bool.h>
+#include <geometry_msgs/Pose2D.h>
+#include <sensor_msgs/PointCloud.h>
+#include "boost/date_time/posix_time/posix_time.hpp"
+#include <string.h>
+
+//https://stackoverflow.com/a/4974588 but use boost instead of chrono
+
+typedef boost::chrono::high_resolution_clock Clock;
+typedef boost::chrono::milliseconds Milliseconds;
+
 /*
 	Manual mode is when the robot is getting direct input from the joystick
 	Autonomous mode activates self navigation
+	Stalling is for when the tires slip or when the robot otherwise can't drive forward
 */
 namespace drive_mode { // <enum>::<identifier> doesn't compile outside of MSVC. i did not know that. -matt
 	enum state {
 		MANUAL,
-		AUTONOMOUS
+		AUTONOMOUS,
+		STALLING
 	};
-	
+
 	std::string drive_mode_to_string(drive_mode::state mode) {
-		switch(mode) {
+		switch (mode) {
 		case AUTONOMOUS: return "auto";
+		case STALLING: return "stall";
 		case MANUAL:
 		default: return "manual";
 		}
@@ -36,12 +57,17 @@ std::string normal_state;
 
 ros::Publisher control_pub;
 ros::Publisher drive_mode_pub;
+ros::Publisher stallVelocityPub
 
 ros::Subscriber state_sub;
 ros::Subscriber auto_sub;
 ros::Subscriber manual_sub;
+ros::Subscriber stall_sub;
 ros::Subscriber joystick_sub;
 
+StallDetection stallDetector;
+geometry_msgs::Twist stallVelocity;
+
 // functions
 
 void publish_drive_mode() {
@@ -51,14 +77,17 @@ void publish_drive_mode() {
 }
 
 void joystick_cb(const isc_joy::xinput::ConstPtr &joy) {
-	if(joy->Start) {
+	if (joy->Start) {
 		start_pressed = true;
-	} else if (start_pressed && !joy->Start) {
+	}
+	else if (start_pressed && !joy->Start) {
 		start_pressed = false;
 
-		if (mode == drive_mode::AUTONOMOUS) {
+		//if the robot is in autonomous mode or stalling and we press start, switch to manual
+		if (mode == drive_mode::AUTONOMOUS || mode == drive_mode::STALLING) {
 			mode = drive_mode::MANUAL;
-		} else if (mode == drive_mode::MANUAL && robot_state == normal_state) {
+		}
+		else if (mode == drive_mode::MANUAL && robot_state == normal_state) {
 			mode = drive_mode::AUTONOMOUS;
 		}
 
@@ -87,6 +116,45 @@ void manual_control_cb(const geometry_msgs::Twist::ConstPtr &control) {
 	}
 }
 
+void stall_control_cb(const geometry_msgs::Twist::ConstPtr &control) {
+	if (mode == drive_mode::STALLING) {
+		control_pub.publish(*control);
+
+	}
+
+}
+
+//Above function overridden, switching back to autonomous mode
+//This should only be called when in AUTONOMOUS or STALLING modes
+void stall_control_cb() {
+
+	//prevent EStop from backing up with this
+		//TODO: delete once EStop bool exists
+		bool EStop = false;
+
+	//check if we're stalling
+	if ( (mode == drive_mode::STALLING)  && (mode != drive_mode::MANUAL) ) {
+		//if we are, try going forward again
+		//if we're going slower than we expect or we're estopped...
+		if ( (EStop) || (stallDetector.getExpectedVelocity() <= stallDetector.getMaxSpeed() ) )//if we are, try going forward again
+		{
+			//TODO: test getExpectedVelocity in EStop scenarios for backups
+
+			//if we can go forward, switch back to drive_mode::AUTONOMOUS;
+			//ie, do nothing
+		}
+		//otherwise back up, then switch back to drive_mode::AUTONOMOUS;
+		else
+		{
+			stallDetector.doBackup(stallVelocity);
+
+		}
+		mode = drive_mode::AUTONOMOUS;
+	}
+
+	publish_drive_mode();
+}
+
 // main
 
 int main(int argc, char **argv) {
@@ -106,8 +174,35 @@ int main(int argc, char **argv) {
 	joystick_sub = nh.subscribe("joystick", 1, &joystick_cb);
 	auto_sub = nh.subscribe("auto_control_vel", 1, &auto_control_cb);
 	manual_sub = nh.subscribe("manual_control_vel", 1, &manual_control_cb);
-
-	ros::spin();
+	stall_sub = nh.subscribe("stall_control_vel", 1, &stall_control_cb);
+
+	//stall detection
+	{
+		ros::NodeHandle handledNode;
+		double reverseDurationInMilli = stallDetector.getReverseDurationInMilli();
+		boost::posix_time::time_duration td = milliseconds(stallDetector.reverseDurationInMilli);
+		stallVelocityPub = handledNode.advertise<geometry_msgs::Twist>("/stall/velocity", 1000);
+
+		while (ros::ok())
+		{
+			//spinOnce is a callback function that cycles through all
+			//the callback functions waiting to be called in the queue 
+			ros::spin();
+			boost::posix_time::ptime lastStuckTime(stallDetector.getLastStuckTime() + td);
+			boost::posix_time::ptime now(second_clock::local_time());
+			if (stallDetector.getStallStatus())
+			{
+				if (lastStuckTime >= now)
+				{
+					mode = drive_mode::STALLING;
+					stall_control_cb();
+					stallDetector.doBackup(stallVelocity);
+					stallVelocityPub.publish(stallVelocity);
+				}
+			}
+		}
+		
+	}
 
 	return 0;
 }