Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 89 → Rev 90

/Microsoft Robotics Studio/MSRS Service/RoboBoardService/RoboBoard.cs
0,0 → 1,520
//------------------------------------------------------------------------------
// <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();
 
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> 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 + ")");});
}
}
}
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;
}
 
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;
}
 
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;
}
 
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);
}
);
 
yield break;
}
 
//---------------------------------------------------------------------
IEnumerator<ITask> OnLoadHandler(OnLoad onLoad)
{
_rbForm = onLoad.RoboBoardForm;
LogInfo("Loaded Form");
sc.ComPort = "COM6";
if (_rbForm != null)
{
if (sc.InitPort() == 0)
{
WinFormsServicePort.FormInvoke(delegate()
{_rbForm.UpdateStatus("Port Open (" + sc.ComPort + ")");});
}
else
{
WinFormsServicePort.FormInvoke(delegate()
{_rbForm.UpdateStatus("Can't open communication port (" + sc.ComPort + ")");});
}
}
//request version
MKRequestVersion();
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()
{
string message = "";
message=sc.CreateMessage((char)'v', (char)0, message);
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); });
}
}
}
}