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
.png)
.JPG)
