StartSimulationEngine  "SimState/basicsim.xml"


AddNewEntity    wall1  	/Position:-0.5  0.25  -2.5
AddBoxShape  	/Dimensions:3.0  0.5  0.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"

AddNewEntity    wall2  	/Position:-0.5  0.25  2.5
AddBoxShape  	/Dimensions:3.0  0.5  0.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"

AddNewEntity    wall3  	/Position:-2.5  0.25  0
AddBoxShape  	/Dimensions:0.5  0.5  4.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"

AddNewEntity    wall4  	/Position:2.5  0.25  0
AddBoxShape  	/Dimensions:0.5  0.5  4.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"



AddDifferentialDriveEntity    myrobot

AddBumperEntity    bumper1	/ParentEntity:myrobot
	/FrontPosition:0    0.1    -0.3
	/RearPosition:0    0.1    0.3

AddLaserRangeFinderEntity    lrf1	/Position:0  0.4  0	/ParentEntity:myrobot

FlushScript  

wait 10000
SaveSceneAs    "SimState/boxsim.xml"  

dssprojectmigration "here_your_unzipped_folder_name"	

using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using System.IO;

using dssp = Microsoft.Dss.ServiceModel.Dssp;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using simengine = Microsoft.Robotics.Simulation.Engine;
using physics = Microsoft.Robotics.Simulation.Physics;

using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;


namespace SimulationFromState3
{
	[Contract(Contract.Identifier)]
	[DisplayName("SimulationFromState3")]
	[Description("SimulationFromState3 service (no description provided)")]
	class SimulationFromState3Service : DsspServiceBase
	{        
        simengine.SimulationEnginePort _notificationTarget;

        DifferentialDriveEntity _robot = null;

		[ServiceState]
		SimulationFromState3State _state = new SimulationFromState3State();
		
		[ServicePort("/SimulationFromState3", AllowMultipleInstances = true)]
		SimulationFromState3Operations _mainPort = new SimulationFromState3Operations();
		
		public SimulationFromState3Service(DsspServiceCreationPort creationPort)
			: base(creationPort)
		{
		}

        protected override void Start()
        {
            _notificationTarget = new simengine.SimulationEnginePort();

            base.Start();

            MainPortInterleave.CombineWith(
                new Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                        Arbiter.Receive(true, _notificationTarget, InsertEntityNotificationHandler),
                        Arbiter.Receive(true, _notificationTarget, DeleteEntityNotificationHandler)                        
                    ),
                    new ConcurrentReceiverGroup()
                )
            );


            //Start simulation engine from the state file
            string filename = "SimState/boxsim.xml";

            dssp.PartnerType pt = new dssp.PartnerType();
            System.Xml.XmlQualifiedName qName = 
            	new System.Xml.XmlQualifiedName("StateService", "http://schemas.microsoft.com/xw/2004/10/dssp.html");
            pt.Name = qName;
            pt.Service = filename;
            pt.Contract = "http://schemas.microsoft.com/robotics/2006/04/simulationengine.html";

            engineproxy.Contract.CreateService(ConstructorPort, new PartnerType[] { pt });


            //Request to robot platform bilding
            simengine.EntitySubscribeRequestType esrt = new simengine.EntitySubscribeRequestType();
            esrt.Name = "myrobot";
            simengine.SimulationEngine.GlobalInstancePort.Subscribe(esrt, _notificationTarget);
        }



        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {           
            if (ins.Body.State.Name == "myrobot")
            {
                _robot = (DifferentialDriveEntity)ins.Body;

                LogInfo(LogGroups.Console, _robot.State.Name + " was connected!");

                SpawnIterator(ConnectSensors);
            }
        }
        

        void DeleteEntityNotificationHandler(simengine.DeleteSimulationEntity del)
        {
            if (del.Body.State.Name == "myrobot")
            {
                _robot = null;
            }
        }  


        public IEnumerator ConnectSensors()
        {
            //Start driving
            SpawnIterator(DriveRobot);

            yield break;
        }

       
        public IEnumerator DriveRobot()
        {
            if (_robot != null)
            {
                Port entityResponse = new Port();

                DriveAction da = new DriveAction();
                da.DiffDrive = _robot;
                da.EntityResponse = entityResponse;

                Task deferredTask = new Task(da, ExecuteDriveDistance);

                while (true)
                {

                    //Go forwards
                    da.Distance = 0.5f;
                    da.MotorPower = 0.2f;

                    deferredTask = new Task(da, ExecuteDriveDistance);
                    da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                    yield return Arbiter.Receive(false, entityResponse,
                        delegate(simengine.OperationResult result)
                        {
                        }
                    );

                    //Turn 90 degrees with 0.2 power
                    da.Degrees = 90.0f;
                    da.MotorPower = 0.2f;

                    deferredTask = new Task(da, ExecuteRotateDegrees);
                    da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                    yield return Arbiter.Receive(false, entityResponse,
                        delegate(simengine.OperationResult result)
                        {
                        }
                    );

                    //Go forwards with 0.2 and 0.2 powers                        
                    da.LeftPower = 0.2f;
                    da.RightPower = 0.2f;

                    deferredTask = new Task(da, ExecuteSetDrivePower);
                    da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                    //Wait 1 sec
                    yield return Arbiter.Receive(false, TimeoutPort(1000), delegate { });
                }
            }

            yield break;
        }

        void ExecuteSetDrivePower(DriveAction da)
        {
            if (da.DiffDrive.IsEnabled)
                da.DiffDrive.IsEnabled = true;

            da.DiffDrive.SetMotorTorque(da.LeftPower, da.RightPower);
        }


        void ExecuteRotateDegrees(DriveAction da)
        {
            if (da.DiffDrive.IsEnabled)
                da.DiffDrive.IsEnabled = true;

            da.DiffDrive.RotateDegrees(da.Degrees, da.MotorPower, da.EntityResponse);
        }

        void ExecuteDriveDistance(DriveAction da)
        {
            if (da.DiffDrive.IsEnabled)
                da.DiffDrive.IsEnabled = true;

            da.DiffDrive.DriveDistance(da.Distance, da.MotorPower, da.EntityResponse);
        }
	}


    public class DriveAction
    {
        public DifferentialDriveEntity DiffDrive = null;

        public float MotorPower = 0.0f;

        public float LeftPower = 0.0f;

        public float RightPower = 0.0f;

        public float Degrees = 0.0f;

        public float Distance = 0.0f;

        public Port EntityResponse = null;
    }
}

using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;

namespace SimulationFromState3
{
	public sealed class Contract
	{
		[DataMember]
		public const string Identifier = "http://schemas.tempuri.org/2009/12/simulationfromstate3.html";
	}
	
	[DataContract]
	public class SimulationFromState3State
	{
	}
	
	[ServicePort]
	public class SimulationFromState3Operations : PortSet
	{
	}
	
	public class Get : Get>
	{
		public Get()
		{
		}
		
		public Get(GetRequestType body)
			: base(body)
		{
		}
		
		public Get(GetRequestType body, PortSet responsePort)
			: base(body, responsePort)
		{
		}
	}
}