From 8c2f067520cdedc51a0064a84de76346d5404bf1 Mon Sep 17 00:00:00 2001
From: Alena Kazakova <alenadanse@gmail.com>
Date: Mon, 27 Nov 2017 19:33:32 +0900
Subject: [PATCH] Update robot.c

---
 src/lib/robot.c | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/lib/robot.c b/src/lib/robot.c
index 20042db..e1b6eef 100644
--- a/src/lib/robot.c
+++ b/src/lib/robot.c
@@ -13,6 +13,10 @@ volatile int goingBW = 0;
 /* Track current speed */
 volatile int curr_speedA;
 volatile int curr_speedB;
+/* Track previous direction*/
+volatile char prev_dir;
+/* Track current direction*/
+volatile char curr_dir;
 
 /* Setup function */
 void setup(void)
@@ -73,6 +77,7 @@ void move(char dir, int speedA, int speedB)
 /* Motor state control with gradual deceleration */
 void move_slow(char dir, int speedA, int speedB)
 {
+	/*
 	if (getFlag(dir)) {
 		if ((curr_speedA > MINSPEED) && (curr_speedB > MINSPEED)){
 			curr_speedA-=1;
@@ -87,6 +92,23 @@ void move_slow(char dir, int speedA, int speedB)
 		setSpeed(curr_speedA, curr_speedB);
 		setDir(dir);
 	}
+	*/
+
+	if (curr_dir == prev_dir) {
+		prev_dir = dir;
+		if ((curr_speedA > MINSPEED) && (curr_speedB > MINSPEED)){
+			curr_speedA-=1;
+			curr_speedB-=1;
+		}
+		setSpeed(curr_speedA, curr_speedB);
+	}
+	else {	
+		curr_dir = dir;
+		curr_speedA = speedA;
+		curr_speedB = speedB;
+		setSpeed(curr_speedA, curr_speedB);
+		setDir(dir);
+	}
 }
 
 /* Initialize ultrasonic sensor */