Subversion Repositories Projects

//------------------------------------------------------------------------------
// <auto-generated>
//     This code was generated by a tool.
//     Runtime Version:2.0.50727.312
//
//     Changes to this file may cause incorrect behavior and will be lost if
//     the code is regenerated.
// </auto-generated>
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Core.DsspHttp;
using System;
using System.Collections.Generic;
using System.Security.Permissions;
using System.ComponentModel;
using System.Xml;
using roboboard = Robotics.RoboBoard;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using Roboboard = Robotics.Serial;
using Microsoft.Ccr.Adapters.WinForms;
using soap = W3C.Soap;
using dssp = Microsoft.Dss.ServiceModel.Dssp;
using xml = System.Xml;
using W3C.Soap;


namespace Robotics.RoboBoard
{
    /// <summary>
    /// Implementation class for RoboBoard
    /// </summary>
    [DisplayName("RoboBoard")]
    [Description("The RoboBoard Service")]
    [Contract(Contract.Identifier)]
    public class RoboBoardService : DsspServiceBase
    {
        RoboBoardForm _rbForm;
        RoboBoardEvents _eventsPort = new RoboBoardEvents();

        bool subs = false;
        bool isloaded = false;

        private Roboboard.SerialCom sc = new Robotics.Serial.SerialCom();
        /// <summary>
        /// _state
        /// </summary>
        private RoboBoardState _state = new RoboBoardState();
        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/roboboard", AllowMultipleInstances=false)]
        private RoboBoardOperations _mainPort = new RoboBoardOperations();

        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        //---------------------------------------------------------------------
        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public RoboBoardService(DsspServiceCreationPort creationPort) :
                base(creationPort)
        {
        }
       
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            Activate(Arbiter.Interleave(
                new TeardownReceiverGroup
                (
                ),
                new ExclusiveReceiverGroup
                (
                    Arbiter.ReceiveWithIterator<OnLoad>(true, _eventsPort, OnLoadHandler)
                ),
                new ConcurrentReceiverGroup
                (
                )
            ));
           
            base.Start();
            LogInfo("Init Service..");
                        // Add service specific initialization here.
           
            // Open Status and Debug Dialog
            WinFormsServicePort.Post(new RunForm(CreateForm));

            Roboboard.SerialCom.BotEvent +=new Robotics.Serial.SerialCom.BotEventHandler(sc_BotEvent);
        }

        System.Windows.Forms.Form CreateForm()
        {
            return new RoboBoardForm(_eventsPort);
        }


        //---------------------------------------------------------------------
        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        //---------------------------------------------------------------------
        /// <summary>
        /// Http Get Handler
        /// </summary>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(_state));
            yield break;
        }

        //---------------------------------------------------------------------
        /// <summary>
        /// Replace Handler
        /// </summary>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> ReplaceHandler(Replace replace)
        {
            _state = replace.Body;

//            #region CODECLIP 04-1
//            base.SendNotification(_submgrPort, replace);
//            #endregion

            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> InitComplete(InitComplete initComplete)
        {
            LogInfo("Init");
            LogInfo("Command:" + initComplete.Body.Command);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> SetComPortHandler(SetComPort setComPort)
        {
            LogInfo("Set Port Name ("+setComPort.Body.PortName+")");
            if ((setComPort.Body.PortName == sc.ComPort)&&(sc.isOpen==true))
            {
                LogInfo("no need to change com port");
                // port name equals the old port name - no action
            }
            else
            {
                // port name is different, close the old port and open the new one
                if(sc.isOpen==true) {
                    LogInfo("closing Port");
                    sc.ClosePort();
                }
                sc.ComPort = setComPort.Body.PortName;
                if (_rbForm != null)
                {
                    if (sc.InitPort() == 0)
                    {
                        LogInfo("Init Port succseeded");
                        WinFormsServicePort.FormInvoke(delegate()
                        { _rbForm.UpdateStatus("Port Open (" + sc.ComPort + ")"); });
                    }
                    else
                    {
                        LogInfo("Init Port failed");
                        WinFormsServicePort.FormInvoke(delegate()
                        { _rbForm.UpdateStatus("Can't open communication port (" + sc.ComPort + ")"); });
                    }
                }
                else
                {
                    LogInfo("Board communication not ready! Setport Ignored");
                }
            }
            LogInfo("Exit Set Comport");
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> IncrementTickHandler(IncrementTick incrementTick)
        {
            LogInfo("Tick: ");
            LogInfo("Command:" + incrementTick.Body.Command);
       
            base.SendNotification(_submgrPort,  incrementTick);

            incrementTick.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> ReceiveCommandHandler(ReceiveCommand receiveCommand)
        {
            LogInfo("Receive ");
            LogInfo("Command:" + receiveCommand.Body.Command);

            base.SendNotification(_submgrPort, receiveCommand);

            receiveCommand.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> SendCommandHandler(SendCommand sendCommand)
        {
            LogInfo("Send ");
            LogInfo("Command:" + sendCommand.Body.Command);

            MKRequestTunnelMessage(sendCommand.Body.Command);
                       
            base.SendNotification(_submgrPort, sendCommand);
            sendCommand.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> LevelHandler(Level level)
        {
            LogInfo("Level ");
            LogInfo("Nick:"+level.Body.Nick+"Roll:"+level.Body.Roll);
            base.SendNotification(_submgrPort, level);
            level.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> SetAltituteHandler(SetAltitute setaltitute)
        {
            LogInfo("SetAltitute ");
            LogInfo("Altitute:" + setaltitute.Body.Altitute);
            if (_rbForm != null)
            {
                _rbForm.Control_alt = (Int16)setaltitute.Body.Altitute;
            }
            //TODO: send alt control to MK
            base.SendNotification(_submgrPort, setaltitute);
            setaltitute.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> SetGierHandler(SetGier setgier)
        {
            LogInfo("SetGier ");
            LogInfo("SetGier:" + setgier.Body.Gier);
            Int16 igier = (Int16)(((float)128 * setgier.Body.Gier) / (float)1000);
            if (_rbForm != null)
            {
                _rbForm.Control_gier = igier;
            }

            if (MK_latest_gier != igier)
            {
                MK_latest_gier = igier;
                MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas);
            }
            base.SendNotification(_submgrPort, setgier);
            setgier.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> SetGasHandler(SetGas setgas)
        {
            LogInfo("SetGas ");
            LogInfo("SetGas:" + setgas.Body.Gas);
            Int16 igas = (Int16)(((float)128 * setgas.Body.Gas) / (float)1000);
            if (_rbForm != null)
            {
               _rbForm.Control_gas = igas;
            }

            if (MK_latest_gas != igas)
            {
                MK_latest_gas = igas;
                MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas);
            }

            base.SendNotification(_submgrPort, setgas);
            setgas.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
     
        public IEnumerator<ITask> ControlHandler(Control control)
        {
            LogInfo("Control ");
            LogInfo("Nick:" + control.Body.Nick + " Roll:" +control.Body.Roll);

            Int16 inick = (Int16)(((float)128 * control.Body.Nick) / (float)(-1000));
            Int16 iroll = (Int16)(((float)128 * control.Body.Roll) / (float)(-1000));

            if (_rbForm != null)
            {
                _rbForm.Control_nick = inick;
                _rbForm.Control_roll = iroll;
            }

            if ((MK_latest_roll != iroll) || (MK_latest_nick != inick))
            {
                MK_latest_nick = inick;
                MK_latest_roll = iroll;
                MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas);
            }

            base.SendNotification(_submgrPort, control);
            control.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //---------------------------------------------------------------------
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> SubscribeHandler(Subscribe subscribe)
        {
            SubscribeRequestType request = subscribe.Body;
            LogInfo("Subscribe request from: " + request.Subscriber);

            yield return Arbiter.Choice(
                SubscribeHelper(_submgrPort, request, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    base.SendNotification<Replace>(_submgrPort, request.Subscriber, _state);
                },
                delegate(Exception e)
                {
                    LogError(null, "Subscribe failed", e);
                }
            );
            if (isloaded)
            {
                base.SendNotification(_submgrPort, new InitComplete("OK"));
            }
            subs = true;
            yield break;
        }

        //---------------------------------------------------------------------
        IEnumerator<ITask> OnLoadHandler(OnLoad onLoad)
        {
            _rbForm = onLoad.RoboBoardForm;
            LogInfo("Loaded Form");
            sc.ComPort = "COM7";
            if (_rbForm != null)
            {
                if (sc.InitPort() == 0)
                {
                    LogInfo("Port Open");
                    WinFormsServicePort.FormInvoke(delegate()
                    {_rbForm.UpdateStatus("Port Open (" + sc.ComPort + ")");});
                }
                else
                {
                    LogInfo("Port failed to Open");
                    WinFormsServicePort.FormInvoke(delegate()
                    {_rbForm.UpdateStatus("Can't open communication port (" + sc.ComPort + ")");});
                }
            }
            //request version
            MKRequestVersion();
            if (subs)
            {
                base.SendNotification(_submgrPort, new InitComplete("OK"));
            }
            isloaded = true;
            yield break;
        }

        //---------------------------------------------------------------------
        void MKRequestTunnelMessage(string message)
        {
            message = sc.CreateMessage((char)'X', (char)0, message);
            sc.SerialWrite(message);
        }

        //---------------------------------------------------------------------
        bool MKRequestSend = false;
        Int16 MK_old_roll = 0;
        Int16 MK_old_nick = 0;
        Int16 MK_old_gier = 0;
        Int16 MK_old_gas = 0;

        Int16 MK_latest_roll = 0;
        Int16 MK_latest_nick = 0;
        Int16 MK_latest_gier = 0;
        Int16 MK_latest_gas = 0;

        void MKRequestControl(Int16 nick, Int16 roll, Int16 gier, Int16 gas)
        {
            string message = "";
            if ((MK_old_gas == gas) &&
                (MK_old_gier == gier) &&
                (MK_old_nick == nick) &&
                (MK_old_roll == roll))
            {
                LogInfo("no update");
                return;
            }
            if (MKRequestSend == true) return;
            LogInfo("Send");
            if (_rbForm != null)
            {
                WinFormsServicePort.FormInvoke(delegate()
                { _rbForm.MonitorMKSensorUpdate(); });
            }
            message = sc.MessageAddInt16(roll, message);
            MK_old_roll = roll;
            message = sc.MessageAddInt16(nick, message);
            MK_old_nick = nick;
            message = sc.MessageAddInt16(gier, message);
            MK_old_gier = gier;
            message = sc.MessageAddInt16(gas, message);
            MK_old_gas = gas;
            message = sc.CreateMessage((char)'C', (char)0, message);
            MKRequestSend = true;
            sc.SerialWrite(message);
        }

        void MKDecodeControl()
        {
            LogInfo("Control OK");
            MKRequestSend = false;
            MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas);
        }
       
        //---------------------------------------------------------------------
        void MKRequestVersion()
        {
            LogInfo("MK RequestVersion");
            string message = "";
            message=sc.CreateMessage((char)'v', (char)0, message);
            if (sc.isOpen)
            {
                LogInfo("Version Send");
                sc.SerialWrite(message);
            }
        }

        string MKDecodeVersion(string message)
        {
            string Versionstring = "";
            Versionstring += "Version: "+sc.MessageReadByte(message).ToString();
            message=message.Substring(1);
            sc.MessageReadByte(message);
            Versionstring += "." + sc.MessageReadByte(message).ToString();
            message = message.Substring(1);
            sc.MessageReadByte(message);
            Versionstring += " (STEP:" + sc.MessageReadByte(message).ToString()+")";
            if (_rbForm != null)
            {
                WinFormsServicePort.FormInvoke(delegate()
                { _rbForm.UpdateStatus(Versionstring); });
            }
            return Versionstring;
        }

        //---------------------------------------------------------------------
        void MKDecodeDebugData(string message)
        {
            Int16[] analog = new Int16[32];
            byte[] digital = new byte[2];
            int i = 0;
            for (i = 0; i < 2; i++)
            {
                digital[i]=sc.MessageReadByte(message);
                message = message.Substring(1);
            }
            for (i = 0; i < 32; i++)
            {
                analog[i] = sc.MessageReadInt16(message);

                Int16 analogx = (Int16)analog[i];
                if (_rbForm != null)
                {
                    _rbForm.MK_analog[i]= analog[i];
                    _rbForm.Mk_analog_name[i] = "Analog" + i.ToString();
                        //WinFormsServicePort.FormInvoke(delegate()
                        //{ _rbForm.MonitorMKSensor(i,analogx,"Analog"+i.ToString()); });
                }
                message = message.Substring(2);
            }
            if (_rbForm != null)
            {
                WinFormsServicePort.FormInvoke(delegate()
                { _rbForm.MonitorMKSensorUpdate(); });
            }
            base.SendNotification(_submgrPort, new Level((int)analog[0], (int)analog[1]));
        }

        //---------------------------------------------------------------------
        void sc_BotEvent(object sender, EventArgs ev)
        {

            string message = "";
            char cmd = (char)0;
            char adr = (char)0;
            if (sc.IsMessageValid() == true)
            {
                message = sc.GetLastMessage();
                cmd = sc.GetMessageCmd();
                adr = sc.GetMessageAdr();
                    if (cmd.CompareTo((char)'V')==0)
                    {
                        MKDecodeVersion(message);
                        base.SendNotification(_submgrPort, new ReceiveCommand("V"+message));
                    }
                else
                    if (cmd.CompareTo((char)'D') == 0)
                    {
                        MKDecodeDebugData(message);
                        //base.SendNotification(_submgrPort, new ReceiveCommand("D"+message));
                    }
                else
                    if (cmd.CompareTo((char)'X') == 0)
                    {
                        base.SendNotification(_submgrPort, new ReceiveCommand(message));
                    }
                else
                    if (cmd.CompareTo((char)'C') == 0)
                    {
                        MKDecodeControl();                      
                    }

                sc.InvalidateMessage();
            }

            if (_rbForm != null)
            {
                WinFormsServicePort.FormInvoke(delegate()
                { _rbForm.MonitorCommand(message); });
            }
        }
    }
}