Introduction
In this article, I’m going to explain about creating a fully computer controlled Lego Mindstorms NXT 2.0 Bot (with camera mounted on it) using Microsoft Robotics Developer Studio (MRDS).
Background
My previous article Microsoft Robotics Service for LEGO NXT 2.0 should be useful if you haven’t created any service using the Studio.
Requirements
Demo
Check out this video.
Using the Code
Create a MRDS Service and add the partners - NxtDrive and NxtBattery. This will automatically declare and instantiate an object for BatteryOperations
and an object for DriveOperations
. Please add one more for DriveOperations
as the code mentioned below:
[Partner("NxtBattery", Contract = battery.Contract.Identifier,
CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
battery.BatteryOperations _nxtBatteryPort = new battery.BatteryOperations();
[Partner("NxtDrive", Contract = drive.Contract.Identifier,
CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
drive.DriveOperations _nxtDrivePort = new drive.DriveOperations();
drive.DriveOperations _nxtDriveNotify = new drive.DriveOperations();
Add a new class library project to the solution, and add an interface to it. This project needs to be referenced in the MRDS Service and the WPF UI projects (will explain the UI project later).
public interface IMyLegoCarService
{
double GearPower { get; set; }
long LeftEncoderCurrent { get; set; }
long RightEncoderCurrent { get; set; }
double LeftPowerCurrent { get; set; }
double RightPowerCurrent { get; set; }
double BatteryPower { get; set; }
void Drive(DriveAction driveDirection);
void StopEngine();
}
Change the MRDS Service in such a way that it implements this interface. Values for all the properties but the first property (GearPower
) will be set in the service, and they will be retrieved and used in the UI layer.
Now, add a new WPF project. Declare a property named Service
of type IMyLegoCarService
, and add a constructor, something like this:
public Dashboard(IMyLegoCarService service) : this()
{
Service = service;
Service.GearPower = 0;
brsr_ipcamera.Navigate(new Uri
("http://ipoftheiphoneorthewebserver/iphonecamera/index.htm"));
UpdateInitialOdometer();
uiTimer = new DispatcherTimer();
uiTimer.Interval = TimeSpan.FromSeconds(1);
uiTimer.Tick += uiTimer_Tick;
uiTimer.Start();
}
This constructor should be called from the Service
’s constructor, so that the service and the UI will be on the same thread. Here “brsr_ipcamera
” is the web browser control to display the ip camera’s image/video (in my case my iphone). I added an HTML page to my webserver displaying only the video from the camera. Add a timer control to display the information retrieved from the service periodically. Here I’ve used WPF Dashboard Controls’ dial controls as speedometers (for left motor front, left motor reverse, right motor front and right motor reverse), odometer control as odometer and progress bar control as fuel gauge. Left/Right Power Current properties were used to initialize the speedometers. Left/Right Encoder properties were used to initialize the odometer, these properties basically give us the degrees that the servo motors rotated. Using the formula: distance = Convert.ToInt32(Math.Abs(currentEncoderCurrent) / 360 * 2 * 3.14 * 0.75
, we can calculate the distance covered. Here, pi = 3.14 and 0.75 is the radius of the wheels.
Coming back to the service. Declare and/or instantiate the following classes:
wpf.WpfServicePort _wpfServicePort;
drive.SetDriveRequest _nxtSetDriveRequest = new drive.SetDriveRequest();
battery.BatteryState _nxtBatteryState;
WpfServicePort
is used to invoke the WPF UI, SetDriveRequest
to rotate the motors and the BatteryState
to get the battery information.
Add a port named “TimerTick
” to the service types similar to the automatically created ports “Get
”, “Subscribe
”, etc. Now your serviceoperations
class declaration will be something like this:
[ServicePort]
public class MyLegoCarServiceOperations :
PortSet<DsspDefaultLookup, DsspDefaultDrop, Get, Subscribe, TimerTick>
{
}
public class TimerTick:Update<TimerTickRequest, PortSet<DefaultUpdateResponseType, Fault>>
{
public TimerTick()
: base(new TimerTickRequest())
{
}
}
[DataContract]
public class TimerTickRequest
{
}
Modify the service’s start
method something like this:
protected override void Start()
{
SpawnIterator(DoStart);
}
private IEnumerator<ITask> DoStart()
{
DispatcherQueue queue = new DispatcherQueue();
this._wpfServicePort = wpf.WpfAdapter.Create(queue);
var runWindow = this._wpfServicePort.RunWindow(() => (Window)new Dashboard(this));
yield return (Choice)runWindow;
var exception = (Exception)runWindow;
if (exception != null)
{
LogError(exception);
StartFailed();
yield break;
}
var subscribe1 = this._nxtDrivePort.Subscribe(_nxtDriveNotify);
yield return (Choice)subscribe1;
_timerPort.Post(DateTime.Now);
Activate<ITask>(
Arbiter.Receive<drive.DriveEncodersUpdate>
(true, _nxtDriveNotify, DriveEncoderHandler),
Arbiter.Receive(true, _timerPort, TimerHandler)
);
StartHandlers();
}
private void StartHandlers()
{
base.Start();
}
Timerport
is used to retrieve the battery information periodically.
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> TimerTickHandler(TimerTick incrementTick)
{
incrementTick.ResponsePort.Post(DefaultUpdateResponseType.Instance);
battery.Get batteryGet;
yield return _nxtBatteryPort.Get(GetRequestType.Instance, out batteryGet);
_nxtBatteryState = batteryGet.ResponsePort;
if (_nxtBatteryState != null)
{
BatteryPower = _nxtBatteryState.PercentBatteryPower;
}
yield break;
}
void TimerHandler(DateTime signal)
{
_mainPort.Post(new TimerTick());
Activate(
Arbiter.Receive(false, TimeoutPort(3000),
delegate(DateTime time)
{
_timerPort.Post(time);
}
)
);
}
DriveEncoderUpdate
to retrieve the information from the servo motors.
private void DriveEncoderHandler(drive.DriveEncodersUpdate statistics)
{
LeftEncoderCurrent = statistics.Body.LeftEncoderCurrent;
RightEncoderCurrent = statistics.Body.RightEncoderCurrent;
LeftPowerCurrent = statistics.Body.LeftPowerCurrent;
RightPowerCurrent = statistics.Body.RightPowerCurrent;
}
Create an enum
named “DriveAction
” in the common class library project. This is to handle the keyboard or the click events from the UI Layer.
public enum DriveAction
{
Front,
Back,
Left,
Right,
Stop
}
Implement the Drive
method in the service.
public void Drive(DriveAction driveAction)
{
switch (driveAction)
{
case DriveAction.Front:
_nxtSetDriveRequest.LeftPower = -GearPower;
_nxtSetDriveRequest.RightPower = -GearPower;
_nxtDrivePort.DriveDistance(_nxtSetDriveRequest);
break;
case DriveAction.Back:
_nxtSetDriveRequest.LeftPower = GearPower;
_nxtSetDriveRequest.RightPower = GearPower;
_nxtDrivePort.DriveDistance(_nxtSetDriveRequest);
break;
case DriveAction.Left:
_nxtSetDriveRequest.LeftPower = -.4;
_nxtSetDriveRequest.RightPower = .4;
_nxtDrivePort.DriveDistance(_nxtSetDriveRequest);
break;
case DriveAction.Right:
_nxtSetDriveRequest.LeftPower = .4;
_nxtSetDriveRequest.RightPower = -.4;
_nxtDrivePort.DriveDistance(_nxtSetDriveRequest);
break;
case DriveAction.Stop:
_nxtDrivePort.AllStop(MotorStopState.Coast);
break;
default:
break;
}
}
Hope this helps in creating a service for controlling your Bot.