Gokulnath's weBLOG - My Lego Bot

Friday, November 05, 2010

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). 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:
Lego Mindstorms NXT 2.0 Kit
Microsoft Robotics Developer Studio
Network/IP Camera (I used my iPhone with WiFi Camera app installed on it)
WPF Dashboard Controls

Download Source Code:
MyLegoCar.zip (388.43 KB)

Create a MRDS Service and add the partners - NxtDrive and NxtBattery. Adding the partners 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.

        /// <summary>

        /// NxtBattery partner

        /// </summary>

        [Partner("NxtBattery", Contract = battery.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]

        battery.BatteryOperations _nxtBatteryPort = new battery.BatteryOperations();

        /// <summary>

        /// NxtDrive partner

        /// </summary>

        [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. VS will choose x86 as the targeted platform by default, change it to "Any CPU". 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 toinvoke 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);

            // invoke the UI

            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;

            // Subscribe to partners

            var subscribe1 = this._nxtDrivePort.Subscribe(_nxtDriveNotify);

            yield return (Choice)subscribe1;

            _timerPort.Post(DateTime.Now);

            // Activate independent tasks

            Activate<ITask>(

                Arbiter.Receive<drive.DriveEncodersUpdate>(true, _nxtDriveNotify, DriveEncoderHandler),

                Arbiter.Receive(true, _timerPort, TimerHandler)

);

            // Start operation handlers and insert into directory service.

            StartHandlers();

        private void StartHandlers()

            // Activate message handlers for this service and insert into the directory.

            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;

I suppose I’ve explained most of the important parts in the service. Please let me know if you have any questions.

My bot in action:

demo

Friday, November 05, 2010 4:08:10 PM (GMT Standard Time, UTC+00:00)

Comments [2] -
.NET | Robotics

Friday, July 08, 2011 7:29:53 PM (GMT Daylight Time, UTC+01:00)

This looks really good . Did you integrate the mobile camera with MSRS? if yes how are able to get the frames from the mobile camera to MSRS.

Robtics is my hobby please visit my site http://programmablerobots.blogspot.com/

George Peter

Friday, July 08, 2011 11:41:32 PM (GMT Daylight Time, UTC+01:00)

@George:

I connected my iPhone to my wifi network. The wifi camera app (installed in my phone) had an html page, I just customized the page based on my needs and hosted it in my PC's web server. Then, I integrated the html page on the WPF application using web browser control.

Between, your robotic projects on your site are amazing!! keep up the good work, quite motivating!! I haven't touched the MRDS' virtual environment yet, I'm thinking of starting of a project with it. Feel free to contact me if you have any questions!!

Gokul

Comments are closed.