From cce67f7659a6389e3af7a797d5badc4f2e7a0969 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Fri, 31 Aug 2018 17:49:59 +0200
Subject: [PATCH] Fix stop function in Servo driver

---
 main.cpp              |  7 +------
 src/drivers/servo.cpp | 17 +++++++----------
 2 files changed, 8 insertions(+), 16 deletions(-)

diff --git a/main.cpp b/main.cpp
index 512a906..0093ef6 100644
--- a/main.cpp
+++ b/main.cpp
@@ -124,9 +124,6 @@ int main() {
     int wmi = imu.whoami();
     int scale = imu.set_acc_scale(BITS_FS_16G);
 
-    // Stop the sweeping arm and do some other stuff with it
-    servo.stop();
-
     Thread::wait(1000);
 
     servo.nod();
@@ -135,9 +132,7 @@ int main() {
 
     servo.sweep(1.0, 1.5, 3);
 
-    servo.stop();
-
-    servo.setPosition(-1);
+    servo.setPosition(-0.5);
 
     while (true) 
     {
diff --git a/src/drivers/servo.cpp b/src/drivers/servo.cpp
index f7bd065..23af624 100644
--- a/src/drivers/servo.cpp
+++ b/src/drivers/servo.cpp
@@ -72,8 +72,6 @@ Servo::Servo(PinName servoArm)
 
 void Servo::center()
 {
-    //stop();
-    
     setPosition(0.0);
 }
 
@@ -84,10 +82,7 @@ void Servo::resetTimers()
 
 void Servo::setPosition(double position)
 {
-    /* TODO: For now we need to call "stop()" 
-    before setting pos when we know we've ran a thread before
-    Stop does not work here for some reason */
-    //stop(); 
+    stop(); 
 
     double increment = position*500.0;
     
@@ -112,6 +107,8 @@ void Servo::invert()
 
 void Servo::sweep(double from, double to, double intervalInSec)
 {
+    stop();
+
     pServoThread = new Thread;
 
     SweepData sweepData;
@@ -126,6 +123,8 @@ void Servo::sweep(double from, double to, double intervalInSec)
 
 void Servo::nod()
 {   
+    stop();
+
     NodData nodData;
     nodData.pServo = &m_servo;
     nodData.latestPosition = m_latestPosition;
@@ -139,13 +138,11 @@ void Servo::nod()
 
 void Servo::stop()
 {
-    if (pServoThread->get_state() != Thread::Deleted)
+    if (pServoThread != NULL && pServoThread->get_state() != Thread::Deleted)
     {
         pServoThread->terminate();
-    }
-    if (pServoThread != NULL)
-    {
         delete pServoThread;
+        pServoThread = NULL;
     }
 }