diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 57a6dde..df1d947 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -1,3 +1,11 @@
+0.4.0
+---
+* Software Servo code added
+  - Only works on the LCD and CSI pins
+* Fixed cleanup() for the SOFTPWM and SERVO
+  - The per pin cleanup for SOFTPWM doesn't work as stop() clears up the memory for the pin used
+  - SERVO code was based on SOFTPWM, so it inherited this issue
+
 0.3.5
 ---
 * Merged in brettcvz's code to read a byte of data from the GPIO
diff --git a/README.rst b/README.rst
index b1abb7e..f13cc18 100644
--- a/README.rst
+++ b/README.rst
@@ -285,10 +285,8 @@ Hardware PWM requires a DTB Overlay loaded on the CHIP to allow the kernel to kn
     SPWM.set_frequency("XIO-P7", 10)
     # To Stop SPWM
     SPWM.stop("XIO-P7")
-    # Cleanup can have no argument to clean up all SoftPWM outputs
+    # Cleanup
     SPWM.cleanup()
-    # Or you can specify a single SoftPWM output to cleanup (keeping the rest intact)
-    SPWM.cleanup("XIO-P7")
     #For specific polarity: this example sets polarity to 1 on start:
     SPWM.start("XIO-P7", 50, 2000, 1)
 
@@ -296,6 +294,24 @@ Use SOFTPWM at low speeds (hundreds of Hz) for the best results. Do not use for
 
 If using SOFTPWM and PWM at the same time, import CHIP_IO.SOFTPWM as SPWM or something different than PWM as to not confuse the library.
 
+**SERVO**::
+
+    import CHIP_IO.SERVO as SERVO
+    # Enable/Disable Debug
+    SERVO.toggle_debug()
+    #SPWM.start(channel, angle=0, range=180)
+    #angle values are between +/- range/2)
+    #you can choose any pin except the XIO's
+    SERVO.start("CSID4", 50)
+    SERVO.set_angle("CSID4", 25.5)
+    SERVO.set_range("CSID4", 90)
+    # To Stop Servo
+    SERVO.stop("CSID4")
+    # Cleanup
+    SERVO.cleanup()
+
+The Software Servo control only works on the LCD and CSI pins.  The XIO is too slow to control.
+
 **LRADC**::
 
 The LRADC was enabled in the 4.4.13-ntc-mlc.  This is a 6 bit ADC that is 2 Volt tolerant.
diff --git a/setup.py b/setup.py
index 33b37a6..c78f4c2 100644
--- a/setup.py
+++ b/setup.py
@@ -20,7 +20,7 @@ classifiers = ['Development Status :: 3 - Alpha',
                'Topic :: System :: Hardware']
 
 setup(name             = 'CHIP_IO',
-      version          = '0.3.5',
+      version          = '0.4.0',
       author           = 'Robert Wolterman',
       author_email     = 'robert.wolterman@gmail.com',
       description      = 'A module to control CHIP IO channels',
@@ -32,5 +32,6 @@ setup(name             = 'CHIP_IO',
       packages         = find_packages(),
       ext_modules      = [Extension('CHIP_IO.GPIO', ['source/py_gpio.c', 'source/event_gpio.c', 'source/constants.c', 'source/common.c'], extra_compile_args=['-Wno-format-security']),
                           Extension('CHIP_IO.PWM', ['source/py_pwm.c', 'source/c_pwm.c', 'source/constants.c', 'source/common.c'], extra_compile_args=['-Wno-format-security']),
-                          Extension('CHIP_IO.SOFTPWM', ['source/py_softpwm.c', 'source/c_softpwm.c', 'source/constants.c', 'source/common.c', 'source/event_gpio.c'], extra_compile_args=['-Wno-format-security'])]) #,
+                          Extension('CHIP_IO.SOFTPWM', ['source/py_softpwm.c', 'source/c_softpwm.c', 'source/constants.c', 'source/common.c', 'source/event_gpio.c'], extra_compile_args=['-Wno-format-security']),
+                          Extension('CHIP_IO.SERVO', ['source/py_servo.c', 'source/c_softservo.c', 'source/constants.c', 'source/common.c', 'source/event_gpio.c'], extra_compile_args=['-Wno-format-security'])]) #,
 #                          Extension('CHIP_IO.ADC', ['source/py_adc.c', 'source/c_adc.c', 'source/constants.c', 'source/common.c'], extra_compile_args=['-Wno-format-security']),
diff --git a/source/c_softpwm.c b/source/c_softpwm.c
index 2e428c7..92078ea 100644
--- a/source/c_softpwm.c
+++ b/source/c_softpwm.c
@@ -39,7 +39,7 @@ SOFTWARE.
 #include <unistd.h>
 #include <pthread.h>
 #include <time.h>
-#include "c_pwm.h"
+#include "c_softpwm.h"
 #include "common.h"
 #include "event_gpio.h"
 
diff --git a/source/c_softservo.c b/source/c_softservo.c
new file mode 100644
index 0000000..1c3ee11
--- /dev/null
+++ b/source/c_softservo.c
@@ -0,0 +1,355 @@
+/*
+Copyright (c) 2017 Robert Wolterman
+
+Using CHIP_IO servo code from Brady Hurlburt as a basis for the servo code
+
+Copyright (c) 2016 Brady Hurlburt
+
+Original BBIO Author Justin Cooper
+Modified for CHIP_IO Author Brady Hurlburt
+
+This file incorporates work covered by the following copyright and
+permission notice, all modified code adopts the original license:
+
+Copyright (c) 2013 Adafruit
+Author: Justin Cooper
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sys/types.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <time.h>
+#include "c_softservo.h"
+#include "common.h"
+#include "event_gpio.h"
+
+#define KEYLEN 7
+
+#define PERIOD 0
+#define DUTY 1
+
+#define MSTONS 1000000
+#define BLOCKNS 20 * MSTONS
+#define FUDGEFACTOR 450
+#define MSTOMICROS 1000
+#define MICROSTONS 1000
+#define MINMICROS 1000
+#define NEUTRAL 1500
+#define MAXMICROS 2000
+
+int servo_initialized = 0;
+
+struct servo_params
+{
+    float range;
+    float min_angle;
+    float max_angle;
+    float current_angle;
+    bool enabled;
+    bool stop_flag;
+};
+
+struct servo
+{
+    char key[KEYLEN+1]; /* leave room for terminating NUL byte */
+    int gpio;
+    struct servo_params params;
+    pthread_mutex_t* params_lock;
+    pthread_t thread;
+    struct servo *next;
+};
+struct servo *exported_servos = NULL;
+
+struct servo *lookup_exported_servo(const char *key)
+{
+    struct servo *srv = exported_servos;
+
+    while (srv != NULL)
+    {
+        if (strcmp(srv->key, key) == 0) {
+            return srv;
+        }
+        srv = srv->next;
+    }
+
+    return NULL; /* standard for pointers */
+}
+
+void *servo_thread_toggle(void *arg)
+{
+    struct servo *srv = (struct servo *)arg;
+    int gpio = srv->gpio;
+    struct timespec tim_on;
+    struct timespec tim_off;
+    float on_time_microsec = 0;
+    unsigned int on_ns;
+    unsigned int off_ns;
+
+    /* Used to determine if something has
+     * has changed
+    */
+    float angle_local = 0;
+    float range_local = 0;
+    bool stop_flag_local = false;
+    bool enabled_local = false;
+    bool recalculate_timing = false;
+
+    while (!stop_flag_local) {
+        /* Take a snapshot of the parameter block */
+        pthread_mutex_lock(srv->params_lock);
+        if ((angle_local != srv->params.current_angle) || (range_local != srv->params.range)) {
+            recalculate_timing = true;
+        }
+        angle_local = srv->params.current_angle;
+        range_local = srv->params.range;
+        enabled_local = srv->params.enabled;
+        stop_flag_local = srv->params.stop_flag;
+        pthread_mutex_unlock(srv->params_lock);
+
+        /* If freq or duty has been changed, update the
+         * sleep times
+        */
+        if (recalculate_timing) {
+            if (DEBUG)
+                printf(" ** servo updating timing: new angle: (%.2f)\n",angle_local);
+            on_time_microsec = (((MAXMICROS - MINMICROS) / range_local) * angle_local) + NEUTRAL;
+            on_ns = (unsigned long)(on_time_microsec * MICROSTONS - FUDGEFACTOR);
+            off_ns = BLOCKNS - on_ns;
+            tim_on.tv_sec = 0;
+            tim_on.tv_nsec = on_ns;
+            tim_off.tv_sec = 0;
+            tim_off.tv_nsec = off_ns;
+            recalculate_timing = false;
+        }
+
+        if (enabled_local)
+        {
+            /* Set gpio */
+            gpio_set_value(gpio, HIGH);
+
+            nanosleep(&tim_on, NULL);
+
+            /* Unset gpio */
+            gpio_set_value(gpio, LOW);
+
+            nanosleep(&tim_off, NULL);
+            //printf("AFTER SECOND NANOSLEEP\n");
+        }  /* if enabled_local */
+    }  /* while !stop_flag_local */
+
+    gpio_set_value(gpio, LOW);
+
+    /* This servo has been disabled */
+    pthread_exit(NULL);
+}
+
+int servo_start(const char *key, float angle, float range)
+{
+    struct servo *new_srv, *srv;
+    pthread_t new_thread;
+    pthread_mutex_t *new_params_lock;
+    int gpio;
+    int ret;
+
+    if (get_gpio_number(key, &gpio) < 0) {
+        if (DEBUG)
+            printf(" ** servo_start: invalid gpio specified **\n");
+        return -1;
+    }
+    
+    if (gpio_export(gpio) < 0) {
+       char err[2000];
+       snprintf(err, sizeof(err), "Error setting up servo on pin %d, maybe already exported? (%s)", gpio, get_error_msg());
+       add_error_msg(err);
+       return -1;
+    }
+    
+    if (DEBUG)
+        printf(" ** servo_start: %d exported **\n", gpio);
+    
+    if (gpio_set_direction(gpio, OUTPUT) < 0) {
+        if (DEBUG)
+            printf(" ** servo_start: gpio_set_direction failed **\n");
+        return -1;
+    }
+
+    printf("c_softservo.c: servo_start(%d,%.2f,%.2f)\n",gpio,angle,range);
+    
+    // add to list
+    new_srv = malloc(sizeof(struct servo));  ASSRT(new_srv != NULL);
+    new_params_lock = (pthread_mutex_t *)malloc(sizeof(pthread_mutex_t));
+    if (new_srv == 0) {
+        return -1; // out of memory
+    }
+    pthread_mutex_init(new_params_lock, NULL);
+    pthread_mutex_lock(new_params_lock);
+
+    strncpy(new_srv->key, key, KEYLEN);  /* can leave string unterminated */
+    new_srv->key[KEYLEN] = '\0'; /* terminate string */
+    new_srv->gpio = gpio;
+    new_srv->params.enabled = true;
+    new_srv->params.stop_flag = false;
+    new_srv->params_lock = new_params_lock;
+    new_srv->next = NULL;
+
+    if (exported_servos == NULL)
+    {
+        // create new list
+        exported_servos = new_srv;
+    } else {
+        // add to end of existing list
+        srv = exported_servos;
+        while (srv->next != NULL)
+            srv = srv->next;
+        srv->next = new_srv;
+    }
+    pthread_mutex_unlock(new_params_lock);
+
+    if (DEBUG)
+        printf(" ** servo_enable: setting servo parameters **\n");
+    ASSRT(servo_set_range(new_srv->key, range) == 0);
+    ASSRT(servo_set_angle(new_srv->key, angle) == 0);
+
+    pthread_mutex_lock(new_params_lock);
+    // create thread for srv
+    if (DEBUG)
+        printf(" ** servo_enable: creating thread **\n");
+    ret = pthread_create(&new_thread, NULL, servo_thread_toggle, (void *)new_srv);
+    ASSRT(ret == 0);
+
+    new_srv->thread = new_thread;
+
+    pthread_mutex_unlock(new_params_lock);
+
+    return 1;
+}
+
+int servo_disable(const char *key)
+{
+    struct servo *srv, *temp, *prev_srv = NULL;
+
+    if (DEBUG)
+        printf(" ** in servo_disable **\n");
+    // remove from list
+    srv = exported_servos;
+    while (srv != NULL)
+    {
+        if (strcmp(srv->key, key) == 0)
+        {
+            if (DEBUG)
+                printf(" ** servo_disable: found pin **\n");
+            pthread_mutex_lock(srv->params_lock);
+            srv->params.stop_flag = true;
+            pthread_mutex_unlock(srv->params_lock);
+            pthread_join(srv->thread, NULL);  /* wait for thread to exit */
+
+            if (DEBUG)
+                printf(" ** servo_disable: unexporting %d **\n", srv->gpio);
+            gpio_unexport(srv->gpio);
+
+            if (prev_srv == NULL)
+            {
+                exported_servos = srv->next;
+                prev_srv = srv;
+            } else {
+                prev_srv->next = srv->next;
+            }
+
+            temp = srv;
+            srv = srv->next;
+            free(temp->params_lock);
+            free(temp);
+        } else {
+            prev_srv = srv;
+            srv = srv->next;
+        }
+    }
+    return 0;
+}
+
+int servo_set_range(const char *key, float range)
+{
+    struct servo *srv;
+    float min_angle, max_angle;
+
+    srv = lookup_exported_servo(key);
+
+    if (srv == NULL) {
+        return -1;
+    }
+
+    // Compute the min and max angle
+    max_angle = range / 2.0;
+    min_angle = -max_angle;
+
+    if (DEBUG)
+        printf(" ** servo_set_range(%d,%.2f = %.2f,%.2f)\n",srv->gpio,range,min_angle,max_angle);
+    pthread_mutex_lock(srv->params_lock);
+    srv->params.range = range;
+    srv->params.min_angle = min_angle;
+    srv->params.max_angle = max_angle;
+    pthread_mutex_unlock(srv->params_lock);
+
+    return 0;
+}
+
+int servo_set_angle(const char *key, float angle)
+{
+    struct servo *srv;
+
+    srv = lookup_exported_servo(key);
+
+    if (srv == NULL) {
+        return -1;
+    }
+
+    // Make sure we're between the range of allowable angles
+    if (angle < srv->params.min_angle || angle > srv->params.max_angle) {
+       char err[2000];
+       snprintf(err, sizeof(err), "Angle specified (%.2f) for pin %d, is outside allowable range (%.2f,%.2f)", angle, srv->gpio, srv->params.min_angle,srv->params.max_angle);
+       add_error_msg(err);
+       return -1;
+    }
+    
+    if (DEBUG)
+        printf(" ** servo_set_angle(%d,%.2f)\n",srv->gpio,angle);
+    pthread_mutex_lock(srv->params_lock);
+    srv->params.current_angle = angle;
+    pthread_mutex_unlock(srv->params_lock);
+
+    return 0;
+}
+
+void servo_cleanup(void)
+{
+    while (exported_servos != NULL) {
+        servo_disable(exported_servos->key);
+    }
+}
+
+
+
diff --git a/source/c_softservo.h b/source/c_softservo.h
new file mode 100644
index 0000000..5f45179
--- /dev/null
+++ b/source/c_softservo.h
@@ -0,0 +1,40 @@
+/*
+Copyright (c) 2017 Robert Wolterman
+
+Using CHIP_IO SoftPWM code from Brady Hurlburt as a basis for the servo code
+
+Copyright (c) 2016 Brady Hurlburt
+
+Original BBIO Author Justin Cooper
+Modified for CHIP_IO Author Brady Hurlburt
+
+This file incorporates work covered by the following copyright and
+permission notice, all modified code adopts the original license:
+
+Copyright (c) 2013 Adafruit
+Author: Justin Cooper
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+int servo_start(const char *key, float angle, float range);
+int servo_disable(const char *key);
+int servo_set_range(const char *key, float range);
+int servo_set_angle(const char *key, float angle);
+void servo_cleanup(void);
diff --git a/source/constants.c b/source/constants.c
index 1f73a35..fc25c6a 100644
--- a/source/constants.c
+++ b/source/constants.c
@@ -88,6 +88,6 @@ void define_constants(PyObject *module)
    module_debug = Py_BuildValue("i", DEBUG ? Py_True: Py_False);
    PyModule_AddObject(module, "DEBUG", module_debug);
 
-   version = Py_BuildValue("s", "0.3.5");
+   version = Py_BuildValue("s", "0.4.0");
    PyModule_AddObject(module, "VERSION", version);
 }
diff --git a/source/event_gpio.c b/source/event_gpio.c
index 7c07b6c..3f5e0eb 100644
--- a/source/event_gpio.c
+++ b/source/event_gpio.c
@@ -486,8 +486,8 @@ int gpio_set_value(int gpio, unsigned int value)
         strncpy(vstr, "0", ARRAY_SIZE(vstr) - 1);
     }
 
-    if (DEBUG)
-        printf(" ** gpio_set_value: writing %s **\n", vstr);
+    //if (DEBUG)
+    //    printf(" ** gpio_set_value: writing %s **\n", vstr);
 
     ssize_t s = write(fd, vstr, strlen(vstr));  e_no = errno;
 
diff --git a/source/py_servo.c b/source/py_servo.c
new file mode 100644
index 0000000..a3d6902
--- /dev/null
+++ b/source/py_servo.c
@@ -0,0 +1,258 @@
+/*
+Copyright (c) 2017 Robert Wolterman
+
+Using CHIP_IO servo code from Brady Hurlburt as a basis for the servo code
+
+Copyright (c) 2016 Brady Hurlburt
+
+Original BBIO Author Justin Cooper
+Modified for CHIP_IO Author Brady Hurlburt
+
+This file incorporates work covered by the following copyright and
+permission notice, all modified code adopts the original license:
+
+Copyright (c) 2013 Adafruit
+Author: Justin Cooper
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include "Python.h"
+#include "constants.h"
+#include "common.h"
+#include "c_softservo.h"
+
+// python function toggle_debug()
+static PyObject *py_toggle_debug(PyObject *self, PyObject *args)
+{
+    // toggle debug printing
+    toggle_debug();
+
+    Py_RETURN_NONE;
+}
+
+// python function cleanup()
+static PyObject *py_cleanup(PyObject *self, PyObject *args)
+{
+    // unexport the Servo
+    servo_cleanup();
+
+    Py_RETURN_NONE;
+}
+
+// python function start(channel, angle, range)
+static PyObject *py_start_channel(PyObject *self, PyObject *args, PyObject *kwargs)
+{
+    int gpio;
+    char key[8];
+    char *channel = NULL;
+    float angle = 0.0;
+    float range = 180.0;
+    static char *kwlist[] = {"channel", "angle", "range", NULL};
+
+    clear_error_msg();
+
+    if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s|ff", kwlist, &channel, &angle, &range)) {
+        return NULL;
+    }
+    ASSRT(channel != NULL);
+
+    if (!get_key(channel, key)) {
+        PyErr_SetString(PyExc_ValueError, "Invalid Servo key or name.");
+        return NULL;
+    }
+
+    // check to ensure gpio is one of the allowed pins
+    // Not protecting the call as if the get_key() fails, we won't make it here
+    get_gpio_number(channel, &gpio);
+    if ((gpio >= lookup_gpio_by_name("XIO-P0") && gpio <= lookup_gpio_by_name("XIO-P7"))) {
+        PyErr_SetString(PyExc_ValueError, "Servo currently not available on XIO-P0 to XIO-P7");
+        return NULL;
+    }
+
+    if (servo_start(key, angle, range) < 0) {
+       printf("servo_start failed");
+       char err[2000];
+       snprintf(err, sizeof(err), "Error starting servo on pin %s (%s)", key, get_error_msg());
+       PyErr_SetString(PyExc_RuntimeError, err);
+       return NULL;
+    }
+
+    Py_RETURN_NONE;
+}
+
+// python function stop(channel)
+static PyObject *py_stop_channel(PyObject *self, PyObject *args, PyObject *kwargs)
+{
+    int gpio;
+    char key[8];
+    char *channel;
+
+    clear_error_msg();
+
+    if (!PyArg_ParseTuple(args, "s", &channel))
+        return NULL;
+
+    if (!get_key(channel, key)) {
+        PyErr_SetString(PyExc_ValueError, "Invalid key or name");
+        return NULL;
+    }
+
+    // check to ensure gpio is one of the allowed pins
+    // Not protecting the call as if the get_key() fails, we won't make it here
+    get_gpio_number(channel, &gpio);
+    if ((gpio >= lookup_gpio_by_name("XIO-P0") && gpio <= lookup_gpio_by_name("XIO-P7"))) {
+        PyErr_SetString(PyExc_ValueError, "Servo currently not available on XIO-P0 to XIO-P7");
+        return NULL;
+    }
+
+    servo_disable(key);
+
+    Py_RETURN_NONE;
+}
+
+// python method SERVO.set_range(channel, duty_cycle)
+static PyObject *py_set_range(PyObject *self, PyObject *args, PyObject *kwargs)
+{
+    int gpio;
+    char key[8];
+    char *channel;
+    float range = 180.0;
+    static char *kwlist[] = {"channel", "range", NULL};
+
+    clear_error_msg();
+
+    if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s|f", kwlist, &channel, &range))
+        return NULL;
+
+    if (range > 360.0) {
+        PyErr_SetString(PyExc_ValueError, "range can not be greater than 360.0");
+        return NULL;
+    }
+
+    if (!get_key(channel, key)) {
+        PyErr_SetString(PyExc_ValueError, "Invalid key or name.");
+        return NULL;
+    }
+
+    // check to ensure gpio is one of the allowed pins
+    // Not protecting the call as if the get_key() fails, we won't make it here
+    get_gpio_number(channel, &gpio);
+    if ((gpio >= lookup_gpio_by_name("XIO-P0") && gpio <= lookup_gpio_by_name("XIO-P7"))) {
+        PyErr_SetString(PyExc_ValueError, "Servo currently not available on XIO-P0 to XIO-P7");
+        return NULL;
+    }
+
+    if (servo_set_range(key, range) == -1) {
+        PyErr_SetString(PyExc_RuntimeError, "You must start() the Servo channel first");
+        return NULL;
+    }
+
+    Py_RETURN_NONE;
+}
+
+// python method SERVO.set_angle(channel, duty_cycle)
+static PyObject *py_set_angle(PyObject *self, PyObject *args, PyObject *kwargs)
+{
+    int gpio;
+    char key[8];
+    char *channel;
+    float angle = 0.0;
+    static char *kwlist[] = {"channel", "angle", NULL};
+
+    clear_error_msg();
+
+    if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s|f", kwlist, &channel, &angle))
+        return NULL;
+
+    //if (range > 360.0) {
+    //    PyErr_SetString(PyExc_ValueError, "range can not be greater than 360.0");
+    //    return NULL;
+    //}
+
+    if (!get_key(channel, key)) {
+        PyErr_SetString(PyExc_ValueError, "Invalid key or name.");
+        return NULL;
+    }
+
+    // check to ensure gpio is one of the allowed pins
+    // Not protecting the call as if the get_key() fails, we won't make it here
+    get_gpio_number(channel, &gpio);
+    if ((gpio >= lookup_gpio_by_name("XIO-P0") && gpio <= lookup_gpio_by_name("XIO-P7"))) {
+        PyErr_SetString(PyExc_ValueError, "Servo currently not available on XIO-P0 to XIO-P7");
+        return NULL;
+    }
+
+    if (servo_set_angle(key, angle) == -1) {
+       char err[2000];
+       snprintf(err, sizeof(err), "Error setting servo angle on pin %s (%s)", key, get_error_msg());
+       PyErr_SetString(PyExc_RuntimeError, err);
+       return NULL;
+    }
+
+    Py_RETURN_NONE;
+}
+
+static const char moduledocstring[] = "Software Servo functionality of a CHIP using Python";
+
+PyMethodDef servo_methods[] = {
+    {"start", (PyCFunction)py_start_channel, METH_VARARGS | METH_KEYWORDS, "Set up and start the Servo.  channel can be in the form of 'XIO-P0', or 'U14_13'"},
+    {"stop", (PyCFunction)py_stop_channel, METH_VARARGS | METH_KEYWORDS, "Stop the Servo.  channel can be in the form of 'XIO-P0', or 'U14_13'"},
+    {"set_range", (PyCFunction)py_set_range, METH_VARARGS, "Change the servo range\nrange - max angular range of the servo" },
+    {"set_angle", (PyCFunction)py_set_angle, METH_VARARGS, "Change the servo angle\nangle - angle of the servo between +/-(range/2)" },
+    {"cleanup", (PyCFunction)py_cleanup, METH_VARARGS, "Clean up by resetting All or one Servo that have been used by this program."},
+    {"toggle_debug", py_toggle_debug, METH_VARARGS, "Toggles the enabling/disabling of Debug print output"},
+    {NULL, NULL, 0, NULL}
+};
+
+#if PY_MAJOR_VERSION > 2
+static struct PyModuleDef chipservomodule = {
+    PyModuleDef_HEAD_INIT,
+    "SERVO",       // name of module
+    moduledocstring,  // module documentation, may be NULL
+    -1,               // size of per-interpreter state of the module, or -1 if the module keeps state in global variables.
+    servo_methods
+};
+#endif
+
+#if PY_MAJOR_VERSION > 2
+PyMODINIT_FUNC PyInit_SERVO(void)
+#else
+PyMODINIT_FUNC initSERVO(void)
+#endif
+{
+    PyObject *module = NULL;
+
+#if PY_MAJOR_VERSION > 2
+    if ((module = PyModule_Create(&chipservomodule)) == NULL)
+       return NULL;
+#else
+    if ((module = Py_InitModule3("SERVO", servo_methods, moduledocstring)) == NULL)
+       return;
+#endif
+
+   define_constants(module);
+
+
+#if PY_MAJOR_VERSION > 2
+    return module;
+#else
+    return;
+#endif
+}
diff --git a/source/py_softpwm.c b/source/py_softpwm.c
index d1bd265..92470bc 100644
--- a/source/py_softpwm.c
+++ b/source/py_softpwm.c
@@ -43,29 +43,11 @@ static PyObject *py_toggle_debug(PyObject *self, PyObject *args)
     Py_RETURN_NONE;
 }
 
-// python function cleanup(channel=None)
+// python function cleanup()
 static PyObject *py_cleanup(PyObject *self, PyObject *args)
 {
     // unexport the PWM
-    char key[8];
-    char *channel = NULL;
-    
-    clear_error_msg();
-    
-    // Channel is optional
-    if (!PyArg_ParseTuple(args, "|s", &channel))
-        return NULL;
-                    
-    // The !channel fixes issue #50
-    if (channel == NULL || strcmp(channel, "\0") == 0) {
-        softpwm_cleanup();
-        return NULL;
-    } else {
-        if (!get_key(channel, key)) {
-            softpwm_cleanup();
-        }
-        softpwm_disable(key);
-    }
+    softpwm_cleanup();
 
     Py_RETURN_NONE;
 }
diff --git a/test/lradc_test.py b/test/lradctest.py
similarity index 98%
rename from test/lradc_test.py
rename to test/lradctest.py
index a948d36..6820373 100644
--- a/test/lradc_test.py
+++ b/test/lradctest.py
@@ -1,5 +1,3 @@
-#!/usr/bin/python
-
 import CHIP_IO.LRADC as ADC
 
 # == ENABLE DEBUG ==
diff --git a/test/servotest.py b/test/servotest.py
new file mode 100644
index 0000000..76439c0
--- /dev/null
+++ b/test/servotest.py
@@ -0,0 +1,71 @@
+import CHIP_IO.SERVO as SERVO
+import CHIP_IO.GPIO as GPIO
+import time
+import datetime
+import threading
+
+class ServoTestReceiver(threading.Thread):
+    def __init__(self,gpio,key,maxcount=20,sleeptime=0.005):
+        self.gpio = gpio
+        self.key = key
+        self.counter = 0
+        self.maxcount = maxcount
+        self.sleeptime = sleeptime
+        self.dead = False
+        threading.Thread.__init__(self)
+
+    def kill(self):
+        self.dead = True
+
+    def run(self):
+        print("SETTING UP RECEIVER GPIO")
+        self.gpio.cleanup()
+        self.gpio.setup(self.key, self.gpio.IN)
+        print("STARTING RECEIVE LOOP")
+        try:
+            #while self.counter < self.maxcount:
+            while not self.dead:
+                pwmval = self.gpio.input(self.key)
+                print("SERVO VALUE: {0} @ {1}".format(pwmval, datetime.datetime.now()))
+                time.sleep(self.sleeptime)
+                self.counter += 1
+        except KeyboardInterrupt:
+            self.gpio.cleanup(self.key)
+
+
+if __name__ == "__main__":
+    # SETUP VARIABLES
+    SERVOGPIO = "CSID1" #"XIO-P7"
+    RECEIVERGPIO = "CSID7"
+    COUNT = 120
+    SLEEPTIME = 0.0001
+    RANGE = 180
+
+    # SETUP PWM
+    try:
+        print("SERVO START")
+        SERVO.toggle_debug()
+        SERVO.start(SERVOGPIO, 0, RANGE)
+
+        # SETUP SERVO RECEIVER
+        #rcvr = ServoTestReceiver(GPIO, RECEIVERGPIO, COUNT, SLEEPTIME)
+        #rcvr.start()
+
+        # LOOP THROUGH RANGE
+        for i in range(-(RANGE/2),(RANGE/2),5):
+            print("SETTING ANGLE: {0}".format(i))
+            SERVO.set_angle(SERVOGPIO, i)
+            time.sleep(1)
+        
+        #rcvr.kill()
+        raw_input("PRESS ENTER WHEN DONE")
+
+    except:
+        raise
+    finally:
+        # CLEANUP
+        print("CLEANUP")
+        SERVO.stop(SERVOGPIO)
+        SERVO.cleanup(SERVOGPIO)
+
+