Rev 90 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 90 | Rev 97 | ||
---|---|---|---|
1 | //------------------------------------------------------------------------------ |
1 | //------------------------------------------------------------------------------ |
2 | // <auto-generated> |
2 | // <auto-generated> |
3 | // This code was generated by a tool. |
3 | // This code was generated by a tool. |
4 | // Runtime Version:2.0.50727.312 |
4 | // Runtime Version:2.0.50727.312 |
5 | // |
5 | // |
6 | // Changes to this file may cause incorrect behavior and will be lost if |
6 | // Changes to this file may cause incorrect behavior and will be lost if |
7 | // the code is regenerated. |
7 | // the code is regenerated. |
8 | // </auto-generated> |
8 | // </auto-generated> |
9 | //------------------------------------------------------------------------------ |
9 | //------------------------------------------------------------------------------ |
10 | 10 | ||
11 | using Microsoft.Ccr.Core; |
11 | using Microsoft.Ccr.Core; |
12 | using Microsoft.Dss.Core; |
12 | using Microsoft.Dss.Core; |
13 | using Microsoft.Dss.Core.Attributes; |
13 | using Microsoft.Dss.Core.Attributes; |
14 | using Microsoft.Dss.ServiceModel.Dssp; |
14 | using Microsoft.Dss.ServiceModel.Dssp; |
15 | using Microsoft.Dss.ServiceModel.DsspServiceBase; |
15 | using Microsoft.Dss.ServiceModel.DsspServiceBase; |
16 | using Microsoft.Dss.Core.DsspHttp; |
16 | using Microsoft.Dss.Core.DsspHttp; |
17 | using System; |
17 | using System; |
18 | using System.Collections.Generic; |
18 | using System.Collections.Generic; |
19 | using System.Security.Permissions; |
19 | using System.Security.Permissions; |
20 | using System.ComponentModel; |
20 | using System.ComponentModel; |
21 | using System.Xml; |
21 | using System.Xml; |
22 | using roboboard = Robotics.RoboBoard; |
22 | using roboboard = Robotics.RoboBoard; |
23 | using submgr = Microsoft.Dss.Services.SubscriptionManager; |
23 | using submgr = Microsoft.Dss.Services.SubscriptionManager; |
24 | using Roboboard = Robotics.Serial; |
24 | using Roboboard = Robotics.Serial; |
25 | using Microsoft.Ccr.Adapters.WinForms; |
25 | using Microsoft.Ccr.Adapters.WinForms; |
26 | using soap = W3C.Soap; |
26 | using soap = W3C.Soap; |
27 | using dssp = Microsoft.Dss.ServiceModel.Dssp; |
27 | using dssp = Microsoft.Dss.ServiceModel.Dssp; |
28 | using xml = System.Xml; |
28 | using xml = System.Xml; |
29 | using W3C.Soap; |
29 | using W3C.Soap; |
30 | 30 | ||
31 | 31 | ||
32 | namespace Robotics.RoboBoard |
32 | namespace Robotics.RoboBoard |
33 | { |
33 | { |
34 | /// <summary> |
34 | /// <summary> |
35 | /// Implementation class for RoboBoard |
35 | /// Implementation class for RoboBoard |
36 | /// </summary> |
36 | /// </summary> |
37 | [DisplayName("RoboBoard")] |
37 | [DisplayName("RoboBoard")] |
38 | [Description("The RoboBoard Service")] |
38 | [Description("The RoboBoard Service")] |
39 | [Contract(Contract.Identifier)] |
39 | [Contract(Contract.Identifier)] |
40 | public class RoboBoardService : DsspServiceBase |
40 | public class RoboBoardService : DsspServiceBase |
41 | { |
41 | { |
42 | RoboBoardForm _rbForm; |
42 | RoboBoardForm _rbForm; |
43 | RoboBoardEvents _eventsPort = new RoboBoardEvents(); |
43 | RoboBoardEvents _eventsPort = new RoboBoardEvents(); |
- | 44 | ||
- | 45 | bool subs = false; |
|
- | 46 | bool isloaded = false; |
|
44 | 47 | ||
45 | private Roboboard.SerialCom sc = new Robotics.Serial.SerialCom(); |
48 | private Roboboard.SerialCom sc = new Robotics.Serial.SerialCom(); |
46 | /// <summary> |
49 | /// <summary> |
47 | /// _state |
50 | /// _state |
48 | /// </summary> |
51 | /// </summary> |
49 | private RoboBoardState _state = new RoboBoardState(); |
52 | private RoboBoardState _state = new RoboBoardState(); |
50 | /// <summary> |
53 | /// <summary> |
51 | /// _main Port |
54 | /// _main Port |
52 | /// </summary> |
55 | /// </summary> |
53 | [ServicePort("/roboboard", AllowMultipleInstances=false)] |
56 | [ServicePort("/roboboard", AllowMultipleInstances=false)] |
54 | private RoboBoardOperations _mainPort = new RoboBoardOperations(); |
57 | private RoboBoardOperations _mainPort = new RoboBoardOperations(); |
55 | 58 | ||
56 | [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)] |
59 | [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)] |
57 | private submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort(); |
60 | private submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort(); |
58 | 61 | ||
59 | //--------------------------------------------------------------------- |
62 | //--------------------------------------------------------------------- |
60 | /// <summary> |
63 | /// <summary> |
61 | /// Default Service Constructor |
64 | /// Default Service Constructor |
62 | /// </summary> |
65 | /// </summary> |
63 | public RoboBoardService(DsspServiceCreationPort creationPort) : |
66 | public RoboBoardService(DsspServiceCreationPort creationPort) : |
64 | base(creationPort) |
67 | base(creationPort) |
65 | { |
68 | { |
66 | } |
69 | } |
67 | 70 | ||
68 | /// <summary> |
71 | /// <summary> |
69 | /// Service Start |
72 | /// Service Start |
70 | /// </summary> |
73 | /// </summary> |
71 | protected override void Start() |
74 | protected override void Start() |
72 | { |
75 | { |
73 | Activate(Arbiter.Interleave( |
76 | Activate(Arbiter.Interleave( |
74 | new TeardownReceiverGroup |
77 | new TeardownReceiverGroup |
75 | ( |
78 | ( |
76 | ), |
79 | ), |
77 | new ExclusiveReceiverGroup |
80 | new ExclusiveReceiverGroup |
78 | ( |
81 | ( |
79 | Arbiter.ReceiveWithIterator<OnLoad>(true, _eventsPort, OnLoadHandler) |
82 | Arbiter.ReceiveWithIterator<OnLoad>(true, _eventsPort, OnLoadHandler) |
80 | ), |
83 | ), |
81 | new ConcurrentReceiverGroup |
84 | new ConcurrentReceiverGroup |
82 | ( |
85 | ( |
83 | ) |
86 | ) |
84 | )); |
87 | )); |
85 | 88 | ||
86 | base.Start(); |
89 | base.Start(); |
87 | LogInfo("Init Service.."); |
90 | LogInfo("Init Service.."); |
88 | // Add service specific initialization here. |
91 | // Add service specific initialization here. |
89 | 92 | ||
90 | // Open Status and Debug Dialog |
93 | // Open Status and Debug Dialog |
91 | WinFormsServicePort.Post(new RunForm(CreateForm)); |
94 | WinFormsServicePort.Post(new RunForm(CreateForm)); |
92 | 95 | ||
93 | Roboboard.SerialCom.BotEvent +=new Robotics.Serial.SerialCom.BotEventHandler(sc_BotEvent); |
96 | Roboboard.SerialCom.BotEvent +=new Robotics.Serial.SerialCom.BotEventHandler(sc_BotEvent); |
94 | } |
97 | } |
95 | 98 | ||
96 | System.Windows.Forms.Form CreateForm() |
99 | System.Windows.Forms.Form CreateForm() |
97 | { |
100 | { |
98 | return new RoboBoardForm(_eventsPort); |
101 | return new RoboBoardForm(_eventsPort); |
99 | } |
102 | } |
100 | 103 | ||
101 | 104 | ||
102 | //--------------------------------------------------------------------- |
105 | //--------------------------------------------------------------------- |
103 | /// <summary> |
106 | /// <summary> |
104 | /// Get Handler |
107 | /// Get Handler |
105 | /// </summary> |
108 | /// </summary> |
106 | /// <param name="get"></param> |
109 | /// <param name="get"></param> |
107 | /// <returns></returns> |
110 | /// <returns></returns> |
108 | [ServiceHandler(ServiceHandlerBehavior.Concurrent)] |
111 | [ServiceHandler(ServiceHandlerBehavior.Concurrent)] |
109 | public virtual IEnumerator<ITask> GetHandler(Get get) |
112 | public virtual IEnumerator<ITask> GetHandler(Get get) |
110 | { |
113 | { |
111 | get.ResponsePort.Post(_state); |
114 | get.ResponsePort.Post(_state); |
112 | yield break; |
115 | yield break; |
113 | } |
116 | } |
114 | 117 | ||
115 | //--------------------------------------------------------------------- |
118 | //--------------------------------------------------------------------- |
116 | /// <summary> |
119 | /// <summary> |
117 | /// Http Get Handler |
120 | /// Http Get Handler |
118 | /// </summary> |
121 | /// </summary> |
119 | [ServiceHandler(ServiceHandlerBehavior.Concurrent)] |
122 | [ServiceHandler(ServiceHandlerBehavior.Concurrent)] |
120 | public IEnumerator<ITask> HttpGetHandler(HttpGet httpGet) |
123 | public IEnumerator<ITask> HttpGetHandler(HttpGet httpGet) |
121 | { |
124 | { |
122 | httpGet.ResponsePort.Post(new HttpResponseType(_state)); |
125 | httpGet.ResponsePort.Post(new HttpResponseType(_state)); |
123 | yield break; |
126 | yield break; |
124 | } |
127 | } |
125 | 128 | ||
126 | //--------------------------------------------------------------------- |
129 | //--------------------------------------------------------------------- |
127 | /// <summary> |
130 | /// <summary> |
128 | /// Replace Handler |
131 | /// Replace Handler |
129 | /// </summary> |
132 | /// </summary> |
130 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
133 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
131 | public IEnumerator<ITask> ReplaceHandler(Replace replace) |
134 | public IEnumerator<ITask> ReplaceHandler(Replace replace) |
132 | { |
135 | { |
133 | _state = replace.Body; |
136 | _state = replace.Body; |
134 | 137 | ||
135 | // #region CODECLIP 04-1 |
138 | // #region CODECLIP 04-1 |
136 | // base.SendNotification(_submgrPort, replace); |
139 | // base.SendNotification(_submgrPort, replace); |
137 | // #endregion |
140 | // #endregion |
138 | 141 | ||
139 | replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); |
142 | replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); |
140 | yield break; |
143 | yield break; |
141 | } |
144 | } |
142 | 145 | ||
143 | //--------------------------------------------------------------------- |
146 | //--------------------------------------------------------------------- |
144 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
147 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
- | 148 | public IEnumerator<ITask> InitComplete(InitComplete initComplete) |
|
- | 149 | { |
|
- | 150 | LogInfo("Init"); |
|
- | 151 | LogInfo("Command:" + initComplete.Body.Command); |
|
- | 152 | yield break; |
|
- | 153 | } |
|
- | 154 | ||
- | 155 | //--------------------------------------------------------------------- |
|
- | 156 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
|
145 | public IEnumerator<ITask> SetComPortHandler(SetComPort setComPort) |
157 | public IEnumerator<ITask> SetComPortHandler(SetComPort setComPort) |
146 | { |
158 | { |
147 | LogInfo("Set Port Name ("+setComPort.Body.PortName+")"); |
159 | LogInfo("Set Port Name ("+setComPort.Body.PortName+")"); |
148 | if ((setComPort.Body.PortName == sc.ComPort)&&(sc.isOpen==true)) |
160 | if ((setComPort.Body.PortName == sc.ComPort)&&(sc.isOpen==true)) |
149 | { |
161 | { |
150 | LogInfo("no need to change com port"); |
162 | LogInfo("no need to change com port"); |
151 | // port name equals the old port name - no action |
163 | // port name equals the old port name - no action |
152 | } |
164 | } |
153 | else |
165 | else |
154 | { |
166 | { |
155 | // port name is different, close the old port and open the new one |
167 | // port name is different, close the old port and open the new one |
156 | if(sc.isOpen==true) { |
168 | if(sc.isOpen==true) { |
157 | LogInfo("closing Port"); |
169 | LogInfo("closing Port"); |
158 | sc.ClosePort(); |
170 | sc.ClosePort(); |
159 | } |
171 | } |
160 | sc.ComPort = setComPort.Body.PortName; |
172 | sc.ComPort = setComPort.Body.PortName; |
161 | if (_rbForm != null) |
173 | if (_rbForm != null) |
162 | { |
174 | { |
163 | if (sc.InitPort() == 0) |
175 | if (sc.InitPort() == 0) |
164 | { |
176 | { |
165 | LogInfo("Init Port succseeded"); |
177 | LogInfo("Init Port succseeded"); |
166 | WinFormsServicePort.FormInvoke(delegate() |
178 | WinFormsServicePort.FormInvoke(delegate() |
167 | {_rbForm.UpdateStatus("Port Open (" + sc.ComPort + ")");}); |
179 | { _rbForm.UpdateStatus("Port Open (" + sc.ComPort + ")"); }); |
168 | } |
180 | } |
169 | else |
181 | else |
170 | { |
182 | { |
171 | LogInfo("Init Port failed"); |
183 | LogInfo("Init Port failed"); |
172 | WinFormsServicePort.FormInvoke(delegate() |
184 | WinFormsServicePort.FormInvoke(delegate() |
173 | {_rbForm.UpdateStatus("Can't open communication port (" + sc.ComPort + ")");}); |
185 | { _rbForm.UpdateStatus("Can't open communication port (" + sc.ComPort + ")"); }); |
174 | } |
186 | } |
175 | } |
187 | } |
- | 188 | else |
|
- | 189 | { |
|
- | 190 | LogInfo("Board communication not ready! Setport Ignored"); |
|
- | 191 | } |
|
176 | } |
192 | } |
- | 193 | LogInfo("Exit Set Comport"); |
|
177 | yield break; |
194 | yield break; |
178 | } |
195 | } |
179 | 196 | ||
180 | //--------------------------------------------------------------------- |
197 | //--------------------------------------------------------------------- |
181 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
198 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
182 | public IEnumerator<ITask> IncrementTickHandler(IncrementTick incrementTick) |
199 | public IEnumerator<ITask> IncrementTickHandler(IncrementTick incrementTick) |
183 | { |
200 | { |
184 | LogInfo("Tick: "); |
201 | LogInfo("Tick: "); |
185 | LogInfo("Command:" + incrementTick.Body.Command); |
202 | LogInfo("Command:" + incrementTick.Body.Command); |
186 | 203 | ||
187 | base.SendNotification(_submgrPort, incrementTick); |
204 | base.SendNotification(_submgrPort, incrementTick); |
188 | 205 | ||
189 | incrementTick.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
206 | incrementTick.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
190 | yield break; |
207 | yield break; |
191 | } |
208 | } |
192 | 209 | ||
193 | //--------------------------------------------------------------------- |
210 | //--------------------------------------------------------------------- |
194 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
211 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
195 | public IEnumerator<ITask> ReceiveCommandHandler(ReceiveCommand receiveCommand) |
212 | public IEnumerator<ITask> ReceiveCommandHandler(ReceiveCommand receiveCommand) |
196 | { |
213 | { |
197 | LogInfo("Receive "); |
214 | LogInfo("Receive "); |
198 | LogInfo("Command:" + receiveCommand.Body.Command); |
215 | LogInfo("Command:" + receiveCommand.Body.Command); |
199 | 216 | ||
200 | base.SendNotification(_submgrPort, receiveCommand); |
217 | base.SendNotification(_submgrPort, receiveCommand); |
201 | 218 | ||
202 | receiveCommand.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
219 | receiveCommand.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
203 | yield break; |
220 | yield break; |
204 | } |
221 | } |
205 | 222 | ||
206 | //--------------------------------------------------------------------- |
223 | //--------------------------------------------------------------------- |
207 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
224 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
208 | public IEnumerator<ITask> SendCommandHandler(SendCommand sendCommand) |
225 | public IEnumerator<ITask> SendCommandHandler(SendCommand sendCommand) |
209 | { |
226 | { |
210 | LogInfo("Send "); |
227 | LogInfo("Send "); |
211 | LogInfo("Command:" + sendCommand.Body.Command); |
228 | LogInfo("Command:" + sendCommand.Body.Command); |
212 | 229 | ||
213 | MKRequestTunnelMessage(sendCommand.Body.Command); |
230 | MKRequestTunnelMessage(sendCommand.Body.Command); |
214 | 231 | ||
215 | base.SendNotification(_submgrPort, sendCommand); |
232 | base.SendNotification(_submgrPort, sendCommand); |
216 | sendCommand.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
233 | sendCommand.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
217 | yield break; |
234 | yield break; |
218 | } |
235 | } |
219 | 236 | ||
220 | //--------------------------------------------------------------------- |
237 | //--------------------------------------------------------------------- |
221 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
238 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
222 | public IEnumerator<ITask> LevelHandler(Level level) |
239 | public IEnumerator<ITask> LevelHandler(Level level) |
223 | { |
240 | { |
224 | LogInfo("Level "); |
241 | LogInfo("Level "); |
225 | LogInfo("Nick:"+level.Body.Nick+"Roll:"+level.Body.Roll); |
242 | LogInfo("Nick:"+level.Body.Nick+"Roll:"+level.Body.Roll); |
226 | base.SendNotification(_submgrPort, level); |
243 | base.SendNotification(_submgrPort, level); |
227 | level.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
244 | level.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
228 | yield break; |
245 | yield break; |
229 | } |
246 | } |
230 | 247 | ||
231 | //--------------------------------------------------------------------- |
248 | //--------------------------------------------------------------------- |
232 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
249 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
233 | public IEnumerator<ITask> SetAltituteHandler(SetAltitute setaltitute) |
250 | public IEnumerator<ITask> SetAltituteHandler(SetAltitute setaltitute) |
234 | { |
251 | { |
235 | LogInfo("SetAltitute "); |
252 | LogInfo("SetAltitute "); |
236 | LogInfo("Altitute:" + setaltitute.Body.Altitute); |
253 | LogInfo("Altitute:" + setaltitute.Body.Altitute); |
237 | if (_rbForm != null) |
254 | if (_rbForm != null) |
238 | { |
255 | { |
239 | _rbForm.Control_alt = (Int16)setaltitute.Body.Altitute; |
256 | _rbForm.Control_alt = (Int16)setaltitute.Body.Altitute; |
240 | } |
257 | } |
241 | //TODO: send alt control to MK |
258 | //TODO: send alt control to MK |
242 | base.SendNotification(_submgrPort, setaltitute); |
259 | base.SendNotification(_submgrPort, setaltitute); |
243 | setaltitute.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
260 | setaltitute.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
244 | yield break; |
261 | yield break; |
245 | } |
262 | } |
246 | 263 | ||
247 | //--------------------------------------------------------------------- |
264 | //--------------------------------------------------------------------- |
248 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
265 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
249 | public IEnumerator<ITask> SetGierHandler(SetGier setgier) |
266 | public IEnumerator<ITask> SetGierHandler(SetGier setgier) |
250 | { |
267 | { |
251 | LogInfo("SetGier "); |
268 | LogInfo("SetGier "); |
252 | LogInfo("SetGier:" + setgier.Body.Gier); |
269 | LogInfo("SetGier:" + setgier.Body.Gier); |
253 | Int16 igier = (Int16)(((float)128 * setgier.Body.Gier) / (float)1000); |
270 | Int16 igier = (Int16)(((float)128 * setgier.Body.Gier) / (float)1000); |
254 | if (_rbForm != null) |
271 | if (_rbForm != null) |
255 | { |
272 | { |
256 | _rbForm.Control_gier = igier; |
273 | _rbForm.Control_gier = igier; |
257 | } |
274 | } |
- | 275 | ||
- | 276 | if (MK_latest_gier != igier) |
|
258 | 277 | { |
|
259 | MK_latest_gier = igier; |
278 | MK_latest_gier = igier; |
260 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
279 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
261 | 280 | } |
|
262 | base.SendNotification(_submgrPort, setgier); |
281 | base.SendNotification(_submgrPort, setgier); |
263 | setgier.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
282 | setgier.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
264 | yield break; |
283 | yield break; |
265 | } |
284 | } |
266 | 285 | ||
267 | //--------------------------------------------------------------------- |
286 | //--------------------------------------------------------------------- |
268 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
287 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
269 | public IEnumerator<ITask> SetGasHandler(SetGas setgas) |
288 | public IEnumerator<ITask> SetGasHandler(SetGas setgas) |
270 | { |
289 | { |
271 | LogInfo("SetGas "); |
290 | LogInfo("SetGas "); |
272 | LogInfo("SetGas:" + setgas.Body.Gas); |
291 | LogInfo("SetGas:" + setgas.Body.Gas); |
273 | Int16 igas = (Int16)(((float)128 * setgas.Body.Gas) / (float)1000); |
292 | Int16 igas = (Int16)(((float)128 * setgas.Body.Gas) / (float)1000); |
274 | if (_rbForm != null) |
293 | if (_rbForm != null) |
275 | { |
294 | { |
276 | _rbForm.Control_gas = igas; |
295 | _rbForm.Control_gas = igas; |
277 | } |
296 | } |
- | 297 | ||
- | 298 | if (MK_latest_gas != igas) |
|
278 | 299 | { |
|
279 | MK_latest_gas = igas; |
300 | MK_latest_gas = igas; |
- | 301 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
|
280 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
302 | } |
281 | 303 | ||
282 | base.SendNotification(_submgrPort, setgas); |
304 | base.SendNotification(_submgrPort, setgas); |
283 | setgas.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
305 | setgas.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
284 | yield break; |
306 | yield break; |
285 | } |
307 | } |
286 | 308 | ||
287 | //--------------------------------------------------------------------- |
309 | //--------------------------------------------------------------------- |
288 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
310 | [ServiceHandler(ServiceHandlerBehavior.Exclusive)] |
289 | 311 | ||
290 | public IEnumerator<ITask> ControlHandler(Control control) |
312 | public IEnumerator<ITask> ControlHandler(Control control) |
291 | { |
313 | { |
292 | LogInfo("Control "); |
314 | LogInfo("Control "); |
293 | LogInfo("Nick:" + control.Body.Nick + " Roll:" +control.Body.Roll); |
315 | LogInfo("Nick:" + control.Body.Nick + " Roll:" +control.Body.Roll); |
294 | 316 | ||
295 | Int16 inick = (Int16)(((float)128 * control.Body.Nick) / (float)(-1000)); |
317 | Int16 inick = (Int16)(((float)128 * control.Body.Nick) / (float)(-1000)); |
296 | Int16 iroll = (Int16)(((float)128 * control.Body.Roll) / (float)(-1000)); |
318 | Int16 iroll = (Int16)(((float)128 * control.Body.Roll) / (float)(-1000)); |
297 | 319 | ||
298 | if (_rbForm != null) |
320 | if (_rbForm != null) |
299 | { |
321 | { |
300 | _rbForm.Control_nick = inick; |
322 | _rbForm.Control_nick = inick; |
301 | _rbForm.Control_roll = iroll; |
323 | _rbForm.Control_roll = iroll; |
302 | } |
324 | } |
- | 325 | ||
- | 326 | if ((MK_latest_roll != iroll) || (MK_latest_nick != inick)) |
|
303 | 327 | { |
|
304 | MK_latest_nick = inick; |
328 | MK_latest_nick = inick; |
305 | MK_latest_roll = iroll; |
329 | MK_latest_roll = iroll; |
- | 330 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
|
306 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
331 | } |
307 | 332 | ||
308 | base.SendNotification(_submgrPort, control); |
333 | base.SendNotification(_submgrPort, control); |
309 | control.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
334 | control.ResponsePort.Post(DefaultUpdateResponseType.Instance); |
310 | yield break; |
335 | yield break; |
311 | } |
336 | } |
312 | 337 | ||
313 | //--------------------------------------------------------------------- |
338 | //--------------------------------------------------------------------- |
314 | [ServiceHandler(ServiceHandlerBehavior.Concurrent)] |
339 | [ServiceHandler(ServiceHandlerBehavior.Concurrent)] |
315 | public IEnumerator<ITask> SubscribeHandler(Subscribe subscribe) |
340 | public IEnumerator<ITask> SubscribeHandler(Subscribe subscribe) |
316 | { |
341 | { |
317 | SubscribeRequestType request = subscribe.Body; |
342 | SubscribeRequestType request = subscribe.Body; |
318 | LogInfo("Subscribe request from: " + request.Subscriber); |
343 | LogInfo("Subscribe request from: " + request.Subscriber); |
319 | 344 | ||
320 | yield return Arbiter.Choice( |
345 | yield return Arbiter.Choice( |
321 | SubscribeHelper(_submgrPort, request, subscribe.ResponsePort), |
346 | SubscribeHelper(_submgrPort, request, subscribe.ResponsePort), |
322 | delegate(SuccessResult success) |
347 | delegate(SuccessResult success) |
323 | { |
348 | { |
324 | base.SendNotification<Replace>(_submgrPort, request.Subscriber, _state); |
349 | base.SendNotification<Replace>(_submgrPort, request.Subscriber, _state); |
325 | }, |
350 | }, |
326 | delegate(Exception e) |
351 | delegate(Exception e) |
327 | { |
352 | { |
328 | LogError(null, "Subscribe failed", e); |
353 | LogError(null, "Subscribe failed", e); |
329 | } |
354 | } |
330 | ); |
355 | ); |
- | 356 | if (isloaded) |
|
- | 357 | { |
|
- | 358 | base.SendNotification(_submgrPort, new InitComplete("OK")); |
|
331 | 359 | } |
|
- | 360 | subs = true; |
|
332 | yield break; |
361 | yield break; |
333 | } |
362 | } |
334 | 363 | ||
335 | //--------------------------------------------------------------------- |
364 | //--------------------------------------------------------------------- |
336 | IEnumerator<ITask> OnLoadHandler(OnLoad onLoad) |
365 | IEnumerator<ITask> OnLoadHandler(OnLoad onLoad) |
337 | { |
366 | { |
338 | _rbForm = onLoad.RoboBoardForm; |
367 | _rbForm = onLoad.RoboBoardForm; |
339 | LogInfo("Loaded Form"); |
368 | LogInfo("Loaded Form"); |
340 | sc.ComPort = "COM6"; |
369 | sc.ComPort = "COM7"; |
341 | if (_rbForm != null) |
370 | if (_rbForm != null) |
342 | { |
371 | { |
343 | if (sc.InitPort() == 0) |
372 | if (sc.InitPort() == 0) |
344 | { |
373 | { |
- | 374 | LogInfo("Port Open"); |
|
345 | WinFormsServicePort.FormInvoke(delegate() |
375 | WinFormsServicePort.FormInvoke(delegate() |
346 | {_rbForm.UpdateStatus("Port Open (" + sc.ComPort + ")");}); |
376 | {_rbForm.UpdateStatus("Port Open (" + sc.ComPort + ")");}); |
347 | } |
377 | } |
348 | else |
378 | else |
349 | { |
379 | { |
- | 380 | LogInfo("Port failed to Open"); |
|
350 | WinFormsServicePort.FormInvoke(delegate() |
381 | WinFormsServicePort.FormInvoke(delegate() |
351 | {_rbForm.UpdateStatus("Can't open communication port (" + sc.ComPort + ")");}); |
382 | {_rbForm.UpdateStatus("Can't open communication port (" + sc.ComPort + ")");}); |
352 | } |
383 | } |
353 | } |
384 | } |
354 | - | ||
355 | //request version |
385 | //request version |
356 | MKRequestVersion(); |
386 | MKRequestVersion(); |
- | 387 | if (subs) |
|
- | 388 | { |
|
- | 389 | base.SendNotification(_submgrPort, new InitComplete("OK")); |
|
- | 390 | } |
|
- | 391 | isloaded = true; |
|
357 | yield break; |
392 | yield break; |
358 | } |
393 | } |
359 | 394 | ||
360 | //--------------------------------------------------------------------- |
395 | //--------------------------------------------------------------------- |
361 | void MKRequestTunnelMessage(string message) |
396 | void MKRequestTunnelMessage(string message) |
362 | { |
397 | { |
363 | message = sc.CreateMessage((char)'X', (char)0, message); |
398 | message = sc.CreateMessage((char)'X', (char)0, message); |
364 | sc.SerialWrite(message); |
399 | sc.SerialWrite(message); |
365 | } |
400 | } |
366 | 401 | ||
367 | //--------------------------------------------------------------------- |
402 | //--------------------------------------------------------------------- |
368 | bool MKRequestSend = false; |
403 | bool MKRequestSend = false; |
369 | Int16 MK_old_roll = 0; |
404 | Int16 MK_old_roll = 0; |
370 | Int16 MK_old_nick = 0; |
405 | Int16 MK_old_nick = 0; |
371 | Int16 MK_old_gier = 0; |
406 | Int16 MK_old_gier = 0; |
372 | Int16 MK_old_gas = 0; |
407 | Int16 MK_old_gas = 0; |
373 | 408 | ||
374 | Int16 MK_latest_roll = 0; |
409 | Int16 MK_latest_roll = 0; |
375 | Int16 MK_latest_nick = 0; |
410 | Int16 MK_latest_nick = 0; |
376 | Int16 MK_latest_gier = 0; |
411 | Int16 MK_latest_gier = 0; |
377 | Int16 MK_latest_gas = 0; |
412 | Int16 MK_latest_gas = 0; |
378 | 413 | ||
379 | void MKRequestControl(Int16 nick, Int16 roll, Int16 gier, Int16 gas) |
414 | void MKRequestControl(Int16 nick, Int16 roll, Int16 gier, Int16 gas) |
380 | { |
415 | { |
381 | string message = ""; |
416 | string message = ""; |
382 | if ((MK_old_gas == gas) && |
417 | if ((MK_old_gas == gas) && |
383 | (MK_old_gier == gier) && |
418 | (MK_old_gier == gier) && |
384 | (MK_old_nick == nick) && |
419 | (MK_old_nick == nick) && |
385 | (MK_old_roll == roll)) |
420 | (MK_old_roll == roll)) |
386 | { |
421 | { |
387 | LogInfo("no update"); |
422 | LogInfo("no update"); |
388 | return; |
423 | return; |
389 | } |
424 | } |
390 | if (MKRequestSend == true) return; |
425 | if (MKRequestSend == true) return; |
391 | LogInfo("Send"); |
426 | LogInfo("Send"); |
392 | if (_rbForm != null) |
427 | if (_rbForm != null) |
393 | { |
428 | { |
394 | WinFormsServicePort.FormInvoke(delegate() |
429 | WinFormsServicePort.FormInvoke(delegate() |
395 | { _rbForm.MonitorMKSensorUpdate(); }); |
430 | { _rbForm.MonitorMKSensorUpdate(); }); |
396 | } |
431 | } |
397 | message = sc.MessageAddInt16(roll, message); |
432 | message = sc.MessageAddInt16(roll, message); |
398 | MK_old_roll = roll; |
433 | MK_old_roll = roll; |
399 | message = sc.MessageAddInt16(nick, message); |
434 | message = sc.MessageAddInt16(nick, message); |
400 | MK_old_nick = nick; |
435 | MK_old_nick = nick; |
401 | message = sc.MessageAddInt16(gier, message); |
436 | message = sc.MessageAddInt16(gier, message); |
402 | MK_old_gier = gier; |
437 | MK_old_gier = gier; |
403 | message = sc.MessageAddInt16(gas, message); |
438 | message = sc.MessageAddInt16(gas, message); |
404 | MK_old_gas = gas; |
439 | MK_old_gas = gas; |
405 | message = sc.CreateMessage((char)'C', (char)0, message); |
440 | message = sc.CreateMessage((char)'C', (char)0, message); |
406 | MKRequestSend = true; |
441 | MKRequestSend = true; |
407 | sc.SerialWrite(message); |
442 | sc.SerialWrite(message); |
408 | } |
443 | } |
409 | 444 | ||
410 | void MKDecodeControl() |
445 | void MKDecodeControl() |
411 | { |
446 | { |
412 | LogInfo("Control OK"); |
447 | LogInfo("Control OK"); |
413 | MKRequestSend = false; |
448 | MKRequestSend = false; |
414 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
449 | MKRequestControl(MK_latest_nick, MK_latest_roll, MK_latest_gier, MK_latest_gas); |
415 | } |
450 | } |
416 | 451 | ||
417 | //--------------------------------------------------------------------- |
452 | //--------------------------------------------------------------------- |
418 | void MKRequestVersion() |
453 | void MKRequestVersion() |
419 | { |
454 | { |
- | 455 | LogInfo("MK RequestVersion"); |
|
420 | string message = ""; |
456 | string message = ""; |
421 | message=sc.CreateMessage((char)'v', (char)0, message); |
457 | message=sc.CreateMessage((char)'v', (char)0, message); |
- | 458 | if (sc.isOpen) |
|
- | 459 | { |
|
- | 460 | LogInfo("Version Send"); |
|
422 | sc.SerialWrite(message); |
461 | sc.SerialWrite(message); |
- | 462 | } |
|
423 | } |
463 | } |
424 | 464 | ||
425 | string MKDecodeVersion(string message) |
465 | string MKDecodeVersion(string message) |
426 | { |
466 | { |
427 | string Versionstring = ""; |
467 | string Versionstring = ""; |
428 | Versionstring += "Version: "+sc.MessageReadByte(message).ToString(); |
468 | Versionstring += "Version: "+sc.MessageReadByte(message).ToString(); |
429 | message=message.Substring(1); |
469 | message=message.Substring(1); |
430 | sc.MessageReadByte(message); |
470 | sc.MessageReadByte(message); |
431 | Versionstring += "." + sc.MessageReadByte(message).ToString(); |
471 | Versionstring += "." + sc.MessageReadByte(message).ToString(); |
432 | message = message.Substring(1); |
472 | message = message.Substring(1); |
433 | sc.MessageReadByte(message); |
473 | sc.MessageReadByte(message); |
434 | Versionstring += " (STEP:" + sc.MessageReadByte(message).ToString()+")"; |
474 | Versionstring += " (STEP:" + sc.MessageReadByte(message).ToString()+")"; |
435 | if (_rbForm != null) |
475 | if (_rbForm != null) |
436 | { |
476 | { |
437 | WinFormsServicePort.FormInvoke(delegate() |
477 | WinFormsServicePort.FormInvoke(delegate() |
438 | { _rbForm.UpdateStatus(Versionstring); }); |
478 | { _rbForm.UpdateStatus(Versionstring); }); |
439 | } |
479 | } |
440 | return Versionstring; |
480 | return Versionstring; |
441 | } |
481 | } |
442 | 482 | ||
443 | //--------------------------------------------------------------------- |
483 | //--------------------------------------------------------------------- |
444 | void MKDecodeDebugData(string message) |
484 | void MKDecodeDebugData(string message) |
445 | { |
485 | { |
446 | Int16[] analog = new Int16[32]; |
486 | Int16[] analog = new Int16[32]; |
447 | byte[] digital = new byte[2]; |
487 | byte[] digital = new byte[2]; |
448 | int i = 0; |
488 | int i = 0; |
449 | for (i = 0; i < 2; i++) |
489 | for (i = 0; i < 2; i++) |
450 | { |
490 | { |
451 | digital[i]=sc.MessageReadByte(message); |
491 | digital[i]=sc.MessageReadByte(message); |
452 | message = message.Substring(1); |
492 | message = message.Substring(1); |
453 | } |
493 | } |
454 | for (i = 0; i < 32; i++) |
494 | for (i = 0; i < 32; i++) |
455 | { |
495 | { |
456 | analog[i] = sc.MessageReadInt16(message); |
496 | analog[i] = sc.MessageReadInt16(message); |
457 | 497 | ||
458 | Int16 analogx = (Int16)analog[i]; |
498 | Int16 analogx = (Int16)analog[i]; |
459 | if (_rbForm != null) |
499 | if (_rbForm != null) |
460 | { |
500 | { |
461 | _rbForm.MK_analog[i]= analog[i]; |
501 | _rbForm.MK_analog[i]= analog[i]; |
462 | _rbForm.Mk_analog_name[i] = "Analog" + i.ToString(); |
502 | _rbForm.Mk_analog_name[i] = "Analog" + i.ToString(); |
463 | //WinFormsServicePort.FormInvoke(delegate() |
503 | //WinFormsServicePort.FormInvoke(delegate() |
464 | //{ _rbForm.MonitorMKSensor(i,analogx,"Analog"+i.ToString()); }); |
504 | //{ _rbForm.MonitorMKSensor(i,analogx,"Analog"+i.ToString()); }); |
465 | } |
505 | } |
466 | message = message.Substring(2); |
506 | message = message.Substring(2); |
467 | } |
507 | } |
468 | if (_rbForm != null) |
508 | if (_rbForm != null) |
469 | { |
509 | { |
470 | WinFormsServicePort.FormInvoke(delegate() |
510 | WinFormsServicePort.FormInvoke(delegate() |
471 | { _rbForm.MonitorMKSensorUpdate(); }); |
511 | { _rbForm.MonitorMKSensorUpdate(); }); |
472 | } |
512 | } |
473 | base.SendNotification(_submgrPort, new Level((int)analog[0], (int)analog[1])); |
513 | base.SendNotification(_submgrPort, new Level((int)analog[0], (int)analog[1])); |
474 | } |
514 | } |
475 | 515 | ||
476 | //--------------------------------------------------------------------- |
516 | //--------------------------------------------------------------------- |
477 | void sc_BotEvent(object sender, EventArgs ev) |
517 | void sc_BotEvent(object sender, EventArgs ev) |
478 | { |
518 | { |
479 | 519 | ||
480 | string message = ""; |
520 | string message = ""; |
481 | char cmd = (char)0; |
521 | char cmd = (char)0; |
482 | char adr = (char)0; |
522 | char adr = (char)0; |
483 | if (sc.IsMessageValid() == true) |
523 | if (sc.IsMessageValid() == true) |
484 | { |
524 | { |
485 | message = sc.GetLastMessage(); |
525 | message = sc.GetLastMessage(); |
486 | cmd = sc.GetMessageCmd(); |
526 | cmd = sc.GetMessageCmd(); |
487 | adr = sc.GetMessageAdr(); |
527 | adr = sc.GetMessageAdr(); |
488 | if (cmd.CompareTo((char)'V')==0) |
528 | if (cmd.CompareTo((char)'V')==0) |
489 | { |
529 | { |
490 | MKDecodeVersion(message); |
530 | MKDecodeVersion(message); |
491 | base.SendNotification(_submgrPort, new ReceiveCommand("V"+message)); |
531 | base.SendNotification(_submgrPort, new ReceiveCommand("V"+message)); |
492 | } |
532 | } |
493 | else |
533 | else |
494 | if (cmd.CompareTo((char)'D') == 0) |
534 | if (cmd.CompareTo((char)'D') == 0) |
495 | { |
535 | { |
496 | MKDecodeDebugData(message); |
536 | MKDecodeDebugData(message); |
497 | //base.SendNotification(_submgrPort, new ReceiveCommand("D"+message)); |
537 | //base.SendNotification(_submgrPort, new ReceiveCommand("D"+message)); |
498 | } |
538 | } |
499 | else |
539 | else |
500 | if (cmd.CompareTo((char)'X') == 0) |
540 | if (cmd.CompareTo((char)'X') == 0) |
501 | { |
541 | { |
502 | base.SendNotification(_submgrPort, new ReceiveCommand(message)); |
542 | base.SendNotification(_submgrPort, new ReceiveCommand(message)); |
503 | } |
543 | } |
504 | else |
544 | else |
505 | if (cmd.CompareTo((char)'C') == 0) |
545 | if (cmd.CompareTo((char)'C') == 0) |
506 | { |
546 | { |
507 | MKDecodeControl(); |
547 | MKDecodeControl(); |
508 | } |
548 | } |
509 | 549 | ||
510 | sc.InvalidateMessage(); |
550 | sc.InvalidateMessage(); |
511 | } |
551 | } |
512 | 552 | ||
513 | if (_rbForm != null) |
553 | if (_rbForm != null) |
514 | { |
554 | { |
515 | WinFormsServicePort.FormInvoke(delegate() |
555 | WinFormsServicePort.FormInvoke(delegate() |
516 | { _rbForm.MonitorCommand(message); }); |
556 | { _rbForm.MonitorCommand(message); }); |
517 | } |
557 | } |
518 | } |
558 | } |
519 | } |
559 | } |
520 | } |
560 | } |
521 | 561 |