Subversion Repositories Projects

Rev

Rev 90 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 90 Rev 97
Line 40... Line 40...
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();
Line -... Line 44...
-
 
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>
Line 140... Line 143...
140
            yield break;
143
            yield break;
141
        }
144
        }
Line 142... Line 145...
142
 
145
 
143
        //---------------------------------------------------------------------
146
        //---------------------------------------------------------------------
-
 
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
        //---------------------------------------------------------------------
144
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
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))
Line 161... Line 173...
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
        }
Line 179... Line 196...
179
 
196
 
180
        //---------------------------------------------------------------------
197
        //---------------------------------------------------------------------
Line 254... Line 271...
254
            if (_rbForm != null)
271
            if (_rbForm != null)
255
            {
272
            {
256
                _rbForm.Control_gier = igier;
273
                _rbForm.Control_gier = igier;
257
            }
274
            }
Line -... Line 275...
-
 
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;
Line 274... Line 293...
274
            if (_rbForm != null)
293
            if (_rbForm != null)
275
            {
294
            {
276
               _rbForm.Control_gas = igas;
295
               _rbForm.Control_gas = igas;
277
            }
296
            }
Line -... Line 297...
-
 
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);
Line 280... Line 302...
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);
Line 299... Line 321...
299
            {
321
            {
300
                _rbForm.Control_nick = inick;
322
                _rbForm.Control_nick = inick;
301
                _rbForm.Control_roll = iroll;
323
                _rbForm.Control_roll = iroll;
302
            }
324
            }
Line -... Line 325...
-
 
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);
Line 306... Line 331...
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);
Line 326... Line 351...
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
        }
Line 334... Line 363...
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)
-
 
373
                {
344
                {
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
-
 
379
                {
349
                {
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
            }
-
 
354
           
384
            }
355
            //request version
385
            //request version
-
 
386
            MKRequestVersion();
-
 
387
            if (subs)
-
 
388
            {
-
 
389
                base.SendNotification(_submgrPort, new InitComplete("OK"));
-
 
390
            }
356
            MKRequestVersion();
391
            isloaded = true;
357
            yield break;
392
            yield break;
Line 358... Line 393...
358
        }
393
        }
359
 
394
 
Line 415... Line 450...
415
        }
450
        }
Line 416... Line 451...
416
       
451
       
417
        //---------------------------------------------------------------------
452
        //---------------------------------------------------------------------
418
        void MKRequestVersion()
453
        void MKRequestVersion()
-
 
454
        {
419
        {
455
            LogInfo("MK RequestVersion");
420
            string message = "";
456
            string message = "";
-
 
457
            message=sc.CreateMessage((char)'v', (char)0, message);
-
 
458
            if (sc.isOpen)
-
 
459
            {
421
            message=sc.CreateMessage((char)'v', (char)0, message);
460
                LogInfo("Version Send");
-
 
461
                sc.SerialWrite(message);
422
            sc.SerialWrite(message);
462
            }
Line 423... Line 463...
423
        }
463
        }
424
 
464
 
425
        string MKDecodeVersion(string message)
465
        string MKDecodeVersion(string message)