123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420 |
- using System;
-
- namespace monitor
- {
- public class DestijlCommandList
- {
- public const string HeaderMtsComDmb = "COM";
- public const string HeaderMtsDmbOrder = "DMB";
- public const string HeaderMtsCamera = "CAM";
- public const string HeaderMtsMessage = "MSG";
-
- public const string DataComOpen = "o";
- public const string DataComClose = "C";
-
- public const string DataCamOpen = "A";
- public const string DataCamClose = "I";
- public const string DataCamAskArena = "y";
- public const string DataCamArenaConfirm = "x";
- public const string DataCamInfirm = "z";
- public const string DataCamComputePosition = "p";
- public const string DataCamStopComputePosition = "s";
-
- public const string HeaderStmAck = "ACK";
- public const string HeaderStmNoAck = "NAK";
- public const string HeaderStmLostDmb = "LCD";
- public const string HeaderStmImage = "IMG";
- public const string HeaderStmPos = "POS";
- public const string HeaderStmMes = "MSG";
- public const string HeaderStmBat = "BAT";
- }
-
- public class RobotCommandList
- {
- public const string RobotPing = "p";
- public const string RobotReset = "r";
- public const string RobotStartWithoutWatchdog = "u";
- public const string RobotStartWithWatchdog = "W";
- public const string RobotGetBattery = "v";
- public const string RobotGetBusyState = "b";
- public const string RobotMove = "M";
- public const string RobotTurn = "T";
- public const string RobotGetVersion = "V";
- public const string RobotPowerOff = "z";
- }
-
- public class DestijlCommandManager
- {
- private CommandManager commandManager = null;
-
- private string receivedHeader = null;
- private string receivedData = null;
-
- public delegate void CommandReceivedEvent(string header, string data);
- public CommandReceivedEvent commandReceivedEvent = null;
-
- public double timeout = 100; // timeout pour les commandes avec acquitement
-
- public enum CommandStatus
- {
- Success,
- Rejected,
- InvalidAnswer,
- Busy,
- CommunicationLostWithRobot,
- CommunicationLostWithServer
- }
-
- public DestijlCommandManager(CommandReceivedEvent callback)
- {
- commandManager = new CommandManager(OnCommandReceived);
- this.commandReceivedEvent += callback;
- }
-
- ~DestijlCommandManager()
- {
- if (commandManager != null) commandManager.Close();
- }
-
- private void OnCommandReceived(string msg)
- {
- string[] msgs = msg.Split(':');
-
- if (msgs.Length >= 1) receivedHeader = msgs[0];
- else receivedHeader = null;
-
- if (msgs.Length >= 2) receivedData = msgs[1];
- else receivedData = null;
-
- this.commandReceivedEvent?.Invoke(receivedHeader, receivedData);
- }
-
- public bool Open(string hostname)
- {
- return this.Open(hostname, Client.defaultPort);
- }
-
- public bool Open(string hostname, int port)
- {
- if (commandManager != null) return commandManager.Open(hostname, port);
- else return false;
- }
-
- public void Close()
- {
- if (commandManager != null) commandManager.Close();
- }
-
- private string CreateCommand(string header, string data)
- {
- return header + ":" + data;
- }
-
- private void SplitCommand(string cmd, out string header, out string data)
- {
- string[] cmdParts = cmd.Split(':');
-
- if (cmdParts.Length > 0) header = cmdParts[0];
- else header = null;
-
- if (cmdParts.Length > 1) data = cmdParts[1];
- else data = null;
- }
-
- private CommandStatus DecodeStatus(CommandManager.CommandManagerStatus localStatus, string answer)
- {
- CommandStatus status = CommandStatus.Success;
-
- if (localStatus == CommandManager.CommandManagerStatus.Timeout) status = CommandStatus.CommunicationLostWithServer;
- else if (localStatus == CommandManager.CommandManagerStatus.Busy) status = CommandStatus.Busy;
- else
- {
- if (answer != null)
- {
- if (answer.ToUpper().Contains(DestijlCommandList.HeaderStmNoAck)) status = CommandStatus.Rejected;
- else if (answer.ToUpper().Contains(DestijlCommandList.HeaderStmLostDmb)) status = CommandStatus.CommunicationLostWithRobot;
- else if (answer.ToUpper().Contains(DestijlCommandList.HeaderStmAck)) status = CommandStatus.Success;
- else if (answer.Length == 0) status = CommandStatus.CommunicationLostWithServer;
- else status = CommandStatus.InvalidAnswer;
- }
- }
-
- return status;
- }
-
- public CommandStatus RobotOpenCom()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsComDmb, DestijlCommandList.DataComOpen),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotCloseCom()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsComDmb, DestijlCommandList.DataComClose),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotPing()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotPing),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotReset()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotReset),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotStartWithWatchdog()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotStartWithWatchdog),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotStartWithoutWatchdog()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotStartWithoutWatchdog),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotMove(int distance)
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotMove + "=" + distance),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotTurn(int angle)
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotTurn + "=" + angle),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- //public CommandStatus RobotGetBattery(out int battery)
- public CommandStatus RobotGetBattery()
- {
- CommandManager.CommandManagerStatus localStatus;
- //CommandStatus status = CommandStatus.Success;
-
- //battery = -1;
-
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotGetBattery),
- out answer,
- 0);
-
- //if (localStatus == CommandManager.CommandManagerStatus.AnswerReceived) {
- // string[] msg = answer.Split(':');
-
- // if (msg.Length > 1)
- // {
- // try
- // {
- // battery = Convert.ToInt32(msg[1]);
- // }
- // catch (Exception) { }
- // }
- //}
- //else if (localStatus == CommandManager.CommandManagerStatus.Timeout)
- //{
- // status = CommandStatus.CommunicationLostWithServer;
- //}
-
- //return status;
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus RobotGetVersion(out string version)
- {
- CommandManager.CommandManagerStatus localStatus;
- CommandStatus status = CommandStatus.Success;
-
- version = "";
-
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotGetVersion),
- out answer,
- this.timeout);
-
- if (localStatus == CommandManager.CommandManagerStatus.AnswerReceived)
- {
- string[] msg = answer.Split(':');
-
- if (msg.Length > 1)
- {
- version = msg[1];
- }
- }
- else if (localStatus == CommandManager.CommandManagerStatus.Timeout)
- {
- status = CommandStatus.CommunicationLostWithServer;
- }
-
- return status;
- }
-
- public CommandStatus RobotPowerOff()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotPowerOff),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus CameraOpen()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamOpen),
- out answer,
- this.timeout);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus CameraClose()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamClose),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus CameraAskArena()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamAskArena),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus CameraArenaConfirm()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamArenaConfirm),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus CameraArenaInfirm()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamInfirm),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus CameraComputePosition()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamComputePosition),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
-
- public CommandStatus CameraStopComputePosition()
- {
- CommandManager.CommandManagerStatus localStatus;
- string answer;
-
- localStatus = commandManager.SendCommand(
- CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamStopComputePosition),
- out answer,
- 0);
-
- return DecodeStatus(localStatus, answer);
- }
- }
- }
|