Kinect Player Radar Using C# and WPF

In this post i'm going to explain how to make a simple top down player radar for the Kinect using wpf and c#

The radar provides a top down view of user locations relative to the Kinect sensor.

I needed something like this for part of a larger r&d project i'm working on, but wasn't able to find any examples of how to do it. Turns out it's actually pretty simple.

SkeletonStream

The Kinect's SkeletonStream allows us to track the position of a user infront of the Kinect, and also the positions of up to 20 joints on the user's body.

SkeletonStream provides position tracking for up to six users. Full skeletal tracking is only possible for  two users, however for the purposes of this project i only needed the global position of each user.

The Kinect SDK provides a Skeleton Class (Microsoft.Kinect.Skeleton) which is used to contain the data for a tracked user skeleton.

There are three members of the Skeleton Class that we are interested in.

1. TrackingState : this property tells us the tracking state of the skeleton, either Tracked, PositionOnly, or NotTracked
2. TrackingId : a unique id for each tracked skeleton
3. Position : provides the 3d position of a tracked user

Position data is represented by the SkeletonPoint structure which has 3 distance properties, X: which represents the horizontal distance from the sensor, Y: which represents the vertical distance from the sensor, and Z: which represents depth. The values returned are in meters and the Kinect sensor is at position 0,0,0. For this project i was only concerned with the X and Z values as this is a top down view.

For this simple example i'm going to draw a circle on a wcf canvas panel to represent each tracked player. Each player will be represented by a different colored circle.

I'm using a canvas panel with a width of 600px and a height of 1050px
300 pixels on the canvas represents 1 meter in the real world, so the canvas size maps to a 2m x 3.5m area infront of the kinect.
The values i'm using are oriented so that the kinect sensor is effectively at the top center of the canvas

The Code

I'm going to detail some of the code that i used for this project below.
You can find full example code for this article on github

The xaml markup for the radar is very simple. It's pretty much just a canvas panel.

<Window x:Class="KinectPlayerRadar.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        Title="KinectPlayerRadar" Width="600" Height="1050" Loaded="WindowLoaded" Closing="WindowClosing">

    <Canvas Width="600" Height="1050" Name="canvas" Background="Gray">
    </Canvas>
</Window>

Jumping straight into the codebehind. First up we define some variables to contain a reference to the Kinect sensor, and a list of the skeleton ids that we are currently tracking.
We then initialize the Kinect sensor, enable the SkeletonStream, and subscribe to the sensor's SkeletonFrameReady event. This event fires when a new skeleton frame for the SkeletonStream is available.

private KinectSensor _sensor;
private List<int> _skeletonTrackingIds;

const int PixelsPerMeter = 300;
const string BaseMarkerName = "player";

private void WindowLoaded(object sender, RoutedEventArgs e)
{
    if (!KinectSensor.KinectSensors.Any())
    {
        throw new ApplicationException("no kinect sensor detected");
    }

    _sensor = KinectSensor.KinectSensors[0];
            
    _skeletonTrackingIds = new List<int>();
            
    _sensor.SkeletonStream.Enable();
    _sensor.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(sensor_SkeletonFrameReady);            
    _sensor.Start();
}

SkeletonFrameReady event handler

In the event handler we copy the Skeleton Frame data into a new array. The array will always contain 6  Skeletons, even if nothing is being tracked, so we need to loop through each of the Skeletons and check the TrackingState property. If the Skeleton is being tracked we call the SetMarkerPosition method, passing in the Skeleton and its index in the array as parameters.

Once we have processed each skeleton, the RemoveUnusedMarkers method is called to remove any markers for players that are no longer being tracked.

void SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
    Skeleton[] skeletons = new Skeleton[0];

    using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
    {
        if (skeletonFrame != null)
        {
            skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
            skeletonFrame.CopySkeletonDataTo(skeletons);
        }
    }

    var index = 0;

    foreach (Skeleton skeleton in skeletons)
    {
        if (skeleton.TrackingState != SkeletonTrackingState.NotTracked)
        {
            SetMarkerPosition(skeleton, index);
        }

        index++;
    }

    RemoveUnusedMarkers(skeletons);
}

SetMarkerPosition 

In this method we get the X and Z positions of the tracked skeleton and calculate the top and left values  for our player marker on the canvas.

The X position is negative if the player is to the right of the sensor, and positive if the user is to the left of the sensor. The Y value will always be positive. So for example an X value of -0.8 and a Y value of 1.5 would mean that the user is 0.8 meters to the left of the sensor, and 1.5 meters back from the sensor.

Given these position values we can easily calculate the position of the marker on the canvas.

Since our sensor is assumed to be at the top center of the canvas, the "top" value for our marker will be skeleton.Position.Z * PixelsPerMeter and the "left" value for our marker will be (canvas.Width / 2) + (skeleton.Position.X * PixelsPerMeter)

If the skeleton is already in our list of tracked skeletons then we simply update the position of the player's marker on the canvas.

If the skeleton is not currently being tracked, we add a new marker to the canvas and then set its position.

private void SetMarkerPosition(Skeleton skeleton, int index)
{         
    System.Windows.Shapes.Ellipse marker;
    var canvasCenter = canvas.Width / 2;
    var top = skeleton.Position.Z * PixelsPerMeter;
    var left = (canvas.Width / 2) + (skeleton.Position.X * PixelsPerMeter);

    if (!_skeletonTrackingIds.Contains(skeleton.TrackingId))
    {
        marker = AddToCanvas(skeleton.TrackingId, index);
        _skeletonTrackingIds.Add(skeleton.TrackingId);
    }
    else
    {
        marker = canvas.FindName(BaseMarkerName + skeleton.TrackingId) as System.Windows.Shapes.Ellipse;
    }

    if (marker != null)
    {
        Canvas.SetTop(marker, (skeleton.Position.Z * PixelsPerMeter));
        Canvas.SetLeft(marker, canvasCenter + (skeleton.Position.X * PixelsPerMeter));
    }
}

AddToCanvas

The AddToCanvas method dynamically adds a new ellipse shape to the canvas to represent our player.

private Ellipse AddToCanvas(int skeletonTrackingId, int skeletonIndex)
{
    var ellipse = new System.Windows.Shapes.Ellipse();
    ellipse.Name = BaseMarkerName + skeletonTrackingId;
    ellipse.Fill = GetMarkerColor(skeletonIndex);
    ellipse.Width = 30;
    ellipse.Height = 30;
            
    canvas.Children.Add(ellipse);
    canvas.RegisterName(ellipse.Name, ellipse);
          
    return ellipse;
}

GetMarkerColor

The GetMarkerColor method returns a different color brush for each possible player index.

private Brush GetMarkerColor(int playerIndex)
{
    if (playerIndex == 1) return Brushes.Blue;
    if (playerIndex == 2) return Brushes.Red;
    if (playerIndex == 3) return Brushes.Green;
    if (playerIndex == 4) return Brushes.Orange;
    if (playerIndex == 5) return Brushes.Purple;

    return Brushes.Turquoise;
}

RemoveUnusedMarkers

The RemoveUnusedMarkers method is called after our player positions have been set in order to remove markers for players that are no longer being tracked.

private void RemoveUnusedMarkers(Skeleton[] skeletons)
{
    _skeletonTrackingIds.ForEach(id =>
        {
            if (!skeletons.Select(s => s.TrackingId).Contains(id))
            {
                var marker = canvas.FindName(BaseMarkerName + id) as Ellipse;
                if (marker != null)
                {
                    canvas.Children.Remove(marker);
                }
            }
        });
}

That's pretty much all there is to it. Obviously this is a very basic example, but hopefully someone else out there might find it useful.

If you liked this article you can also find me on twitter.

Get the code from GitHub

Windows 8 - Cisco VPN Client - Failed to enable virtual adapter

So i installed Windows 8 release preview on my desktop a few days ago and so far i've been quite impressed with it.

One problem that i've come across so far is that after upgrading, Cisco vpn client stopped working with a "Failed to enable Virtual Adapter" error when i tried to connect to any vpn.

This is massively annoying for me as i use Cisco vpn client to connect to the network at work.

Fortunately after some frantic googling i managed to find a workaround.

Fire up the registry editor and navigate to the following path.

HKEY_LOCAL_MACHINE\SYSTEM\CurrentControlSet\Services\CVirtA

Modify the DisplayName key

For 64bit Windows, edit the key so that it reads "Cisco Systems VPN Adapter for 64-bit Windows", or for x86 "Cisco Systems VPN Adapter"

Now open the vpn client and try connecting again. It should work now.

Hopefully this post will save someone else a bit of time.

Unit Testing SignalR Hubs

SignalR is an awesome library for building realtime async applications using .Net. If you've not had a chance to use it yet then you should definately check out this SignalR article from Hanselman and find something to use it in immediately. You're missing out on all the fun!!

So all the cool kids are using SignalR, but I haven't seen many people talking about how to unit test a SignalR hub. In this post i'm going to explain how to set up a testable Hub and then write a sample unit test for it.

I would recommend keeping SignalR Hubs as skinny as possible, and moving as much logic as you can into seperate classes and services. However there will probably be times when you will need to write tests against a hub class, in the same way that we need to write tests against an asp.net mvc Controller.

Lets imagine we have a very simple chat application. In our application, when a message is sent by a client, the message is saved to a data store, and then broadcast to all clients.

This scenario is trivial with SignalR

public class ChatHub : Hub
    {
        private readonly IChatRepository _chatRepository;

        public ChatHub(IChatRepository chatRepository)
        {
            _chatRepository = chatRepository;
        }

        public bool Send(string message)
        {
            _chatRepository.SaveMessage(message, Context.ConnectionId);
            Clients.addMessage(message);
            return true;
        }
    }

In this simple example, i want to test that when a message is sent, the SaveMessage method is invoked on _chatRepository with the correct parameters and that the Send method returns true.

In order to test this we need to create a testable instance of ChatHub.

If we try to new up a ChatHub instance in a unit test, we'll get null reference exceptions from both Context.ConnectionId, and Clients.addMessage when Send is called.

We need to wire up the Context and Clients properties of the Hub.
It will also be useful to wire up the Caller property as this will likely be used at some point.

I'm using Moq in this example to mock the various different objects required to create a testable hub.

 public class TestableChatHub : ChatHub  
   {  
     public Mock<IChatRepository> MockChatRepository { get; private set; }
  
     public TestableChatHub(Mock<IChatRepository> mockChatRepository)  
       : base(mockChatRepository.Object)  
     {
       const string connectionId = "1234";  
       const string hubName = "Chat";  
       var mockConnection = new Mock<IConnection>();  
       var mockUser = new Mock<IPrincipal>();  
       var mockCookies = new Mock<IRequestCookieCollection>();
  
       var mockRequest = new Mock<IRequest>();  
       mockRequest.Setup(r => r.User).Returns(mockUser.Object);  
       mockRequest.Setup(r => r.Cookies).Returns(mockCookies.Object);
         
       Clients = new ClientAgent(mockConnection.Object, hubName);  
       Context = new HubCallerContext(mockRequest.Object, connectionId);
  
       var trackingDictionary = new TrackingDictionary();  
       Caller = new StatefulSignalAgent(mockConnection.Object, connectionId, hubName, trackingDictionary);  
     }  
   } 

Once the Context, Clients and Caller properties have been set on the Hub, writing tests against it is simple.

 using Moq;  
 using NUnit.Framework;  
 using SignalRTestableHubExample.Models;
  
 namespace SignalRTestableHubExample.Tests  
 {   
   [TestFixture]  
   public class ChatHubTests  
   {  
     private TestableChatHub _hub;
  
     [SetUp]  
     public void SetUpTests()  
     {  
       _hub = GetTestableChatHub();  
     }
  
     [Test]  
     public void ExampleTest()  
     {  
       const string message = "test";  
       const string connectionId = "1234";
  
       var result = _hub.Send(message);
  
       _hub.MockChatRepository.Verify(r => r.SaveMessage(message, connectionId));
       Assert.IsTrue(result);  
     }
  
     private TestableChatHub GetTestableChatHub()  
     {  
       var mockRepository = new Mock<IChatRepository>();  
       mockRepository.Setup(m => m.SaveMessage(It.IsAny<string>(), It.IsAny<string>())).Returns(true);  
       return new TestableChatHub(mockRepository);  
     }  
   }  
 }

.Net Gadgeteer Robot with PS Vita as controller

This post is about a modification of my .Net Gadgeteer object tracking robot.

The aim of the project was to add a robotic arm to the robot and allow control over wifi using my Playstation Vita.

I had picked up a Playstation Vita a few months back and was really excited when i heard about the Playstation Mobile Developer Program because it would allow me to write code for the vita in c#

After building my object tracking robot, i thought it would be an interesting challenge to try and control it using the vita.

The robot uses the same Rover 5 chassis and explorer pcb motor controller as the original robot.



Here's a couple of videos of him in action.

The parts i used to build this robot were as follows.

• Fez Spider Mainboard
• 2 Gadgeteer Extender Modules
• Gadgeteer Ethernet Module
• Gadgeteer Client DP Module

• Dagu 2dof Robotic Arm
• Dagu Rover 5 Chassis
• Dagu Explorer PCB
• Edimax Portable Router

I'm using a Playstation Vita as the controller for this robot, however you could really use any wifi capable device that you can write a socket client for.

I was planning on picking up a Gadgeteer wifi module for this project, however instead i decided to use this portable router that i had lying around.

The gadgeteer ethernet module connects directly into the portable router while the vita connects to the the router over wifi.
Its kinda cool because the robot has its own self contained network and i dont need to rely on having an external router present in order to use it.


The battery lasts for about 4 hours and i think you can pick these up pretty cheap now.


My rough plan was to run a socket server on the robot which would listen for commands from a socket client running on the ps vita and to use a couple of existing libraries i have made for controlling the motors and robotic arm.

I've explained in previous posts about the chassis and motor controller that i'm using, so i wont go into any more detail about that here.

Breadboard Diagram.

This is how i have wired everything up on a breadboard using the 2 extender modules.
I have soldered headers onto both extender modules to make it easy for me to plug them into a breadboard.

Both extender modules need to be connected to pwm capable sockets on the gadgeteer mainboard. I'm using sockets 8 and 11 on the Spider.

Code

In order to keep things tidy i created a few seperate class librarys for things that i can reuse in other gadgeteer projects.

I'm using the RoboticArmController class, that i detailed in a previous blog post, to control the robotic arm.

I also have a MotorController class which i will detail below. This class is used to drive the motors of the robot. 

I pass in a reference to an extender module in the constructor and set up the pwm and digital outputs that i will need to control the motors. The extender module used to drive the motors will need to be connected to a pwm capable socket on the gadgeteer mainboard.

The MotorController class has a single public method: DriveMotor(int speed, Motor motor) which is used to drive either the left or right motor. The speed ranges from -100 which is reverse, to 100 which is full speed forwards, with 0 being stop.

using System;
using GT = Gadgeteer;
using Gadgeteer.Modules.GHIElectronics;

namespace GadgeteerPwmMotorController
{
    public class MotorController
    {
        public enum Motor
        {
            left, right
        }

        public const int MotorMaxSpeed = 100;
        public const int MotorMinSpeed = -100;
        private uint _pwmPulsePeriod;

        public GT.Interfaces.PWMOutput LeftMotorPwm { get; set; }

        public GT.Interfaces.PWMOutput RightMotorPwm { get; set; }

        public GT.Interfaces.DigitalOutput LeftMotorDirection { get; set; }

        public GT.Interfaces.DigitalOutput RightMotorDirection { get; set; }

        public MotorController(Extender extender, uint pwmPulsePeriod)
        {
            if (extender == null)
            {
                throw new ApplicationException("motor controller pwm extender module not set up correctly");
            }

            LeftMotorPwm = extender.SetupPWMOutput(GT.Socket.Pin.Eight);
            RightMotorPwm = extender.SetupPWMOutput(GT.Socket.Pin.Seven);
            LeftMotorDirection = extender.SetupDigitalOutput(GT.Socket.Pin.Six, true);
            RightMotorDirection = extender.SetupDigitalOutput(GT.Socket.Pin.Five, true);

            _pwmPulsePeriod = pwmPulsePeriod;
        }

        public void DriveMotor(int speed, Motor motor)
        {
            // speed range is from -100 to 100
            // -100 is full speed reverse, 0 is stop, 100 is full speed forwards

            bool forward = true;

            speed = speed > MotorMaxSpeed ? MotorMaxSpeed : speed;

            speed = speed < MotorMinSpeed ? MotorMinSpeed : speed;

            if (System.Math.Abs(speed) < 7) speed = 0; 

            if (speed < 0)
            {
                forward = false;
            }

            if (motor == Motor.left)
            {
                LeftMotorDirection.Write(forward);
                SetPwmPulse(LeftMotorPwm, speed);
            }
            else
            {
                RightMotorDirection.Write(forward);
                SetPwmPulse(RightMotorPwm, speed);
            }
        }

        private void SetPwmPulse(GT.Interfaces.PWMOutput pwm, int speed)
        {
            var pulseHighTime = System.Math.Abs(speed) * (_pwmPulsePeriod / 100);
            pwm.SetPulse(_pwmPulsePeriod, (uint)pulseHighTime);
        }
    }
}

UdpSocketServer Class

I figured it would be better to use Udp for communication between the robot and the vita.

I wasn't too bothered about losing the occasional packet and i wanted it to be as fast as possible so that control of the robot would feel responsive.

I created a UdpSocketServer class which manages the task of listening for Udp data on a specified port.

It is a very simple class that listens for a socket connection on the specified port in a seperate thread so not to block the rest of the program. When data is recieved an event is raised.

 using System;  
 using System.Net;  
 using System.Net.Sockets;  
 using System.Threading;  
 using Microsoft.SPOT;  
 namespace SocketServer  
 {  
   public class UdpSocketServer  
   {  
     private Thread _listeningThread;  
     private int _port;  
     public delegate void DataReceivedEventHander(object sender, byte[] data);  
     public event DataReceivedEventHander DataReceived;  
     public UdpSocketServer(int port)  
     {  
       _port = port;  
     }  
     public void StartListening()  
     {  
       _listeningThread = new Thread(new ThreadStart(Listen));  
       _listeningThread.Start();  
     }  
     public void StopListening()  
     {  
       if (_listeningThread != null)  
       {  
         _listeningThread.Abort();  
       }  
     }  
     private void Listen()  
     {  
       using (var socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp))  
       {  
         EndPoint ipEndPoint = new IPEndPoint(IPAddress.Any, _port);  
         socket.Bind(ipEndPoint);  
         while (true)  
         {  
           if (socket.Poll(-1, SelectMode.SelectRead))  
           {  
             byte[] data = new byte[socket.Available];  
             int i = socket.ReceiveFrom(data, ref ipEndPoint);  
             if (data.Length > 0)  
             {  
               DataReceived(this, data);  
             }  
           }  
         }  
       }  
     }  
   }  
 }  

Putting it all together

With these utility classes in place, the rest of the code is relatively straight forward.

I set up a new Gadgeteer project in visual studio and added the 2 extender modules and Ethernet module using the visual designer.

Added a class to contain the control data received from the vita

 public class ControlData  
   {  
     public int LeftMotor { get; set; }  
     public int RightMotor { get; set; }  
     public bool CloseClaw { get; set; }  
     public bool OpenClaw { get; set; }  
     public bool ArmUp { get; set; }  
     public bool ArmDown { get; set; }  
   }  

Created a  VitaControlListener class to manage dealing with the socket server and reading the data received from the vita.

On the vita i'm using the left analog stick to control the left motor, and the right analog stick to control the right motor. The d-pad up and down buttons are used to move the robotic arm up and down, the left shoulder button opens the claw, and the right should button closes it.

The joystick and button values are read on the vita and sent to the socket server on the robot as a byte array. The VitaControlListener class subscribes to the DataReceived event of the socket server, extracts the control values from the byte array, and puts them into a ControlData object for us to work with. 

 using System;  
 using System.Net;  
 using System.Net.Sockets;  
 using System.Threading;  
 using Microsoft.SPOT;  
 using SocketServer;  
 namespace VitaControlledRobot  
 {  
   class VitaControlListener  
   {  
     private UdpSocketServer _socketServer;  
     public delegate void ControlDataReceivedEventHander(object sender, ControlData data);  
     public event ControlDataReceivedEventHander ControlDataReceived;  
     public VitaControlListener()  
     {  
       _socketServer = new UdpSocketServer(11000);  
       _socketServer.DataReceived += new UdpSocketServer.DataReceivedEventHander(_socketServer_DataReceived);  
     }  
     void _socketServer_DataReceived(object sender, byte[] data)  
     {  
       ReadControlData(data);  
     }  
     public void Start()  
     {  
       _socketServer.StartListening();  
     }  
     public void Stop()  
     {  
       _socketServer.StopListening();  
     }  
     private void ReadControlData(byte[]data)  
     {  
       float left;  
       float right;  
       GHIElectronics.NETMF.System.Util.ExtractValueFromArray(out left, data, 0);  
       GHIElectronics.NETMF.System.Util.ExtractValueFromArray(out right, data, 4);  
       ControlDataReceived(this, new ControlData  
       {  
         LeftMotor = GetSpeedFromJoystickValue(left),  
         RightMotor = GetSpeedFromJoystickValue(right),  
         OpenClaw = data[9] > 0,  
         CloseClaw = data[10] > 0,  
         ArmUp = data[11] > 0,  
         ArmDown = data[12] > 0  
       });  
     }  
     private int GetSpeedFromJoystickValue(float joystickValue)  
     {  
       var speed = (int)(joystickValue * -100);  
       return System.Math.Abs(speed) < 7 ? 0 : speed;  
     }  
   }  
 }  

And finally in Program.cs.

Basically all we are doing in here is listening for events from a VitaControlListener instance and using the RoboticArmController and MotorController classes to control the robotic arm and the movement of the robot depending on what control data was received.

I've given the robot a static ip address and hardcoded the networking details for testing purposes.

 using System;  
 using System.Threading;  
 using Microsoft.SPOT;  
 using Gadgeteer.Networking;  
 using GT = Gadgeteer;  
 using GTM = Gadgeteer.Modules;  
 using GadgeteerPwmMotorController;  
 using GadgeteerRoboticArm;  
 namespace VitaControlledRobot  
 {  
   public partial class Program  
   {      
     private const string IpAddress = "192.168.2.150";  
     private const string SubnetMask = "255.255.255.0";  
     private const string DefaultGateway = "192.168.2.1";  
     private const uint PwmPulsePeriod = 20000000;  
     private const int ClawPositionStep = 25;  
     private const int ArmPositionStep = 10;
  
     private MotorController _motorController;  
     private RoboticArmController _roboticArmController;  
     private VitaControlListener _vitaListener;
  
     void ProgramStarted()  
     {  
       Debug.Print("Program Started");  
       Setup();  
     }
  
     private void Setup()  
     {  
       ethernet.UseStaticIP(IpAddress, SubnetMask, DefaultGateway);  
       ethernet.NetworkUp += new GTM.Module.NetworkModule.NetworkEventHandler(ethernet_NetworkUp);  
       ethernet.NetworkDown += new GTM.Module.NetworkModule.NetworkEventHandler(ethernet_NetworkDown);  
     }
  
     void ethernet_NetworkDown(GTM.Module.NetworkModule sender, GTM.Module.NetworkModule.NetworkState state)  
     {  
       _vitaListener.Stop();  
     }
  
     void ethernet_NetworkUp(GTM.Module.NetworkModule sender, GTM.Module.NetworkModule.NetworkState state)  
     {  
       Debug.Print("network up");  
       Debug.Print(ethernet.NetworkSettings.IPAddress);
  
       _motorController = new MotorController(PwmExtender2, PwmPulsePeriod);  
       _roboticArmController = new RoboticArmController(PwmExtender, GT.Socket.Pin.Seven, GT.Socket.Pin.Eight, PwmPulsePeriod);  
       _vitaListener = new VitaControlListener();
  
       _vitaListener.ControlDataReceived += new VitaControlListener.ControlDataReceivedEventHander(vitaListener_ControlDataReceived);  
       _vitaListener.Start();
  
       Debug.Print("server started");  
     }
  
     void vitaListener_ControlDataReceived(object sender, ControlData data)  
     {  
       _motorController.DriveMotor(data.LeftMotor, MotorController.Motor.left);  
       _motorController.DriveMotor(data.RightMotor, MotorController.Motor.right);  
       if (data.CloseClaw)  
       {  
         _roboticArmController.MoveClaw(_roboticArmController.ClawCurrentPosition + ClawPositionStep);  
       }  
       else if(data.OpenClaw)  
       {  
         _roboticArmController.MoveClaw(_roboticArmController.ClawCurrentPosition - ClawPositionStep);  
       }  
       if (data.ArmUp)  
       {  
         _roboticArmController.MoveArm(_roboticArmController.ArmCurrentPosition + ArmPositionStep);  
       }  
       else if (data.ArmDown)  
       {  
         _roboticArmController.MoveArm(_roboticArmController.ArmCurrentPosition - ArmPositionStep);  
       }  
     }  
   }  
 }  

Thats mostly it on the gadgeteer side.

Obviously this is a work in progress and there are lots of other things that i want to add to it, but i'm quite happy with how it's worked out so far.

I love how easy it is to do stuff like this with Gadgeteer.

In my next post i'll be showing what the ps vita side looks like.

If you liked this post you can also find me on twitter @RikTheManc

Gadgeteer Robotic Arm Controller

In this post i'm going to explain how to control a small 2dof robotic arm using .Net Gadgeteer.


The parts i am using for this project are

• Fez Spider Mainboard
• Dagu 2dof Robotic Arm
• Gadgeteer Extender Module
• Gadgeteer Button Module to control the gripper
• Gadgeteer Potentiometer Module to control the arm.

The robotic arm consists of a gripper and two servos, the first servo controls the movement of the gripper, and the other moves the arm up or down (or left/right depending on how you mount it)

You could very easily adapt the code in this post to deal with a larger robotic arm with more servos.

Here is a quick video of the robotic arm in action. It's currently attached to the front of my gadgeteer robot.

Setting up the robotic arm

In order to wire up the Robotic arm i'm using an extender module with a header soldered onto it so that i can plug it into a breadboard easily.

First up the extender module needs to be connected to a pwm capable socket on the mainboard.
I used socket 8 on the Spider. Pins 7 and 8 on the extender module will be connected to the pwm connectors on the servos. The pwm connectors are orange. Positive is red and Ground is the brown connector.

We need to use a separate power source for the servos. Don't try and power them from the extender module. A battery pack of 4AA batteries or similar will work fine.

Our breadboard should be wired up as shown to the left.









Once everything is connected it's pretty simple to write some code to move the servos.

The only tricky part was figuring out the correct pwm values to send to the gripper servo in order to fully open and close it. This just required a bit of trial and error. The gripper servo can only move around 60 degrees because it is constrained by the gripper mechanism. If you try to move it too far beyond its limits, the servo will heat up extremely fast. The arm servo can move through 180 degrees.

I made a small class which simplifies use of the arm with gadgeteer, and can be re-used in other projects.

In the constructor of the RoboticArmController class we pass in a reference to the extender module, and specify which pins will be used for the gripper and the arm.

There are public methods which can be used to move the position of the gripper and arm. The position value passed to these methods should be in the range of 0 - 100. At position 0 the gripper is fully open, and at 100 it is fully closed.

 using System;  
 using Microsoft.SPOT;  
 using Gadgeteer.Modules.GHIElectronics;  
 using GT = Gadgeteer;  
 namespace GadgeteerRoboticArm  
 {  
   public class RoboticArmController  
   {  
     private const int ClawPwmMin = 1500;  
     private const int ClawPwmMax = 2200;  
     private const int ArmPwmMin = 600;  
     private const int ArmPwmMax = 2400;
  
     private const int ClawPositionMax = 100;  
     private const int ClawPositionMin = 0;  
     private const int ArmPositionMax = 100;  
     private const int ArmPositionMin = 40; 
 
     private uint _pwmPulsePeriod;  
     private GT.Interfaces.PWMOutput _clawPwm;  
     private GT.Interfaces.PWMOutput _armPwm; 
 
     public int ClawCurrentPosition { get; private set; }  
     public int ArmCurrentPosition { get; private set; } 
 
     public RoboticArmController(Extender extender, GT.Socket.Pin clawPin, GT.Socket.Pin armPin, uint pwmPulsePeriod)  
     {  
       if (extender == null)  
       {  
         throw new ApplicationException("robotic arm pwm extender not set up correctly");  
       }  
       _pwmPulsePeriod = pwmPulsePeriod;  
       _clawPwm = extender.SetupPWMOutput(clawPin);  
       _armPwm = extender.SetupPWMOutput(armPin);  
       Reset();  
     }
  
     public void Reset()  
     {  
       MoveClaw(ClawPositionMin);  
       MoveArm(ArmPositionMin);  
     } 
 
     public void MoveClaw(int position)  
     {  
       ClawCurrentPosition = Move(_clawPwm, position, ClawPositionMin, ClawPositionMax, ClawPwmMin, ClawPwmMax);  
     }
  
     public void MoveArm(int position)  
     {  
       ArmCurrentPosition = Move(_armPwm, position, ArmPositionMin, ArmPositionMax, ArmPwmMin, ArmPwmMax);  
     } 
 
     private int Move(GT.Interfaces.PWMOutput pwm, int position, int minPosition, int maxPosition, int pwmMin, int pwmMax)  
     {  
       position = position > maxPosition ? maxPosition : position;  
       position = position < minPosition ? minPosition : position;
  
       Servo(pwm, GetPwmPulseValue(pwmMax, pwmMin, position));
  
       return position;  
     } 
 
     private int GetPwmPulseValue(int max, int min, int position)  
     {  
       return (min + (((max - min) / 100) * position)) * 1000;  
     }
  
     private void Servo(GT.Interfaces.PWMOutput pwm, int pwmPulseHighTime)  
     {  
       pwm.SetPulse(_pwmPulsePeriod, (uint)pwmPulseHighTime);  
     }  
   }  
 }  

Using this class it is then easy to set up a demo application that closes and opens the gripper as a button is pressed and released, and moves the arm position dependant on the position of a potentiometer module.

Create a new Gadgeteer project in visual studio, add the the button, potentiometer and extender modules using the visual designer and then add the following code to Program.cs

 using System;  
 using System.Threading;  
 using Microsoft.SPOT;  
 using GT = Gadgeteer;  
 using Gadgeteer.Modules.GHIElectronics;  
 using GadgeteerRoboticArm;  
 namespace RoboticArmDemo  
 {  
   public partial class Program  
   {  
     private RoboticArmController _roboticArmController;  
     private GT.Timer _timer; 
 
     void ProgramStarted()  
     {  
       Debug.Print("Program Started");  
       _roboticArmController = new RoboticArmController(extender, GT.Socket.Pin.Seven, GT.Socket.Pin.Eight, 20000000);
  
       button.ButtonPressed += new Button.ButtonEventHandler(button_ButtonPressed);  
       button.ButtonReleased +=new Button.ButtonEventHandler(button_ButtonReleased);
  
       _timer = new GT.Timer(500);  
       _timer.Tick += new GT.Timer.TickEventHandler(_timer_Tick);      
     }
  
     void _timer_Tick(GT.Timer timer)  
     {  
       _roboticArmController.MoveArm((int)potentiometer.ReadPotentiometerPercentage());  
     }
  
     void button_ButtonPressed(Button sender, Button.ButtonState state)  
     {  
       _roboticArmController.MoveClaw(100);  
     }
  
     void button_ButtonReleased(Button sender, Button.ButtonState state)  
     {  
       _roboticArmController.MoveClaw(0);  
     }  
   }  
 }  

And thats pretty much all there is to it.

When the program is started, pressing and holding the button will close the gripper. Releasing the button will open the gripper again.


The position of the arm can be set using the potentiometer.


You can grab sample code to accompany this post from github