diff --git a/ros_arduino_python/src/ros_arduino_python/diagnostics.py b/ros_arduino_python/src/ros_arduino_python/diagnostics.py
new file mode 100644
index 0000000..94cc1fe
--- /dev/null
+++ b/ros_arduino_python/src/ros_arduino_python/diagnostics.py
@@ -0,0 +1,135 @@
+#!/usr/bin/env python
+
+"""
+    Diagnostics classes for the arudino_python package
+    
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2016 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+"""
+
+import rospy
+import diagnostic_updater
+from diagnostic_msgs.msg import DiagnosticStatus, KeyValue
+
+class DiagnosticsPublisher():
+    """ Class to handle publications of diagnostics messages. """
+
+    def __init__(self, arduino):
+        self.arduino = arduino
+        self.device = self.arduino.device
+
+    def update(self):
+        # First the arduino itself
+        self.device.diagnostics.diag_updater.update()
+     
+        # Then the sensors
+        for sensor in self.device.sensors:
+            sensor.diagnostics.diag_updater.update()
+                    
+        # Next the servos
+        if self.arduino.have_joints:
+            for servo in self.arduino.servo_controller.servos:
+                servo.diagnostics.diag_updater.update()
+                    
+        # Finally, the base controller
+        if self.arduino.use_base_controller:
+            self.arduino.base_controller.diagnostics.diag_updater.update()
+
+class DiagnosticsUpdater():
+    """ Class to return diagnostic status from a component. """
+    
+    def __init__(self, component, name, error_threshold=10, rate=1.0, create_watchdog=False):
+        self.component = component
+        
+        self.name = name
+            
+        # Determines the OK, WARN and ERROR status flags
+        self.error_threshold = error_threshold
+        
+        # Create a diagnostics updater
+        self.diag_updater = diagnostic_updater.Updater()
+        
+        # Set the period from the rate
+        self.diag_updater.period = 1.0 / rate
+        
+        # Set the hardware ID to name + pin
+        try:
+            self.diag_updater.setHardwareID(self.name + '_pin_' + str(self.component.pin))
+        except:
+            self.diag_updater.setHardwareID(self.name)
+         
+        # Create a frequency monitor that tracks the publishing frequency for this component
+        if self.component.rate > 0:
+            freq_bounds = diagnostic_updater.FrequencyStatusParam({'min': self.component.rate, 'max': self.component.rate}, 0.3, 5)
+            self.freq_diag = diagnostic_updater.HeaderlessTopicDiagnostic(self.component.name + '_freq', self.diag_updater, freq_bounds)
+            
+        # Create an error monitor that tracks timeouts, serial exceptions etc
+        self.error_diag = diagnostic_updater.FunctionDiagnosticTask(self.name, self.get_error_rate)
+        self.diag_updater.add(self.error_diag)
+        
+        # Create a watchdog diagnostic to monitor the connection status
+        if create_watchdog:
+            self.watchdog_diag = diagnostic_updater.FunctionDiagnosticTask(self.name, self.get_connection_status)
+            self.diag_updater.add(self.watchdog_diag)
+            
+        # Counters used for diagnostics
+        self.reads = 0
+        self.total_reads = 0
+        self.errors = 0
+        self.error_rates = [0.0]
+        
+        # Connection status for device
+        self.watchdog = False
+        
+    def get_error_rate(self, stat):        
+        error_rate = 0.0
+
+        # Wait until we have some data before computing error rates
+        if self.reads + self.errors > 100:
+            error_rate = (self.errors * 100.0) / (self.reads + self.errors)
+            self.error_rates.append(error_rate)
+            # Keep only the past 1000 measurements (100 x 10) so that error rates are current
+            if len(self.error_rates) > 10:
+                self.error_rates = self.error_rates[-10:]
+            self.reads = 0
+            self.errors = 0
+            
+        error_rate = sum(self.error_rates) / len(self.error_rates)
+            
+        if error_rate > 3 * self.error_threshold:
+            stat.summary(DiagnosticStatus.ERROR, "Excessive Error Rate")
+            
+        elif error_rate > 2 * self.error_threshold:
+            stat.summary(DiagnosticStatus.WARN, "Moderate Error Rate")
+            
+        else:
+            stat.summary(DiagnosticStatus.OK, "Error Rate OK")
+            
+        stat.add("Reads", self.total_reads)
+        stat.add("Error Rate", error_rate)
+
+        return stat
+    
+    def get_connection_status(self, stat):
+        stat.path = self.name + "/Watchdog"
+        if not self.watchdog:
+            stat.summary(DiagnosticStatus.ERROR, "Connnection lost!")
+        else:
+            stat.summary(DiagnosticStatus.OK, "Alive")
+
+        stat.add("Watchdog", self.watchdog)
+        return stat
+
+            
\ No newline at end of file
diff --git a/ros_arduino_python/src/ros_arduino_python/follow_controller.py b/ros_arduino_python/src/ros_arduino_python/follow_controller.py
new file mode 100644
index 0000000..1999d4e
--- /dev/null
+++ b/ros_arduino_python/src/ros_arduino_python/follow_controller.py
@@ -0,0 +1,161 @@
+#!/usr/bin/env python
+
+"""
+  follow_controller.py - controller for a kinematic chain
+  Copyright (c) 2011 Vanadium Labs LLC.  All right reserved.
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions are met:
+      * Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+      * Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution.
+      * Neither the name of Vanadium Labs LLC nor the names of its 
+        contributors may be used to endorse or promote products derived 
+        from this software without specific prior written permission.
+  
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
+  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+"""
+
+import rospy, actionlib
+
+from control_msgs.msg import FollowJointTrajectoryAction
+from trajectory_msgs.msg import JointTrajectory
+from controllers import *
+
+class FollowController(Controller):
+    """ A controller for joint chains, exposing a FollowJointTrajectory action. """
+
+    def __init__(self, device, name):
+        Controller.__init__(self, device, name)
+        # Parameters: rates and joints
+        self.rate = rospy.get_param('~controllers/' + name + '/rate', 50.0)
+        self.joints = rospy.get_param('~controllers/' + name + '/joints')
+        self.index = rospy.get_param('~controllers/'+ name + '/index', len(device.controllers))
+        for joint in self.joints:
+            self.device.joints[joint].controller = self
+
+        # Action server
+        name = rospy.get_param('~controllers/' + name + '/action_name', 'follow_joint_trajectory')
+        self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False)
+
+        # Good old trajectory
+        rospy.Subscriber(self.name + '/command', JointTrajectory, self.command_cb)
+        self.executing = False
+
+        rospy.loginfo("Started FollowController ("+self.name+"). Joints: " + str(self.joints) + " on C" + str(self.index))
+
+    def startup(self):
+        self.server.start()
+
+    def execute_cb(self, goal):
+        rospy.loginfo(self.name + ": Action goal recieved.")
+        traj = goal.trajectory
+
+        if set(self.joints) != set(traj.joint_names):
+            for j in self.joints:
+                if j not in traj.joint_names:
+                    msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
+                    rospy.logerr(msg)
+                    self.server.set_aborted(text=msg)
+                    return
+            rospy.logwarn("Extra joints in trajectory")
+
+        if not traj.points:
+            msg = "Trajectory empy."
+            rospy.logerr(msg)
+            self.server.set_aborted(text=msg)
+            return
+
+        try:
+            indexes = [traj.joint_names.index(joint) for joint in self.joints]
+        except ValueError as val:
+            msg = "Trajectory invalid."
+            rospy.logerr(msg)
+            self.server.set_aborted(text=msg)
+            return
+
+        if self.execute_trajectory(traj):   
+            self.server.set_succeeded()
+        else:
+            self.server.set_aborted(text="Execution failed.")
+
+        rospy.loginfo(self.name + ": Done.")
+    
+    def command_cb(self, msg):
+        # Don't execute if executing an action
+        if self.server.is_active():
+            rospy.loginfo(self.name + ": Received trajectory, but action is active")
+            return
+        self.executing = True
+        self.execute_trajectory(msg)
+        self.executing = False    
+
+    def execute_trajectory(self, traj):
+        rospy.loginfo("Executing trajectory")
+        rospy.logdebug(traj)
+        # carry out trajectory
+        try:
+            indexes = [traj.joint_names.index(joint) for joint in self.joints]
+        except ValueError as val:
+            rospy.logerr("Invalid joint in trajectory.")
+            return False
+
+        # Get starting timestamp, MoveIt uses 0, need to fill in
+        start = traj.header.stamp
+        if start.secs == 0 and start.nsecs == 0:
+            start = rospy.Time.now()
+
+        r = rospy.Rate(self.rate)
+        last = [ self.device.joints[joint].position for joint in self.joints ]
+        for point in traj.points:
+            while rospy.Time.now() + rospy.Duration(0.01) < start:
+                rospy.sleep(0.01)
+            desired = [ point.positions[k] for k in indexes ]
+            endtime = start + point.time_from_start
+            while rospy.Time.now() + rospy.Duration(0.01) < endtime:
+                err = [ (d-c) for d,c in zip(desired,last) ]
+                velocity = [ abs(x / (self.rate * (endtime - rospy.Time.now()).to_sec())) for x in err ]
+                rospy.logdebug(err)
+                for i in range(len(self.joints)):
+                    if err[i] > 0.001 or err[i] < -0.001:
+                        cmd = err[i] 
+                        top = velocity[i]
+                        if cmd > top:
+                            cmd = top
+                        elif cmd < -top:
+                            cmd = -top
+                        last[i] += cmd
+                        self.device.joints[self.joints[i]].time_move_started = rospy.Time.now()
+                        self.device.joints[self.joints[i]].in_trajectory = True
+                        self.device.joints[self.joints[i]].desired = last[i]
+                    else:
+                        velocity[i] = 0
+                r.sleep()
+        return True
+
+    def active(self):
+        """ Is controller overriding servo internal control? """
+        return self.server.is_active() or self.executing
+
+    def getDiagnostics(self):
+        """ Get a diagnostics status. """
+        msg = DiagnosticStatus()
+        msg.name = self.name
+        msg.level = DiagnosticStatus.OK
+        msg.message = "OK"
+        if self.active():
+            msg.values.append(KeyValue("State", "Active"))
+        else:
+            msg.values.append(KeyValue("State", "Not Active"))
+        return msg