| Wheelchair Control Diploma Thesis, Author: Ondrej Vozda, FEEC BUT, 2013

- Main Page
- Packages
- Classes
- Files

- File List

All Classes Namespaces Functions Variables Enumerations Enumerator Properties Events Pages
FormMain.cs
1 ﻿using System;
2 using System.Collections.Generic;
3 using System.Drawing;
4 using System.Drawing.Imaging;
5 using System.IO;
6 using System.IO.Ports;
7 using System.Net;
8 using System.Net.Sockets;
9 using System.Text;
10 using System.Threading;
11 using System.Windows.Forms;
12
13 //Emgu usings
14 using Emgu.CV;
15 using Emgu.CV.Structure;
16
17 //My namespaces
18 using CsharpUSBJoystick;
19 using World;
20 using MyImageProcessingClasses;
21
22 namespace WheelchairControl
23 {
27 public partial class FormMain : Form
28 {
29 #region Constants
30
34 public const Int32 COM_PORT_STATUS_SAMPLING_PERIOD_MS = 500;
35
39 public const Byte RX_DATA_LENGTH = 4;
40
44 private enum ComboboxControlIndexes : byte
45 {
49 MOMENT,
50
54 SPEED_REGULATOR
55 };
56
60 private enum ComboboxUserInputIndexes : byte
61 {
65 JOYSTICK,
66
70 WIFI
71 };
72
76 private enum ComboboxDetectionMethodIndexes : byte
77 {
81 REFERENCE_HSV,
82
86 SURF_DETECTOR
87 }
88
92 private enum WorldMapRenderingModes : byte
93 {
97 HIGH_SPEED,
98
102 HIGH_QUALITY,
103
107 ANTIALIAS,
108
112 NONE
113 }
114
118 private enum JoyAutoParkingSelesctionIndex : byte
119 {
123 JOYSTICK = 0,
124
128 AUTOPARKING
129 }
130
134 private enum MainPageTabsIndexes : byte
135 {
139 CAMERA,
140
144 SETTINGS,
145
149 LOG
150 }
151
155 private enum DetectedMarksTransmit : byte
156 {
160 DONT_TRANSMIT,
161
165 TRANSMIT
166 }
167
171 private enum PacketHeader : byte
172 {
176 WATCHDOG_NOP = 0xC0,
177
181 JOYSTICK_COMMAND = 0xC1,
182
186 STOP_BUTTON = 0xC2,
187
191 START_BUTTON = 0xC3,
192
196 AUTO_PARKING_START_BUTTON = 0xC4,
197
201 CAMERA_FRAME_REQUEST = 0xC5,
202
206 USER_RESPONSE = 0xC6,
207
211 AUTOPARKING_PHASE = 0xC7,
212
216 CAMERA_FRAME_RESPONSE = 0xD0,
217
221 CAMERA_FRAME_TO_BE_CONFIRMED = 0xD1
222 }
223
227 private enum OdometryPacketByte : byte
228 {
232 X_BASE = 0,
233
237 Y_BASE = 2,
238
242 PHI_BASE = 4,
243
247 SRF08_FRONT_DIST = 6,
248
252 SRF08_REAR_DIST = 7,
253
257 ENABLE_POSITION_REGULATOR = 8,
258
262 ENABLE_WALL_FOLLOWING = 9
263 }
264
268 private enum MapReferenceCoordinates : byte
269 {
273 MARKS_CENTER,
274
278 WHEELCHAIR_CENTER
279 }
280
284 private enum RxReturnCodes : byte
285 {
289 TRANSMISSION_OK,
290
294 PORT_CLOSED,
295
299 PACKET_INCOMPLETE,
300
304 WRONG_CHECKSUM,
305 }
306
310 private const Byte JOYSTICK_RANGE = 255;
311
315 private const Int32 UART_RX_TIMEOUT_MS = 50;
316
320 private const Int32 WIFI_WATCHDOG_TIMEOUT_MS = 1000;
321
325 private const Byte STOP_REQUEST_REPEATS = 3;
326
330 private const UInt16 UDP_PORT = 50005;
331
332 #endregion
333
334 #region Events and delegates
335
339 public delegate void COMPortStatusChangedDelegate();
340
344 public static event COMPortStatusChangedDelegate COMStatusChanged;
345
346 #endregion
347
348 #region Private variables
349
353 private Thread UDPreceiveThread;
354
358 private Thread AutoParkingThread;
359
363 private IPEndPoint ipEp = new IPEndPoint(IPAddress.Any, UDP_PORT);
364
368 private UdpClient incomingUDPClient;
369
373 private UdpClient outgoingUDPClient;
374
378 private AutoResetEvent arUserResponseTimout;
379
383 private DateTime swLogStartTime;
384
388 private USB_Joystick joystick;
389
393 private Thread threadCOMPortStatusGuard;
394
398 private Boolean portState = true;
399
403 private static Object lockUart = new Object();
404
408 private Boolean motorControlAllowed = false;
409
413 private StreamWriter swLogWriter;
414
418 private Fake_USBJoystick wifiJoy = new Fake_USBJoystick(JOYSTICK_RANGE / 2, JOYSTICK_RANGE / 2, 0);
419
420 #endregion
421
422 #region Miscelaneous methods
423
427 private void LoadCalibrationCoordinates()
428 {
429 foreach (CameraCalibrator.PointLocation location in CameraCalibrator.CalibrationPoints.Keys)
430 {
431 //Create string
432 String coordsString = "(" + CameraCalibrator.CalibrationPoints[(Byte)location].X.ToString("F1")
433 + ";" + CameraCalibrator.CalibrationPoints[(Byte)location].Y.ToString("F1") + ")";
434
435 //Assign string to proper radio button
436 switch (location)
437 {
438 case CameraCalibrator.PointLocation.UPPER_LEFT:
439
440 rbUL.Text = "Upper left " + coordsString;
441
442 break;
443
444 case CameraCalibrator.PointLocation.UPPER_RIGHT:
445
446 rbUR.Text = "Upper right " + coordsString;
447
448 break;
449
450 case CameraCalibrator.PointLocation.LOWER_RIGHT:
451
452 rbLR.Text = "Lower right " + coordsString;
453
454 break;
455
456 case CameraCalibrator.PointLocation.LOWER_LEFT:
457
458 rbLL.Text = "Lower left " + coordsString;
459
460 break;
461
462 default: break;
463 }
464 }
465 }
466
470 private void UDPDataReceiver()
471 {
472 try
473 {
474 Byte[] inBuffer; //Incoming data buffer
475 String logMessage = null; //Message to be logged
476
477 //Set socket timeout
478 incomingUDPClient.Client.ReceiveTimeout = WIFI_WATCHDOG_TIMEOUT_MS;
479
480 while (true)
481 {
482 try
483 {
484 //Log message from previous loop (logged immediatelly, since thread waits on Receive() method)
485 //No need to check InvokeRequired property, since this thread is run separately
486 this.Invoke((MethodInvoker)delegate()
487 {
488 if (logMessage != null)
489 {
490 Timestamp();
491 textBoxData.AppendText(logMessage + Environment.NewLine);
492
493 logMessage = null;
494 }
495 });
496
497 //Receive
498 inBuffer = incomingUDPClient.Receive(ref ipEp);
499
500 //Blink indicator
501 //No need to check InvokeRequired property, since this thread is run separately
502 this.Invoke((MethodInvoker)delegate()
503 {
504 if (ssLabelWiFiWatchdog.BackColor == ssMain.BackColor)
505 ssLabelWiFiWatchdog.BackColor = Color.LimeGreen;
506 else
507 ssLabelWiFiWatchdog.BackColor = ssMain.BackColor;
508 });
509
510 //Parse header
511 switch (inBuffer[0])
512 {
513 case (Byte)PacketHeader.JOYSTICK_COMMAND:
514
515 //Set fake joy's coordinates
516 wifiJoy.SetCoordinates(inBuffer[1], inBuffer[2], inBuffer[3]);
517
518 //Create message to log
519 logMessage = "Joy coordinates received: X: " + inBuffer[1].ToString() + ", Y: "
520 + inBuffer[2].ToString() + ", Z: " + inBuffer[3].ToString();
521
522 break;
523
524 case (Byte)PacketHeader.STOP_BUTTON:
525
526 //Click stop button remotely
527 //No need to check InvokeRequired property, since this thread is run separately
528 this.Invoke((MethodInvoker)delegate()
529 {
530 buttonSTOP_Click(this, null);
531 });
532
533 //Create message to log
534 logMessage = "Remote STOP command received";
535
536 break;
537
538 case (Byte)PacketHeader.START_BUTTON:
539
540 //Click start button remotely
541 //No need to check InvokeRequired property, since this thread is run separately
542 this.Invoke((MethodInvoker)delegate()
543 {
544 //Only when wifi is used as input
545 if (comboBoxUserInput.SelectedIndex == (Byte)ComboboxUserInputIndexes.WIFI)
546 buttonSTART_Click(this, null);
547 });
548
549 //Create message to log
550 logMessage = "Remote START command received";
551
552 break;
553
554 case (Byte)PacketHeader.AUTO_PARKING_START_BUTTON:
555
556 //Click auto parking start button remotely
557 this.Invoke((MethodInvoker)delegate()
558 {
559 //Only when wifi is used as input
560 if (comboBoxUserInput.SelectedIndex == (Byte)ComboboxUserInputIndexes.WIFI)
561 buttonSTARTAutoParking_Click(this, null);
562 });
563
564 //Create message to log
565 logMessage = "Remote autopraking START command received";
566
567 break;
568
569 case (Byte)PacketHeader.USER_RESPONSE:
570
571 //Analyze user response
572 wheelchairModel.UserResponse =
573 (WheelchairModel.UserResponseCode)Enum.Parse(typeof(WheelchairModel.UserResponseCode),
574 inBuffer[1].ToString());
575
576 //Signal operation success
577 arUserResponseTimout.Set();
578
579 break;
580
581 case (Byte)PacketHeader.WATCHDOG_NOP:
582 {
583 //Perform watchdog loopback if enabled, else no action
584 if (checkBoxWatchdogResponseEnabled.Checked)
585 {
586 IPAddress targetIp = null;
587
588 try
589 {
590 this.Invoke((MethodInvoker)delegate()
591 {
592 try
593 {
594 targetIp = IPAddress.Parse(textBoxTargetIP.Text);
595 }
596 catch (Exception)
597 { /*Failed to load address*/}
598 });
599 }
600 catch (Exception)
601 {/*Failed to load address*/}
602
603 //Send response back
604 outgoingUDPClient.Send(new Byte[] { (Byte)PacketHeader.WATCHDOG_NOP }, 1, new IPEndPoint(targetIp, UDP_PORT));
605 }
606 }
607 break;
608
609 case (Byte)PacketHeader.CAMERA_FRAME_REQUEST:
610 {
611 //Send actual camera frame
612 List<Byte> packetTemp = new List<Byte>() { (Byte)PacketHeader.CAMERA_FRAME_RESPONSE };
613 MemoryStream outStream = new MemoryStream();
614 IPAddress targetIp = null;
615
616 try
617 {
618 this.Invoke((MethodInvoker)delegate()
619 {
620 try
621 {
622 targetIp = IPAddress.Parse(textBoxTargetIP.Text);
623 }
624 catch (Exception)
625 { /*Failed to load addres*/}
626 });
627 }
628 catch (Exception)
629 { /*Failed to load addres*/}
630
631 //Switch clean image / image with detected marks and parse it into memorystream
632 if (inBuffer[1] == (Byte)DetectedMarksTransmit.DONT_TRANSMIT)
633 {
634 //Set encoder parameters
635 ImageProcessingTools.cameraFrameEncoderParameters.Param[ImageProcessingTools.ENCODER_PARAMETER_FIRST]
636 = ImageProcessingTools.cameraFrameEncoderParameter[(Byte)numericUpDownJpegCompression.Value];
637
638 lastCaptured.ToBitmap().Save(outStream, ImageProcessingTools.jpegCodec, ImageProcessingTools.cameraFrameEncoderParameters);
639 }
640 else
641 {
642 //Set encoder parameters
643 ImageProcessingTools.cameraFrameEncoderParameters.Param[ImageProcessingTools.ENCODER_PARAMETER_FIRST]
644 = ImageProcessingTools.cameraFrameEncoderParameter[(Byte)numericUpDownJpegCompression.Value];
645
646 using (Image tempImg = new Bitmap(pbFrame.Image)) //Buffer image first
647 tempImg.Save(outStream, ImageProcessingTools.jpegCodec, ImageProcessingTools.cameraFrameEncoderParameters);
648 }
649
650 //Append serialized image data
651 foreach (Byte dataByte in outStream.ToArray())
652 packetTemp.Add(dataByte);
653
654 //Send
655 outgoingUDPClient.Send(packetTemp.ToArray(), packetTemp.Count, new IPEndPoint(targetIp, UDP_PORT));
656
657 //Create message to log
658 logMessage = (inBuffer[1] == (Byte)DetectedMarksTransmit.DONT_TRANSMIT ? "Camera frame query received. Marks positions not sent" : "Camera frame query received. Marks positions sent");
659 }
660 break;
661
662 default:
663 //Next loop, don't reset watchdog timer
664 continue;
665 }
666 }
667 catch (SocketException socketEx)
668 {
669 //Process exception only if it is caused by receive() timeout
670 if (socketEx.SocketErrorCode == SocketError.TimedOut)
671 {
672 //Click stop button
673 //No need to check InvokeRequired property, since this thread is run separately
674 this.Invoke((MethodInvoker)delegate()
675 {
676 if (ssLabelWiFiWatchdog.BackColor != Color.Red) //First occurence of watchdog timeout
677 {
678 ssLabelWiFiWatchdog.BackColor = Color.Red;
679
680 Timestamp();
681 textBoxData.AppendText("WiFi watchdog timer elapsed!" + Environment.NewLine);
682
683 buttonSTOP_Click(this, null);
684 }
685 });
686 }
687 else
688 {
689 //Pass exception further
690 throw;
691 }
692 }
693 catch (Exception ex)
694 {
695 //Log error and blink indicator
696 //No need to check InvokeRequired property, since this thread is run separately
697 this.Invoke((MethodInvoker)delegate()
698 {
699 Timestamp();
700 textBoxData.AppendText(ex.Message.ToString() + Environment.NewLine);
701 });
702 }
703 }
704 }
705 catch (ThreadInterruptedException)
706 {/*Handle something here???*/}
707 catch (ThreadAbortException)
708 {/*Handle something here???*/}
709 catch (Exception)
710 {/*Handle something here???*/}
711 }
712
717 private void SendStopSequence()
718 {
719 UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];
720
721 for (Byte i = 0; i < STOP_REQUEST_REPEATS; i++) //Send mutiple times, just for sure
722 {
723 outputBuffer[0] = 0xC4; //Stop request for both motors
724 outputBuffer[1] = 128;
725 outputBuffer[2] = 128;
726 UARTsend(outputBuffer);
727 }
728 }
729
733 private void ResetOdometry()
734 {
735 uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1] { 0xA3, 0x00, 0x00 };
736
737 UARTsend(outputBuffer);
738 }
739
745 private void VisualizePhase(WheelchairModel.AutoParkingPhase phase, Boolean isError)
746 {
747 //Invoke always
748 this.Invoke((MethodInvoker)delegate()
749 {
750 Color lblColor = isError ? Color.Red : Color.Lime;
751
752 //Erase all labels
753 foreach (Control ctrl in tabPageAutoParking.Controls)
754 if (ctrl is Label)
755 ((Label)ctrl).BackColor = tabPageAutoParking.BackColor;
756
757 //Light up correct label
758 switch (phase)
759 {
760 default:
761 case WheelchairModel.AutoParkingPhase.WAITING_FOR_START:
762
763 labelPhaseWaitToStart.BackColor = lblColor;
764
765 break;
766
767 case WheelchairModel.AutoParkingPhase.WALL_FOLLOWING:
768
769 labelPhaseWallFollowing.BackColor = lblColor;
770
771 break;
772
773 case WheelchairModel.AutoParkingPhase.GOING_BACKWARDS:
774
775 labelPhaseBackwardDrive.BackColor = lblColor;
776
777 break;
778
779 case WheelchairModel.AutoParkingPhase.MARKS_TRACKING:
780
781 labelPhaseDetectingMarks.BackColor = lblColor;
782
783 break;
784
785 case WheelchairModel.AutoParkingPhase.PATH_FOLLOWING:
786
787 labelPhaseNextWaypoint.BackColor = lblColor;
788
789 break;
790
791 case WheelchairModel.AutoParkingPhase.END:
792
793 labelPhaseEnd.BackColor = lblColor;
794
795 break;
796 }
797
798 //Send via Wifi, if WiFi is enabled
799 if (comboBoxUserInput.SelectedIndex == (Byte)ComboboxUserInputIndexes.WIFI)
800 {
801 IPAddress targetIp = null;
802
803 try
804 {
805 targetIp = IPAddress.Parse(textBoxTargetIP.Text);
806 }
807 catch (Exception)
808 { /*Failed to load address*/}
809
810 //Send response back
811 outgoingUDPClient.Send(new Byte[] { (Byte)PacketHeader.AUTOPARKING_PHASE, (Byte)phase,
812 isError ? (Byte)WheelchairModel.PhaseError.ERROR : (Byte)WheelchairModel.PhaseError.ERROR },
813 3 /*Packet length*/, new IPEndPoint(targetIp, UDP_PORT));
814 }
815 });
816 }
817
822 private void UARTsend(UInt32[] data)
823 {
824 lock (lockUart)
825 {
826 if (data.Length != RX_DATA_LENGTH - 1)
827 {
828 Timestamp();
829 textBoxData.AppendText("Wrong packet length: " + data.Length.ToString() + ", expected: " + (RX_DATA_LENGTH - 1).ToString() + Environment.NewLine);
830 return;
831 }
832
833 if (!serialPortComPort.IsOpen)
834 {
835 Timestamp();
836 textBoxData.AppendText("Data not sent, port is closed" + Environment.NewLine);
837 return;
838 }
839
840 if (data[0] > 255 || data[1] > 255 || data[2] > 255)
841 {
842 Timestamp();
843 textBoxData.AppendText("Data not sent, input data not valid" + Environment.NewLine);
844 return;
845 }
846
847 serialPortComPort.DiscardOutBuffer();
848
849 Byte[] tempData = new Byte[RX_DATA_LENGTH];
850 tempData[0] = (Byte)data[0];
851 tempData[1] = (Byte)data[1];
852 tempData[2] = (Byte)data[2];
853 tempData[3] = (Byte)(data[0] + data[1] + data[2]);
854
855 if (tempData[0] != 0) //Do not log watchdog NOP packet
856 {
857 Timestamp();
858 textBoxData.AppendText("SENT: " + "0x" + tempData[0].ToString("X") + " " + tempData[1].ToString() + " " + tempData[2].ToString() + " " + tempData[3].ToString() + Environment.NewLine);
859 }
860
861 serialPortComPort.Write(tempData, 0, tempData.Length);
862 }
863 }
864
868 private void Timestamp()
869 {
870 textBoxData.AppendText(DateTime.Now.Hour.ToString("D2") + ":" + DateTime.Now.Minute.ToString("D2") + ":" + DateTime.Now.Second.ToString("D2") + " ");
871 }
872
879 private Int32 UARTreceive(Byte instruction, ref Byte[] outBuffer)
880 {
881 Byte[] tempArray = new Byte[outBuffer.Length];
882 Byte sum = 0;
883 UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];
884
885 //Return port not open error code if port is closed
886 if (!serialPortComPort.IsOpen)
887 {
888 Timestamp();
889 textBoxData.AppendText("Data not read, port closed" + Environment.NewLine);
890
891 return (Int32)RxReturnCodes.PORT_CLOSED;
892 }
893
894 serialPortComPort.DiscardInBuffer();
895
896 //Send request
897 outputBuffer[0] = instruction;
898 outputBuffer[1] = 0x00;
899 outputBuffer[2] = 0x00;
900 UARTsend(outputBuffer);
901
902 DateTime startTime = DateTime.Now;
903
904 while (serialPortComPort.BytesToRead < outBuffer.Length) //Wait for all bytes with specified timeout
905 {
906 if (DateTime.Now - startTime > TimeSpan.FromMilliseconds(UART_RX_TIMEOUT_MS))
907 {
908 //If timeout passes, return error code
909 Timestamp();
910 textBoxData.AppendText("Packet incomplete, received bytes count: " + serialPortComPort.BytesToRead + "B" + Environment.NewLine);
911
912 return (Int32)RxReturnCodes.PACKET_INCOMPLETE;
913 }
914 }
915
916 serialPortComPort.Read(outBuffer, 0, outBuffer.Length);
917
918 Timestamp();
919 textBoxData.AppendText("RECEIVED: " + outBuffer[0].ToString() + " " + outBuffer[1].ToString() + " " + outBuffer[2].ToString() + " " + outBuffer[3].ToString() + " " + outBuffer[4].ToString() + " " + outBuffer[5].ToString() + " " + outBuffer[6].ToString() + " " + outBuffer[7].ToString() + " " + outBuffer[8].ToString() + " " + outBuffer[9].ToString() + " " + outBuffer[10].ToString() + " " + outBuffer[11].ToString() + Environment.NewLine);
920
921 //Calculate checksum
922 for (Int32 i = 0; i < outBuffer.Length - 1; i++)
923 sum += (Byte)outBuffer[i];
924
925 //Return error code if checksum is not correct
926 if (sum != outBuffer[outBuffer.Length - 1])
927 {
928 Timestamp();
929 textBoxData.AppendText("Wrong checksum: " + sum.ToString() + " != " + outBuffer[outBuffer.Length - 1].ToString() + Environment.NewLine);
930
931 return (Int32)RxReturnCodes.WRONG_CHECKSUM;
932 }
933
934 return (Int32)RxReturnCodes.TRANSMISSION_OK;
935 }
936
942 private bool JoystickXinThreshold(Int32 X)
943 {
944 Int32 threshold = (Int32)((JOYSTICK_RANGE / 2) * ((float)numericUpDownThresholdPerc.Value / 100.0));
945
946 if ((JOYSTICK_RANGE / 2 - threshold) > X || (JOYSTICK_RANGE / 2 + threshold) < X)
947 return false;
948
949 return true;
950 }
951
957 private bool JoystickYinThreshold(Int32 Y)
958 {
959 Int32 threshold = (Int32)((JOYSTICK_RANGE / 2) * ((float)numericUpDownThresholdPerc.Value / 100.0));
960
961 if ((JOYSTICK_RANGE / 2 - threshold) > Y || (JOYSTICK_RANGE / 2 + threshold) < Y)
962 return false;
963
964 return true;
965 }
966
973 private void SendCommandsToMotors(Int32 X, Int32 Y, Int32 Z)
974 {
975 Int32 offset = JOYSTICK_RANGE / 2, tempX, tempY;
976 UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];
977 float tempA, tempB, gain;
978
979 //Calculate values
980 gain = (Z / (float)JOYSTICK_RANGE);
981
982 tempX = X - offset;
983 tempY = Y - offset;
984
985 tempA = ((tempY - tempX) * gain) + offset;
986 tempB = ((tempY + tempX) * gain) + offset;
987
988 //Truncate values
989 if (tempA > JOYSTICK_RANGE) tempA = JOYSTICK_RANGE;
990 if (tempB > JOYSTICK_RANGE) tempB = JOYSTICK_RANGE;
991 if (tempA < 0) tempA = 0;
992 if (tempB < 0) tempB = 0;
993
994 //Visualize using progress bars
995 progressBarRight.Value = (Int32)tempA;
996 progressBarLeft.Value = (Int32)tempB;
997 labelRychlostA.Text = ((Int32)tempA).ToString();
998 labelRychlostB.Text = ((Int32)tempB).ToString();
999
1000 //Switch control methods
1001 if (comboBoxMeanOfControl.SelectedIndex == 0)
1002 {
1003 //Use feedforward moment control
1004 outputBuffer[0] = 0xC0;
1005 outputBuffer[1] = 0x00;
1006 outputBuffer[2] = (UInt32)tempA;
1007 UARTsend(outputBuffer);
1008
1009 outputBuffer[0] = 0xC1;
1010 outputBuffer[1] = 0x00;
1011 outputBuffer[2] = (UInt32)tempB;
1012 UARTsend(outputBuffer);
1013 }
1014 else
1015 {
1016 //Use speed regulators
1017 Int16 temp = (Int16)(Convert.ToInt16(labelRychlostA.Text) - offset + 500);
1018
1019 outputBuffer[0] = 0xC2;
1020 outputBuffer[1] = (UInt32)temp >> 8;
1021 outputBuffer[2] = (UInt32)temp % 256;
1022 UARTsend(outputBuffer);
1023
1024 temp = (Int16)(Convert.ToInt16(labelRychlostB.Text) - offset + 500);
1025
1026 outputBuffer[0] = 0xC3;
1027 outputBuffer[1] = (UInt32)temp >> 8;
1028 outputBuffer[2] = (UInt32)temp % 256;
1029 UARTsend(outputBuffer);
1030 }
1031 }
1032
1037 private static void COMPortStatusGuard_Method(FormMain form)
1038 {
1039 while (true)
1040 {
1041 if (form.serialPortComPort.IsOpen != form.portState)
1042 {
1043 //Invoke event
1044 if (COMStatusChanged != null)
1045 COMStatusChanged();
1046 }
1047
1048 form.portState = form.serialPortComPort.IsOpen;
1049
1050 Thread.Sleep(COM_PORT_STATUS_SAMPLING_PERIOD_MS);
1051 }
1052 }
1053
1057 private void AutoParkingThreadMethod()
1058 {
1059 try
1060 {
1061 uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1]; //UART data buffer
1062 Boolean error; //Error flag
1063
1064 while (true)
1065 {
1066 //Reset error flag
1067 error = false;
1068
1069 //Reset regulation flag
1070 wheelchairModel.IsRegulationActive = false;
1071
1072 //Run state machine
1073 switch (wheelchairModel.CurrentPhase)
1074 {
1075 //Init algorithm
1076 case WheelchairModel.AutoParkingPhase.WAITING_FOR_START:
1077
1078 //Start algorithm
1079 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.WALL_FOLLOWING;
1080
1081 break;
1082
1083 //Wall-following phase
1084 case WheelchairModel.AutoParkingPhase.WALL_FOLLOWING:
1085
1086 //Invoke
1087 this.Invoke((MethodInvoker)delegate()
1088 {
1089 //Reset odometry
1090 ResetOdometry();
1091
1092 //Set desired X (wall-following track limit)
1093 outputBuffer[0] = 0xD0;
1094 outputBuffer[1] = (Byte)(Car.DRIVER_TO_REAR_DISTANCE_CM >> 8);
1095 outputBuffer[2] = (Byte)(Car.DRIVER_TO_REAR_DISTANCE_CM & 0x00FF);
1096 UARTsend(outputBuffer);
1097
1098 //Start wall-following
1099 outputBuffer[0] = 0xC6;
1100 outputBuffer[1] = (Byte)(WheelchairModel.VDES + 128);
1101 outputBuffer[2] = (Byte)(WheelchairModel.Y0DES);
1102 UARTsend(outputBuffer);
1103
1104 wheelchairModel.IsRegulationActive = true;
1105 });
1106
1107 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.GOING_BACKWARDS;
1108
1109 break;
1110
1111 //Driving backwards to get marks into the field of view
1112 case WheelchairModel.AutoParkingPhase.GOING_BACKWARDS:
1113 //Invoke
1114 this.Invoke((MethodInvoker)delegate()
1115 {
1116 Int16 temp = (Int16)(WheelchairModel.VDES + 500);
1117
1118 //Reset odometry
1119 ResetOdometry();
1120
1121 //Send commands to both motors to go back(do not use point-to-point regulator, wheelchair would rotate 180°)
1122 outputBuffer[0] = 0xC2;
1123 outputBuffer[1] = (uint)temp >> 8;
1124 outputBuffer[2] = (uint)temp % 256;
1125 UARTsend(outputBuffer);
1126
1127 outputBuffer[0] = 0xC3;
1128 UARTsend(outputBuffer);
1129 });
1130
1131 while (Math.Abs(wheelchairModel.X) < (100 * Car.PLATFORM_LENGTH))
1132 {
1133 //Wait for regulation to end
1134 Thread.Sleep(timerLog.Interval);
1135 }
1136
1137 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.MARKS_TRACKING;
1138
1139 break;
1140
1141 //Look for marks
1142 case WheelchairModel.AutoParkingPhase.MARKS_TRACKING:
1143 {
1144 Point leftMark, rightMark;
1145 Boolean nextLoop = false;
1146
1147 //Invoke
1148 this.Invoke((MethodInvoker)delegate()
1149 {
1150 //Reset detectors
1151 leftMarkDetector.Reset();
1152 rightMarkDetector.Reset();
1153 });
1154
1155 while (!leftMarkDetector.IsBufferFull || !rightMarkDetector.IsBufferFull)
1156 { /*Wait for buffers to fill up*/ }
1157
1158 if (leftMarkDetector.DetectMark(out leftMark) && rightMarkDetector.DetectMark(out rightMark))
1159 {
1160 //Marks detected, actualize map and position
1161 Double tmpX = 0, tmpY = 0, tmpPhi;
1162
1163 //Invoke
1164 this.Invoke((MethodInvoker)delegate()
1165 {
1166 //Check if user confirmation is required
1167 if (checkBoxUserResponseDemanded.Checked)
1168 {
1169 #region Send
1170
1171 //Send image and wait for response
1172 List<Byte> packetTemp = new List<Byte>() { (Byte)PacketHeader.CAMERA_FRAME_TO_BE_CONFIRMED };
1173 MemoryStream outStream = new MemoryStream();
1174 IPAddress targetIp = null;
1175
1176 try
1177 {
1178 targetIp = IPAddress.Parse(textBoxTargetIP.Text);
1179 }
1180 catch (Exception)
1181 { /*Failed to load addres*/}
1182
1183 //Set encoder parameters
1184 ImageProcessingTools.cameraFrameEncoderParameters.Param[ImageProcessingTools.ENCODER_PARAMETER_FIRST]
1185 = ImageProcessingTools.cameraFrameEncoderParameter[(Byte)numericUpDownJpegCompression.Value];
1186
1187 //Serialize image into the stream
1188 using (Image tempImg = new Bitmap(pbFrame.Image))
1189 tempImg.Save(outStream, ImageProcessingTools.jpegCodec, ImageProcessingTools.cameraFrameEncoderParameters);
1190
1191 //Append serialized image data
1192 foreach (Byte dataByte in outStream.ToArray())
1193 packetTemp.Add(dataByte);
1194
1195 //Reset the autoResetEvent
1196 arUserResponseTimout.Reset();
1197
1198 //Send
1199 outgoingUDPClient.Send(packetTemp.ToArray(), packetTemp.Count, new IPEndPoint(targetIp, UDP_PORT));
1200
1201 #endregion
1202
1203 #region Response
1204
1205 //Wait for response
1206 if (!arUserResponseTimout.WaitOne(WheelchairModel.USER_RESPONSE_TIMEOUT_MS))
1207 {
1208 //Response not received, end
1209 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.END;
1210 error = true;
1211 }
1212
1213 //Select proper action
1214 switch (wheelchairModel.UserResponse)
1215 {
1216 //End now
1217 default:
1218 case WheelchairModel.UserResponseCode.END:
1219
1220 //End script
1221 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.END;
1222 error = true;
1223
1224 break;
1225
1226 //Rotate and try to find marks
1227 case WheelchairModel.UserResponseCode.LOOK_FURTHER:
1228
1229 ResetOdometry();
1230
1231 wheelchairModel.IsRegulationActive = true;
1232
1233 //Rotate
1234 outputBuffer[0] = 0xC7;
1235 outputBuffer[1] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD >> 8);
1236 outputBuffer[2] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD & 0x00FF);
1237 UARTsend(outputBuffer);
1238
1239 //Run next loop
1240 nextLoop = true;
1241
1242 break;
1243
1244 //Ok, proceed
1245 case WheelchairModel.UserResponseCode.MARKS_FOUND:
1246 break;
1247 }
1248
1249 #endregion
1250 }
1251
1252 if (!nextLoop) //Exit loop (break / continue cannot be used from within the Invoke() block)
1253 {
1254 //Actualize marks' positions
1255 if (leftMark.X > 0 && leftMark.Y > 0)
1256 worldMap.ModifyMark(WorldMap.MarkPosition.LEFT, leftMark, Color.GreenYellow);
1257
1258 if (rightMark.X > 0 && rightMark.Y > 0)
1259 worldMap.ModifyMark(WorldMap.MarkPosition.RIGHT, rightMark, Color.Red);
1260
1261 //Racalculate coordinates
1262 if (!RecalculateRefPointToMarksCenter(ref tmpX, ref tmpY, out tmpPhi))
1263 error = true;
1264
1265 //Proceed only if no error occured
1266 if (!error)
1267 {
1268 //Actualize wheelchair odometry
1269 try
1270 {
1271 //Convert to Int16
1272 Int16 iX = Convert.ToInt16(100 * tmpX); //Convert to cm
1273 Int16 iY = Convert.ToInt16(100 * tmpY); //Convert to cm
1274 Int16 iPhi = Convert.ToInt16(tmpPhi);
1275
1276 //Actualize odometry
1277 outputBuffer[0] = 0xA0;
1278 outputBuffer[1] = (Byte)(iX >> 8);
1279 outputBuffer[2] = (Byte)(iX & 0x00FF);
1280 UARTsend(outputBuffer);
1281
1282 outputBuffer[0] = 0xA1;
1283 outputBuffer[1] = (Byte)(iY >> 8);
1284 outputBuffer[2] = (Byte)(iY & 0x00FF);
1285 UARTsend(outputBuffer);
1286
1287 outputBuffer[0] = 0xA2;
1288 outputBuffer[1] = (Byte)(iPhi >> 8);
1289 outputBuffer[2] = (Byte)(iPhi & 0x00FF);
1290 UARTsend(outputBuffer);
1291
1292 //Switch state
1293 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.PATH_FOLLOWING;
1294 }
1295 catch (Exception)
1296 {
1297 //Error occured
1298 error = true;
1299 }
1300 }
1301 }
1302 });
1303 }
1304 else
1305 {
1306 //Invoke
1307 this.Invoke((MethodInvoker)delegate()
1308 {
1309 //Marks not detected, rotate and try again
1310 ResetOdometry();
1311
1312 wheelchairModel.IsRegulationActive = true;
1313
1314 //Rotate
1315 outputBuffer[0] = 0xC7;
1316 outputBuffer[1] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD >> 8);
1317 outputBuffer[2] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD & 0x00FF);
1318 UARTsend(outputBuffer);
1319 });
1320 }
1321 }
1322
1323 break;
1324
1325 //Driving to next waypoint
1326 case WheelchairModel.AutoParkingPhase.PATH_FOLLOWING:
1327 {
1328 //Transform target coordinates
1329 Double targetX = worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.X;
1330 Double targetY = worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.Y;
1331 Double tempPhi;
1332
1333 //Racalculate coordinates
1334 if (!RecalculateRefPointToMarksCenter(ref targetX, ref targetY, out tempPhi))
1335 {
1336 error = true;
1337 }
1338
1339 //Check if wheelchair is in the target position
1340 if (Math.Sqrt((Double)(Math.Pow((wheelchairModel.X - targetX / 10 /*mm to cm*/), 2) + Math.Pow((wheelchairModel.Y - targetY / 10 /*mm to cm*/), 2))) < WheelchairModel.TARGET_COORD_ALLOWED_ERROR_CM)
1341 {
1342 //Target reached, remove point from buffer
1343 if (worldMap.TrackPoints.Count == 1)
1344 {
1345 //Last point reached, end
1346 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.END;
1347 }
1348 else
1349 {
1350 //Remove point
1351 worldMap.TrackPoints.Remove(worldMap.TrackPoints[worldMap.TrackPoints.Count - 1]);
1352 }
1353 }
1354
1355 #region Navigate to next point
1356
1357 //Invoke
1358 this.Invoke((MethodInvoker)delegate()
1359 {
1360 //Set desired coordinates
1361 //Set desired X
1362 outputBuffer[0] = 0xD0;
1363 outputBuffer[1] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.X) >> 8);
1364 outputBuffer[2] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.X) & 0x00FF);
1365 UARTsend(outputBuffer);
1366
1367 //Set desired Y
1368 outputBuffer[0] = 0xD1;
1369 outputBuffer[1] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.Y) >> 8);
1370 outputBuffer[2] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.Y) & 0x00FF);
1371 UARTsend(outputBuffer);
1372
1373 //Set regulation flag
1374 wheelchairModel.IsRegulationActive = true;
1375
1376 //Start regulation
1377 outputBuffer[0] = 0xC7;
1378 outputBuffer[1] = 0x00;
1379 outputBuffer[2] = 0x00;
1380 UARTsend(outputBuffer);
1381 });
1382
1383 //Switch state and find marks again
1384 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.MARKS_TRACKING;
1385
1386 #endregion
1387 }
1388
1389 break;
1390
1391 //End script
1392 default:
1393 case WheelchairModel.AutoParkingPhase.END:
1394
1395 //Invoke
1396 this.Invoke((MethodInvoker)delegate()
1397 {
1398 buttonSTOP_Click(this, null);
1399 });
1400
1401 return;
1402 }
1403
1404 //Wait for regulation asks to end
1405 while (wheelchairModel.IsRegulationActive)
1406 {
1407 Thread.Sleep(timerLog.Interval);
1408 }
1409
1410 //Ligh up correct label
1411 VisualizePhase(wheelchairModel.CurrentPhase, error);
1412 }
1413 }
1414 catch (ThreadAbortException)
1415 {
1416 //Restart algorithm
1417 wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.WAITING_FOR_START;
1418 VisualizePhase(wheelchairModel.CurrentPhase, false);
1419 }
1420 catch (Exception)
1421 { }
1422 }
1423
1427 private void FormMain_COMStatusChanged()
1428 {
1429 try
1430 {
1431 //Enable / disable all form's controls
1432 foreach (Control formControl in this.Controls)
1433 {
1434 if (formControl is Button && (String)formControl.Tag == "ON_CLOSED_PORT_DISABLE_TAG")
1435 {
1436 if (this.InvokeRequired)
1437 {
1438 if (serialPortComPort.IsOpen)
1439 this.Invoke((MethodInvoker)delegate() { formControl.Enabled = true; });
1440 else
1441 this.Invoke((MethodInvoker)delegate() { formControl.Enabled = false; });
1442
1443 continue;
1444 }
1445
1446 if (serialPortComPort.IsOpen)
1447 formControl.Enabled = true;
1448 else
1449 formControl.Enabled = false;
1450 }
1451 }
1452
1453 //Enable / disable auto parking start button
1454 if (this.InvokeRequired)
1455 {
1456 if (serialPortComPort.IsOpen)
1457 this.Invoke((MethodInvoker)delegate() { buttonSTARTAutoParking.Enabled = true; });
1458 else
1459 this.Invoke((MethodInvoker)delegate() { buttonSTARTAutoParking.Enabled = false; });
1460 }
1461 }
1462 catch (ObjectDisposedException)
1463 { /*Form main already disposed -> nothing to do*/}
1464 catch (Exception)
1465 { /*Do not process exceptions thrown in this section*/}
1466 }
1467
1473 private void ApplicationExit_EventHandler(object sender, EventArgs e)
1474 {
1475 //Stop and dispose capture
1476 if (capture != null)
1477 {
1478 capture.Stop();
1479 capture.Dispose();
1480 }
1481
1482 //Wait for UDPreceive thread to abort (just for sure)
1483 UDPreceiveThread.Abort();
1484 UDPreceiveThread.Join();
1485
1486 //Wait for auto parking thread to abort (just for sure)
1487 if (AutoParkingThread != null)
1488 {
1489 AutoParkingThread.Abort();
1490 AutoParkingThread.Join();
1491 }
1492 }
1493
1494 #endregion
1495
1499 public FormMain()
1500 {
1501 InitializeComponent();
1502
1503 //World map bitmap
1504 frameWorldMap = new Bitmap(pbWorldMap.Width, pbWorldMap.Height);
1505
1506 //Fill serial ports
1507 foreach (string port in SerialPort.GetPortNames())
1508 comboBoxPortSelection.Items.Add(port);
1509
1510 if (comboBoxPortSelection.Items.Count == 0)
1511 {
1512 comboBoxPortSelection.Items.Add("No port found");
1513 buttonOpenPort.Enabled = false;
1514 }
1515
1516 //Select default comboboxes' items
1517 comboBoxMeanOfControl.SelectedIndex = (Int32)ComboboxControlIndexes.SPEED_REGULATOR;
1518
1519 //Init com port guard thread
1520 threadCOMPortStatusGuard = new Thread(delegate() { COMPortStatusGuard_Method(this); });
1521 threadCOMPortStatusGuard.IsBackground = true;
1522 threadCOMPortStatusGuard.Start();
1523
1524 //UDP cients init
1525 outgoingUDPClient = new UdpClient();
1526 incomingUDPClient = new UdpClient(ipEp);
1527
1528 //Init user response auto reset event
1529 arUserResponseTimout = new AutoResetEvent(false);
1530
1531 //Run UDP receiver thread
1532 UDPreceiveThread = new Thread(new ThreadStart(UDPDataReceiver));
1533 UDPreceiveThread.IsBackground = true;
1534 UDPreceiveThread.Priority = ThreadPriority.Normal;
1535 UDPreceiveThread.Start();
1536
1537 //Register events
1538 COMStatusChanged += new COMPortStatusChangedDelegate(FormMain_COMStatusChanged);
1539 Application.ApplicationExit += ApplicationExit_EventHandler;
1540 capture.ImageGrabbed += ImageGrabbed_EventHandler;
1541
1542 //Start capturing frames - captured frame handled by imageGrabbed event
1543 capture.Start();
1544 }
1545
1546 #region Auto generated control events handlers
1547
1548 private void comboBoxPortSelection_SelectedIndexChanged(object sender, EventArgs e)
1549 {
1550 //Try to change port name
1551 try
1552 {
1553 if (serialPortComPort.IsOpen == false)
1554 serialPortComPort.PortName = comboBoxPortSelection.Text;
1555 }
1556 catch (Exception exc)
1557 {
1558 MessageBox.Show(exc.Message.ToString(), "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
1559 }
1560 }
1561
1562 private void buttonOpenPort_Click(object sender, EventArgs e)
1563 {
1564 if (!serialPortComPort.IsOpen)
1565 {
1566 //Try to open the port
1567 try
1568 {
1569 serialPortComPort.Open();
1570 }
1571 catch (Exception exc)
1572 {
1573 MessageBox.Show(exc.Message.ToString(), "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
1574 }
1575
1576 if (serialPortComPort.IsOpen)
1577 {
1578 //Handle port controls
1579 comboBoxPortSelection.Enabled = false;
1580 labelI2CPortStatus.Text = comboBoxPortSelection.Text + " open";
1581 buttonOpenPort.BackColor = Color.MediumSpringGreen;
1582 buttonOpenPort.Text = "CLOSE";
1583
1584 //Enable START button + send stop command immediately
1585 buttonSTOP_Click(this, null);
1586
1587 //Save port name for future use
1588 WheelchairControl.Properties.Settings.Default.Setting_PortName = serialPortComPort.PortName;
1589 WheelchairControl.Properties.Settings.Default.Save();
1590 }
1591 else
1592 {
1593 labelI2CPortStatus.Text = comboBoxPortSelection.Text + " opening failed";
1594 }
1595 }
1596 else
1597 {
1598 //Stop motors first
1599 buttonSTOP_Click(this, null);
1600
1601 serialPortComPort.Close();
1602
1603 if (!serialPortComPort.IsOpen)
1604 {
1605 //Handle port controls
1606 comboBoxPortSelection.Enabled = true;
1607 labelI2CPortStatus.Text = comboBoxPortSelection.Text + " closed";
1608 buttonOpenPort.BackColor = Color.Red;
1609 buttonOpenPort.Text = "OPEN";
1610
1611 //Disable START button
1612 buttonSTART.Enabled = false;
1613 }
1614 else
1615 {
1616 labelI2CPortStatus.Text = comboBoxPortSelection.Text + " closing failed";
1617 }
1618 }
1619 }
1620
1621 private void buttonRefresh_Click(object sender, EventArgs e)
1622 {
1623 //Refresh available ports
1624 if (serialPortComPort.IsOpen == false)
1625 {
1626 comboBoxPortSelection.Items.Clear();
1627
1628 foreach (string port in SerialPort.GetPortNames())
1629 comboBoxPortSelection.Items.Add(port);
1630
1631 if (comboBoxPortSelection.Items.Count == 0)
1632 {
1633 comboBoxPortSelection.Items.Add("No port found");
1634 buttonOpenPort.Enabled = false;
1635 }
1636 else
1637 {
1638 buttonOpenPort.Enabled = true;
1639 }
1640
1641 comboBoxPortSelection.SelectedIndex = 0;
1642 }
1643 }
1644
1645 private void buttonDelete_Click(object sender, EventArgs e)
1646 {
1647 if (MessageBox.Show("Do you really want to delete the log?", "Are you sure?", MessageBoxButtons.YesNo, MessageBoxIcon.Question) == DialogResult.Yes)
1648 textBoxData.Clear();
1649 }
1650
1651 private void buttonReadData2_Click(object sender, EventArgs e)
1652 {
1653 Byte[] inBuffer = new Byte[12];
1654 Int32 temp;
1655
1656 temp = UARTreceive(0xF2, ref inBuffer);
1657
1658 if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK) //Wrong data received
1659 {
1660 return;
1661 }
1662
1663 textBoxData.AppendText( Environment.NewLine + Environment.NewLine +
1664 "DATA 2 : Refreshed!" + Environment.NewLine + Environment.NewLine +
1665 "Max. Accel.: " + inBuffer[0].ToString() + Environment.NewLine +
1666 "Kp: " + inBuffer[1].ToString() + Environment.NewLine +
1667 "Kd: " + inBuffer[2].ToString() + Environment.NewLine +
1668 "Ki: " + inBuffer[3].ToString() + Environment.NewLine + Environment.NewLine +
1669 "Rounds [imp/50ms] of right wheel: " + ((SByte)inBuffer[4]).ToString() + Environment.NewLine +
1670 "Rounds [imp/50ms] of left wheel: " + ((SByte)inBuffer[5]).ToString() + Environment.NewLine +
1671 "Impulses of right wheel: " + (((SByte)inBuffer[6] << 8) + (inBuffer[7])).ToString() + Environment.NewLine +
1672 "Impulses of left wheel: " + (((SByte)inBuffer[8] << 8) + (inBuffer[9])).ToString() + Environment.NewLine + Environment.NewLine +
1673 "Speed regulator deadband: " + inBuffer[10].ToString() + Environment.NewLine);
1674 }
1675
1676 private void buttonReadData1_Click(object sender, EventArgs e)
1677 {
1678
1679 Byte[] inBuffer = new Byte[12];
1680 Int32 temp;
1681
1682 temp = UARTreceive(0xF1, ref inBuffer);
1683
1684 if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK) //Wrong data received
1685 {
1686 return;
1687 }
1688
1689 textBoxData.AppendText( Environment.NewLine + Environment.NewLine +
1690 "DATA 1 : Refreshed!" + Environment.NewLine + Environment.NewLine +
1691 "SRF08 Front address: " + inBuffer[8].ToString("X") + Environment.NewLine +
1692 "SRF08 Rear address: " + inBuffer[9].ToString("X") + Environment.NewLine + Environment.NewLine +
1693 "Imp. per round right wheel: " + (2 * inBuffer[0]).ToString() + Environment.NewLine +
1694 "Imp. per round left wheel: " + (2 * inBuffer[1]).ToString() + Environment.NewLine +
1695 "Right wheel circumference [cm]: " + ((inBuffer[2] << 8) + (inBuffer[3])).ToString() + Environment.NewLine +
1696 "Left wheel circumference [cm]: " + ((inBuffer[4] << 8) + (inBuffer[5])).ToString() + Environment.NewLine +
1697 "Wheel track [cm]: " + ((inBuffer[6] << 8) + (inBuffer[7])).ToString() + Environment.NewLine + Environment.NewLine +
1698 "SRF08 rear sensor correction [cm]: " + ((SByte)inBuffer[10]).ToString() + Environment.NewLine);
1699
1700 }
1701
1702 private void buttonReadData3_Click(object sender, EventArgs e)
1703 {
1704 Byte[] inBuffer = new Byte[12];
1705 Int32 temp;
1706
1707 temp = UARTreceive(0xF3, ref inBuffer);
1708
1709 if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK) //Wrong data received
1710 {
1711 return;
1712 }
1713
1714 textBoxData.AppendText( Environment.NewLine + Environment.NewLine +
1715 "DATA of the regulator and SRF08 : Refreshed!" + Environment.NewLine + Environment.NewLine +
1716 "Omega: " + inBuffer[0].ToString() + Environment.NewLine +
1717 "L: " + inBuffer[1].ToString() + Environment.NewLine +
1718 "Ksi: " + inBuffer[2].ToString() + Environment.NewLine +
1719 "Ky: " + (((SByte)inBuffer[3] << 8) + (inBuffer[4])).ToString() + Environment.NewLine +
1720 "Kfi: " + (((SByte)inBuffer[5] << 8) + (inBuffer[6])).ToString() + Environment.NewLine + Environment.NewLine +
1721 "Desired speed [cm/s]: " + (inBuffer[7] - 128).ToString() + Environment.NewLine +
1722 "Desired y0 [cm]: " + inBuffer[8].ToString() + Environment.NewLine + Environment.NewLine +
1723 "SRF08 front distance [cm]: " + inBuffer[9].ToString() + Environment.NewLine +
1724 "SRF08 rear distance [cm]: " + inBuffer[10].ToString() + Environment.NewLine);
1725 }
1726
1727 private void buttonSTOP_Click(object sender, EventArgs e)
1728 {
1729 motorControlAllowed = false;
1730
1731 buttonSTART.Enabled = true;
1732
1733 //End auto parking thread
1734 if (AutoParkingThread != null)
1735 AutoParkingThread.Abort();
1736
1737 SendStopSequence();
1738
1739 //Stop log
1740 if (logWheelchairRunDataToFileToolStripMenuItem.Text == "Stop log")
1741 logWheelchairRunDataToFileToolStripMenuItem_Click(sender, null);
1742 }
1743
1744 private void timerWatchdog_Tick(object sender, EventArgs e)
1745 {
1746 if (serialPortComPort.IsOpen)
1747 {
1748 if (!timerLog.Enabled) //If logging timer is enabled, MCU is polled periodically, no need to run this
1749 {
1750 UInt32[] tempData = new UInt32[RX_DATA_LENGTH - 1];
1751
1752 for (Byte i = 0; i < tempData.Length; i++)
1753 tempData[i] = 0x00;
1754
1755 UARTsend(tempData);
1756 }
1757
1758 //Blink label
1759 if (ssLabelUARTWatchdog.BackColor == Color.LimeGreen)
1760 ssLabelUARTWatchdog.BackColor = ssMain.BackColor;
1761 else
1762 ssLabelUARTWatchdog.BackColor = Color.LimeGreen;
1763 }
1764 else
1765 {
1766 ssLabelUARTWatchdog.BackColor = this.BackColor;
1767 }
1768 }
1769
1770 private void timerJoystick_Tick(object sender, EventArgs e)
1771 {
1772 if (comboBoxUserInput.SelectedIndex == (Int32)ComboboxUserInputIndexes.JOYSTICK && joystick != null)
1773 {
1774 Int32 stateX, stateY, stateZ; //Joy axes' states
1775
1776 joystick.GetData();
1777
1778 //Read hw buttons
1779 if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_UPPER_TRIGGER_1))
1780 {
1781 buttonSTOP_Click(joystick, null);
1782 }
1783 else if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_TRIGGER))
1784 {
1785 buttonSTART_Click(joystick, null);
1786 }
1787
1788 //Recalculate joystick data
1789 stateX = joystick.State.X;
1790 stateY = JOYSTICK_RANGE - joystick.State.Y;
1791 stateZ = JOYSTICK_RANGE - joystick.State.Z;
1792
1793 //Determine if values are in threshold
1794 if (JoystickXinThreshold(stateX))
1795 {
1796 stateX = JOYSTICK_RANGE / 2;
1797 }
1798
1799 if (JoystickYinThreshold(stateY))
1800 {
1801 stateY = JOYSTICK_RANGE / 2;
1802 }
1803
1804 //Visualize values
1805 progressBarX.Value = stateX;
1806 progressBarY.Value = stateY;
1807 progressBarZ.Value = stateZ;
1808
1809 labelX.Text = stateX.ToString();
1810 labelY.Text = stateY.ToString();
1811 labelZ.Text = stateZ.ToString();
1812
1813 //Sned commands to motors
1814 if (motorControlAllowed)
1815 {
1816 SendCommandsToMotors(stateX, stateY, stateZ);
1817 }
1818 }
1819 else if (comboBoxUserInput.SelectedIndex == (Int32)ComboboxUserInputIndexes.WIFI)
1820 {
1821 Int32 stateX, stateY, stateZ; //Joy axes' states
1822
1823 //Read joy's buttons and if any of them was clicked, switch control from wifi to joy
1824 if (joystick != null)
1825 {
1826 joystick.GetData();
1827
1828 //Read hw buttons
1829 if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_UPPER_TRIGGER_1))
1830 {
1831 buttonSTOP_Click(joystick, null);
1832 comboBoxUserInput.SelectedIndex = (Byte)ComboboxUserInputIndexes.JOYSTICK;
1833
1834 return;
1835 }
1836 else if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_TRIGGER))
1837 {
1838 buttonSTART_Click(joystick, null);
1839 comboBoxUserInput.SelectedIndex = (Byte)ComboboxUserInputIndexes.JOYSTICK;
1840
1841 return;
1842 }
1843 }
1844
1845 //Read daa from fake joystick
1846 stateX = wifiJoy.X;
1847 stateY = wifiJoy.Y;
1848 stateZ = wifiJoy.Z;
1849
1850 //Check thresholds
1851 if (JoystickXinThreshold(stateX))
1852 {
1853 stateX = JOYSTICK_RANGE / 2;
1854 }
1855
1856 if (JoystickYinThreshold(stateY))
1857 {
1858 stateY = JOYSTICK_RANGE / 2;
1859 }
1860
1861 //Visualize data
1862 progressBarX.Value = stateX;
1863 progressBarY.Value = stateY;
1864 progressBarZ.Value = stateZ;
1865
1866 labelX.Text = stateX.ToString();
1867 labelY.Text = stateY.ToString();
1868 labelZ.Text = stateZ.ToString();
1869
1870 //Drive motors
1871 if (motorControlAllowed)
1872 {
1873 SendCommandsToMotors(stateX, stateY, stateZ);
1874 }
1875 }
1876 }
1877
1878 private void FormMain_Load(object sender, EventArgs e)
1879 {
1880 #region Intialize Joystick
1881
1882 //Initialize joystick
1883 try
1884 {
1885 joystick = new USB_Joystick(0, JOYSTICK_RANGE, 0.2, this);
1886 joystick.PresetValues();
1887 }
1888 catch (Exception)
1889 {
1890 //Display warning
1891 buttonRedetectJoystick.Visible = true;
1892
1893 MessageBox.Show("Failed to intialize joystick!", "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
1894 }
1895
1896 #endregion
1897
1898 #region Load settings
1899
1900 //Load user input combobox's state
1901 comboBoxUserInput.SelectedIndex = (Int32)WheelchairControl.Properties.Settings.Default.Setting_UserInput;
1902
1903 //Load joystick threshold
1904 numericUpDownThresholdPerc.Value = WheelchairControl.Properties.Settings.Default.Setting_JoystickThresh;
1905
1906 //Load auto open enable checkbox's state
1907 checkBoxOpenPortOnStartup.Checked = WheelchairControl.Properties.Settings.Default.Setting_AutoOpenPort;
1908
1909 //Try to open previously used port
1910 Int32 portNameIndex = comboBoxPortSelection.Items.IndexOf(WheelchairControl.Properties.Settings.Default.Setting_PortName);
1911
1912 if (portNameIndex >= 0)
1913 {
1914 comboBoxPortSelection.SelectedIndex = portNameIndex;
1915
1916 //Optionally open port
1917 if (checkBoxOpenPortOnStartup.Checked)
1918 buttonOpenPort_Click(this, null);
1919 }
1920
1921 //Try to load IP
1922 textBoxTargetIP.Text = WheelchairControl.Properties.Settings.Default.Setting_TargetIP;
1923
1924 //Try to load log file default path
1925 labelLogFilePath.Text = WheelchairControl.Properties.Settings.Default.Setting_LogPath;
1926
1927 //Load comboboxDetectionMethod's selected index
1928 comboBoxFeatureDetectionMethod.SelectedIndex = WheelchairControl.Properties.Settings.Default.Setting_DetectionMethod;
1929
1930 //Try to load left model image, init feature detectors
1931 try
1932 {
1933 using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath))
1934 using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
1935 {
1936 ibModelImageLeft.Image = modelImageBGR;
1937
1938 numericUpDownLeftModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);
1939
1940 featureDetectorLeft = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageLeft.Image.Bitmap), (Byte)numericUpDownLeftModelReferenceHue.Value);
1941 }
1942 }
1943 catch (Exception)
1944 {
1945 //Empty image
1946 using(Image<Bgr,Byte> errorImage = new Image<Bgr, Byte>(ibModelImageLeft.Size))
1947 {
1948 ibModelImageLeft.Image = errorImage;
1949 numericUpDownLeftModelReferenceHue.Value = (Byte)numericUpDownLeftModelReferenceHue.Minimum;
1950 featureDetectorLeft = new FeatureDetector(errorImage, (Byte)numericUpDownLeftModelReferenceHue.Value);
1951 }
1952 }
1953
1954 //Try to load right model image
1955 try
1956 {
1957 using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath))
1958 using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
1959 {
1960 ibModelImageRight.Image = modelImageBGR;
1961
1962 numericUpDownRightModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);
1963
1964 featureDetectorRight = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageRight.Image.Bitmap), (Byte)numericUpDownRightModelReferenceHue.Value);
1965 }
1966 }
1967 catch (Exception)
1968 {
1969 //Empty image
1970 using (Image<Bgr, Byte> errorImage = new Image<Bgr, Byte>(ibModelImageRight.Size))
1971 {
1972 ibModelImageRight.Image = errorImage;
1973 numericUpDownRightModelReferenceHue.Value = (Byte)numericUpDownRightModelReferenceHue.Minimum;
1974 featureDetectorRight = new FeatureDetector(errorImage, (Byte)numericUpDownRightModelReferenceHue.Value);
1975 }
1976 }
1977
1978 //Load world map rendering option
1979 comboBoxRenderingMode.SelectedIndex = WheelchairControl.Properties.Settings.Default.Setting_WorldMapRenderingMode;
1980 numericUpDownWorldMapScale.Value = WheelchairControl.Properties.Settings.Default.Setting_WorldMapMaxScale;
1981
1982 //Load world map scale
1983 tbWorldMapScale.Value = (Int32)WheelchairControl.Properties.Settings.Default.Setting_WorldMapScale;
1984
1985 //Load wheelchair drawing settings
1986 checkBoxDrawWheelchair.Checked = WheelchairControl.Properties.Settings.Default.Setting_DrawWheelchair;
1987
1988 //Load user confirmation checkbox state
1989 checkBoxUserResponseDemanded.Checked = WheelchairControl.Properties.Settings.Default.Setting_ConfirmMarksDetection;
1990
1991 //Load watchdog loopback checkbox settings
1992 checkBoxWatchdogResponseEnabled.Checked = WheelchairControl.Properties.Settings.Default.Setting_WatchdogLoopbackEnabled;
1993
1994 //Load jpeg compression level
1995 numericUpDownJpegCompression.Value = WheelchairControl.Properties.Settings.Default.Setting_JpegCompression;
1996
1997 #endregion
1998
1999 #region Other inits
2000
2001 //Set marks center as reference point (default and recommended value)
2002 comboBoxCoordinatesReference.SelectedIndex = (Byte)MapReferenceCoordinates.MARKS_CENTER;
2003
2004 //Load calibration coordinates
2005 LoadCalibrationCoordinates();
2006
2007 //Clear log to delete logged events caused by application start
2008 textBoxData.Clear();
2009
2010 #endregion
2011 }
2012
2013 private void buttonSTART_Click(object sender, EventArgs e)
2014 {
2015 motorControlAllowed = true;
2016
2017 buttonSTART.Enabled = false;
2018 }
2019
2020 private void comboBoxMeanOfControl_SelectedIndexChanged(object sender, EventArgs e)
2021 {
2022 //Stop motors
2023 buttonSTOP_Click(sender, e);
2024
2025 //Toggle menu items
2026 switch (comboBoxMeanOfControl.SelectedIndex)
2027 {
2028 default:
2029
2030 feedforwardMomentControlToolStripMenuItem.Checked = false;
2031 speedRegulatorToolStripMenuItem.Checked = false;
2032
2033 break;
2034
2035 case (Int32)ComboboxControlIndexes.MOMENT:
2036
2037 feedforwardMomentControlToolStripMenuItem.Checked = true;
2038 speedRegulatorToolStripMenuItem.Checked = false;
2039
2040 break;
2041
2042 case (Int32)ComboboxControlIndexes.SPEED_REGULATOR:
2043
2044 feedforwardMomentControlToolStripMenuItem.Checked = false;
2045 speedRegulatorToolStripMenuItem.Checked = true;
2046
2047 break;
2048 }
2049 }
2050
2051 private void buttonReadOdometryData_Click(object sender, EventArgs e)
2052 {
2053 Byte[] inBuffer = new Byte[12];
2054 Int32 temp;
2055
2056 temp = UARTreceive(0xF0, ref inBuffer);
2057
2058 if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK) //Wrong data received
2059 {
2060 return;
2061 }
2062
2063 textBoxData.AppendText(Environment.NewLine + Environment.NewLine +
2064 "DATA of odomety : Refreshed!" + Environment.NewLine + Environment.NewLine +
2065 "X: " + (((SByte)inBuffer[0] << 8) + (inBuffer[1])).ToString() + " [cm]" + Environment.NewLine +
2066 "Y: " + (((SByte)inBuffer[2] << 8) + (inBuffer[3])).ToString() + " [cm]" + Environment.NewLine +
2067 "Phi: " + (((SByte)inBuffer[4] << 8) + (inBuffer[5])).ToString() + " [mrad] = " + ((Double)((180/(1000 * Math.PI))*(((SByte)inBuffer[4] << 8) + (inBuffer[5])))).ToString("F2") + "°" + Environment.NewLine + Environment.NewLine +
2068 "SRF08 front distance: " + inBuffer[6].ToString() + " [cm]" + Environment.NewLine +
2069 "SRF08 rear distance: " + inBuffer[7].ToString() + " [cm]" + Environment.NewLine + Environment.NewLine +
2070 "Position regulator enabled: " + (inBuffer[8] == 0 ? "False" : "True") + Environment.NewLine +
2071 "Wall-following enabled: " + (inBuffer[9] == 0 ? "False" : "True") + Environment.NewLine);
2072 }
2073
2074 private void timerLog_Tick(object sender, EventArgs e)
2075 {
2076 Byte[] inBuffer = new Byte[12];
2077 Int32 temp;
2078
2079 //Acquire data
2080 temp = UARTreceive(0xF0, ref inBuffer);
2081
2082 if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK) //Wrong data received
2083 {
2084 ssLabelMCUPoll.BackColor = Color.Red;
2085
2086 return;
2087 }
2088
2089 try
2090 {
2091 //Read phi and convert it to +-180° range
2092 double phi = (double)((((SByte)inBuffer[(Byte)OdometryPacketByte.PHI_BASE] << 8) + inBuffer[(Byte)OdometryPacketByte.PHI_BASE + 1]) / 1000.0);
2093 phi -= (2 * Math.PI * Math.Floor((phi + Math.PI) / (2 * Math.PI)));
2094 phi *= (180.0 / Math.PI);
2095
2096 if (swLogWriter != null)
2097 {
2098 swLogWriter.WriteLine((System.DateTime.Now - swLogStartTime).Seconds.ToString()
2099 + "." + (System.DateTime.Now - swLogStartTime).Milliseconds.ToString("D3") + ";"
2100 + inBuffer[(Byte)OdometryPacketByte.SRF08_FRONT_DIST].ToString() + ";" //Front SRF08 distance
2101 + inBuffer[(Byte)OdometryPacketByte.SRF08_REAR_DIST].ToString() + ";" //Rear SRF08 distance
2102 + (((SByte)inBuffer[(Byte)OdometryPacketByte.X_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.X_BASE + 1])).ToString() + ";" //X
2103 + (((SByte)inBuffer[(Byte)OdometryPacketByte.Y_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.Y_BASE + 1])).ToString() + ";" //Y
2104 + ((Int16)Math.Truncate(phi)).ToString()); //Phi
2105
2106 swLogWriter.Flush();
2107
2108 //Blink the logging button - symbolize received data
2109 if (logWheelchairRunDataToFileToolStripMenuItem.BackColor == SystemColors.Control)
2110 logWheelchairRunDataToFileToolStripMenuItem.BackColor = Color.LimeGreen;
2111 else
2112 logWheelchairRunDataToFileToolStripMenuItem.BackColor = SystemColors.Control;
2113 }
2114
2115 //Actualize wheelchair model coordinates
2116 wheelchairModel.SetCoordinates((((SByte)inBuffer[(Byte)OdometryPacketByte.X_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.X_BASE + 1])),
2117 (((SByte)inBuffer[(Byte)OdometryPacketByte.Y_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.Y_BASE + 1])),
2118 (((SByte)inBuffer[(Byte)OdometryPacketByte.PHI_BASE] << 8) + inBuffer[(Byte)OdometryPacketByte.PHI_BASE + 1]));
2119
2120 //Stop motors if regulation is done
2121 if (inBuffer[(Byte)OdometryPacketByte.ENABLE_POSITION_REGULATOR] == 0 && inBuffer[(Byte)OdometryPacketByte.ENABLE_WALL_FOLLOWING] == 0) //Read status bytes
2122 {
2123 SendStopSequence();
2124
2125 //Reset flag
2126 wheelchairModel.IsRegulationActive = false;
2127 }
2128 else
2129 {
2130 //Set flag
2131 wheelchairModel.IsRegulationActive = true;
2132 }
2133 }
2134 catch (Exception exc)
2135 {
2136 textBoxData.AppendText("TimerLog: " + exc.Message);
2137
2138 return;
2139 }
2140
2141 //Blink timer active label
2142 if (ssLabelMCUPoll.BackColor == Color.LimeGreen)
2143 ssLabelMCUPoll.BackColor = ssMain.BackColor;
2144 else
2145 ssLabelMCUPoll.BackColor = Color.LimeGreen;
2146 }
2147
2148 private void buttonResetCoor_Click(object sender, EventArgs e)
2149 {
2150 UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];
2151
2152 //Stop motors -> position regulator could start the wheelchair if odometry is reset
2153 buttonSTOP_Click(this, null);
2154
2155 outputBuffer[0] = 0xA3;
2156 outputBuffer[1] = 0x00;
2157 outputBuffer[2] = 0x00;
2158 UARTsend(outputBuffer);
2159 }
2160
2161 private void buttonShowFormManualControl_Click(object sender, EventArgs e)
2162 {
2163 //Stop motors
2164 buttonSTOP_Click(this, null);
2165
2166 //Show form, pass common objects, set focus on form
2167 FormManualControl.GetInstance(serialPortComPort, UARTsend).Show();
2168 FormManualControl.GetInstance(serialPortComPort, UARTsend).BringToFront();
2169 }
2170
2171 private void buttonShowFormSettings_Click(object sender, EventArgs e)
2172 {
2173 //Stop motors
2174 buttonSTOP_Click(this, null);
2175
2176 //Show form, pass common objects, set focus on form
2177 FormSettings.GetInstance(serialPortComPort, UARTsend).Show();
2178 FormSettings.GetInstance(serialPortComPort, UARTsend).BringToFront();
2179 }
2180
2181 private void btnShowFormRegulators_Click(object sender, EventArgs e)
2182 {
2183 //Stop motors
2184 buttonSTOP_Click(this, null);
2185
2186 //Show form, pass common objects, set focus on form
2187 FormRegulators.GetInstance(serialPortComPort, UARTsend,logWheelchairRunDataToFileToolStripMenuItem_Click,logWheelchairRunDataToFileToolStripMenuItem).Show();
2188 FormRegulators.GetInstance(serialPortComPort, UARTsend, logWheelchairRunDataToFileToolStripMenuItem_Click, logWheelchairRunDataToFileToolStripMenuItem).BringToFront();
2189 }
2190
2191 private void buttonOpenPort_EnabledChanged(object sender, EventArgs e)
2192 {
2193 if (!buttonOpenPort.Enabled)
2194 buttonOpenPort.BackColor = SystemColors.Control;
2195 else
2196 buttonOpenPort.BackColor = Color.Red;
2197 }
2198
2199 private void buttonRedetectJoystick_Click(object sender, EventArgs e)
2200 {
2201 try //Try to initialize joystick
2202 {
2203 joystick = new USB_Joystick(0, JOYSTICK_RANGE, 0.2, this);
2204 joystick.PresetValues();
2205
2206 buttonRedetectJoystick.Visible = false;
2207 }
2208 catch (Exception)
2209 {
2210
2211 MessageBox.Show("Failed to intialize joystick!", "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
2212 }
2213 }
2214
2215 private void checkBoxOpenPortOnStartup_CheckedChanged(object sender, EventArgs e)
2216 {
2217 //Save checkbox's status
2218 WheelchairControl.Properties.Settings.Default.Setting_AutoOpenPort = checkBoxOpenPortOnStartup.Checked;
2219 WheelchairControl.Properties.Settings.Default.Save();
2220 }
2221
2222 private void buttonReadRegulatorSettings_Click(object sender, EventArgs e)
2223 {
2224 Byte[] inBuffer = new Byte[12];
2225 Int32 temp;
2226
2227 temp = UARTreceive(0xF4, ref inBuffer);
2228
2229 if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK) //Wrong data received
2230 {
2231 return;
2232 }
2233
2234 textBoxData.AppendText(Environment.NewLine + Environment.NewLine +
2235 "DATA of position regulator : Refreshed!" + Environment.NewLine + Environment.NewLine +
2236 "X desired: " + (((SByte)inBuffer[0] << 8) + (inBuffer[1])).ToString() + " [cm]" + Environment.NewLine +
2237 "Y desired: " + (((SByte)inBuffer[2] << 8) + (inBuffer[3])).ToString() + " [cm]" + Environment.NewLine +
2238 "Gamma: " + ((Byte)inBuffer[4]).ToString() + Environment.NewLine +
2239 "Epsilon: " + ((Byte)inBuffer[5]).ToString() + " [cm]" + Environment.NewLine +
2240 "Angle gain: " + ((Byte)inBuffer[6]).ToString() + Environment.NewLine +
2241 "Position gain: " + ((Byte)inBuffer[7]).ToString() + Environment.NewLine +
2242 "Constant speed: " + ((Byte)inBuffer[8]).ToString() + " [cmps]" + Environment.NewLine);
2243
2244 }
2245
2246 private void buttonRedetectJoystick_VisibleChanged(object sender, EventArgs e)
2247 {
2248 //Toggle main menu item enable property
2249 reconnectJoystickToolStripMenuItem.Enabled = buttonRedetectJoystick.Visible;
2250 }
2251
2252 private void buttonSTOP_EnabledChanged(object sender, EventArgs e)
2253 {
2254 //Toggle main menu item enable property
2255 stopToolStripMenuItem.Enabled = buttonSTOP.Enabled;
2256 }
2257
2258 private void buttonSTART_EnabledChanged(object sender, EventArgs e)
2259 {
2260 //Toggle main menu item enable property
2261 startToolStripMenuItem.Enabled = buttonSTART.Enabled;
2262 }
2263
2264 private void exitToolStripMenuItem1_Click(object sender, EventArgs e)
2265 {
2266 //Exit with native exit code
2267 Application.Exit();
2268 }
2269
2270 private void aboutToolStripMenuItem_Click(object sender, EventArgs e)
2271 {
2272 //Show infobox
2273 MessageBox.Show("Application:\tWheelchair control\n\t\tDiploma thesis\n\nAuthor:\t\tOndrej Vozda\nUniversity:\t\tFEEC BUT\nYear:\t\t2013",
2274 "About", MessageBoxButtons.OK, MessageBoxIcon.Information);
2275 }
2276
2277 private void feedforwardMomentControlToolStripMenuItem_Click(object sender, EventArgs e)
2278 {
2279 //Switch indexes
2280 comboBoxMeanOfControl.SelectedIndex = (Int32)ComboboxControlIndexes.MOMENT;
2281 }
2282
2283 private void speedRegulatorToolStripMenuItem_Click(object sender, EventArgs e)
2284 {
2285 //Switch indexes
2286 comboBoxMeanOfControl.SelectedIndex = (Int32)ComboboxControlIndexes.SPEED_REGULATOR;
2287 }
2288
2289 private void numericUpDownThresholdPerc_ValueChanged(object sender, EventArgs e)
2290 {
2291 //Update menu item
2292 joystickThresholdToolStripMenuItem.Text = "Joystick threshold: " + numericUpDownThresholdPerc.Value.ToString() +"%";
2293
2294 //Save setting
2295 WheelchairControl.Properties.Settings.Default.Setting_JoystickThresh = (Byte)numericUpDownThresholdPerc.Value;
2296 WheelchairControl.Properties.Settings.Default.Save();
2297 }
2298
2299 private void buttonSaveLogToFile_Click(object sender, EventArgs e)
2300 {
2301 SaveFileDialog sfDialog = new SaveFileDialog();
2302 TextWriter writer = null;
2303
2304 //Some initial settings here
2305 sfDialog.AddExtension = true;
2306 sfDialog.InitialDirectory = AppDomain.CurrentDomain.BaseDirectory;
2307 sfDialog.DefaultExt = "txt";
2308 sfDialog.Filter = "Text file (*.txt)|*.txt";
2309 sfDialog.FileName = "Log " + System.DateTime.Now.Hour.ToString("D2") + "_" + System.DateTime.Now.Minute.ToString("D2") + "_" + System.DateTime.Now.Second.ToString("D2");
2310
2311 //Exit if OK button is not clicked
2312 if (sfDialog.ShowDialog() != DialogResult.OK)
2313 return;
2314
2315 //Init writer
2316 try
2317 {
2318 //Don't append, overwrite file
2319 writer = new StreamWriter(sfDialog.FileName,false);
2320
2321 //Write to file
2322 foreach (String line in textBoxData.Lines)
2323 {
2324 writer.WriteLine(line);
2325 }
2326
2327 //Inform user
2328 MessageBox.Show("Log " + sfDialog.FileName + " saved!", "Success!", MessageBoxButtons.OK, MessageBoxIcon.Information);
2329 }
2330 catch(Exception)
2331 {
2332 //Error, show messagebox
2333 MessageBox.Show("Error, file not saved!", "Error!", MessageBoxButtons.AbortRetryIgnore, MessageBoxIcon.Error);
2334 }
2335 finally
2336 {
2337 //Close the file in any case
2338 if(writer != null)
2339 writer.Close();
2340 }
2341 }
2342
2343 private void comboBoxUserInput_SelectedIndexChanged(object sender, EventArgs e)
2344 {
2345 //Erase labels and stop the motors for sure
2346 progressBarX.Value = 0;
2347 progressBarY.Value = 0;
2348 progressBarZ.Value = 0;
2349
2350 labelX.Text = "-";
2351 labelY.Text = "-";
2352 labelZ.Text = "-";
2353
2354 buttonSTOP_Click(sender, e);
2355
2356 //Save settings
2357 WheelchairControl.Properties.Settings.Default.Setting_UserInput = (Byte)comboBoxUserInput.SelectedIndex;
2358 WheelchairControl.Properties.Settings.Default.Save();
2359 }
2360
2361 private void buttonRefreshLocalIP_Click(object sender, EventArgs e)
2362 {
2363 //Get ip
2364 IPHostEntry host = Dns.GetHostEntry(Dns.GetHostName());
2365
2366 foreach (IPAddress ip in host.AddressList)
2367 if (ip.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
2368 labelLocalIP.Text = ip.ToString();
2369 }
2370
2371 private void tabPageSettings_Enter(object sender, EventArgs e)
2372 {
2373 //Try to get local IP
2374 buttonRefreshLocalIP_Click(sender, e);
2375 }
2376
2377 private void textBoxTargetIP_KeyDown(object sender, KeyEventArgs e)
2378 {
2379 if (e.KeyCode == Keys.Enter)
2380 {
2381 IPAddress targetIP;
2382
2383 //Save the new IP, if it is valid
2384 if (IPAddress.TryParse(textBoxTargetIP.Text, out targetIP))
2385 {
2386 WheelchairControl.Properties.Settings.Default.Setting_TargetIP = targetIP.ToString();
2387
2388 WheelchairControl.Properties.Settings.Default.Save();
2389
2390 MessageBox.Show("IP address " + targetIP.ToString() + " saved!", "Success!", MessageBoxButtons.OK, MessageBoxIcon.Information);
2391 }
2392 else
2393 {
2394 MessageBox.Show("IP not valid!", "Error!", MessageBoxButtons.OK, MessageBoxIcon.Error);
2395 }
2396 }
2397 }
2398
2399 private void collapsibleGroupBoxWifiSettings_CollapseBoxClickedEvent(object sender)
2400 {
2401 const UInt16 COMBOBOXES_DISTANCE = 17;
2402
2403 //Move COM settings group box
2404 collapsibleGroupBoxCOM.Top = collapsibleGroupBoxWifiSettings.Top + collapsibleGroupBoxWifiSettings.Height + COMBOBOXES_DISTANCE;
2405 }
2406
2407 private void pbFrame_MouseClick(object sender, MouseEventArgs e)
2408 {
2409 //Switch tabs
2410 //Marks' reference selection
2411 if (tcActionSwitch.SelectedTab == tabPageSelectColor)
2412 {
2413 SelectColorMouseClickRoutine(e);
2414
2415 return;
2416 }
2417
2418 //Camera calibration points selection
2419 if (tcActionSwitch.SelectedTab == tabPageCalibrate)
2420 {
2421 CalibratePointsMouseClickRoutine(e);
2422
2423 return;
2424 }
2425
2426 //Other tabs can be added here
2427 }
2428
2429 private void pbFrame_MouseMove(object sender, MouseEventArgs e)
2430 {
2431 Point cursorCoord = new Point(e.X, e.Y);
2432 PointF worldCoord = cameraCalibrator.TransformPoint(cursorCoord);
2433
2434 //Write coordinates to status bar
2435 ssMainLabelPositionXY.Text = "X: " + e.X.ToString() + " Y: " + e.Y.ToString();
2436 ssMainLabelWorldXY.Text = "X: " + worldCoord.X.ToString("F3") + " Y: " + worldCoord.Y.ToString("F3");
2437
2438 //Change cursor
2439 Cursor = Cursors.Cross;
2440 }
2441
2442 private void pbFrame_MouseLeave(object sender, EventArgs e)
2443 {
2444 ssMainLabelPositionXY.Text = ssMainLabelWorldXY.Text = "X: --- Y: ---";
2445
2446 Cursor = Cursors.Default;
2447 }
2448
2449 private void pbWorldMap_MouseMove(object sender, MouseEventArgs e)
2450 {
2451 //Create data array from current mouse point
2452 Double[,] data = new Double[3, 1]{ {(Double)e.X}, //X
2453 {(Double)e.Y}, //Y
2454 {1.0} //Scale
2455 };
2456
2457 //Matrix from data
2458 Matrix<Double> matrixTemp = new Matrix<double>(data);
2459
2460 //Transform vector using homography matrix
2461 matrixTemp = homMatWorldMapToWorld.Mul(matrixTemp);
2462
2463 //Write coordinates to status bar
2464 ssMainLabelPositionXY.Text = "X: " + e.X.ToString() + " Y: " + e.Y.ToString();
2465
2466 if(comboBoxCoordinatesReference.SelectedIndex == (Byte)MapReferenceCoordinates.WHEELCHAIR_CENTER)
2467 {
2468 //Coordinates' reference point is in center of the wheelchair
2469 ssMainLabelWorldXY.Text = "X: " + matrixTemp[0, 0].ToString("F3") + " Y: " + matrixTemp[1, 0].ToString("F3");
2470 }
2471 else
2472 {
2473 //Do the recalculations first
2474 Double xTemp = matrixTemp[0, 0];
2475 Double yTemp = matrixTemp[1, 0];
2476 Double phiTemp;
2477
2478 if (RecalculateRefPointToMarksCenter(ref xTemp, ref yTemp, out phiTemp))
2479 {
2480 //Display coordinates
2481 ssMainLabelWorldXY.Text = "X: " + xTemp.ToString("F3") + " Y: " + yTemp.ToString("F3") + " Phi: " + (phiTemp / 1000.0).ToString("F3") + " rad ~ " + (180 * phiTemp / (1000 * Math.PI)).ToString("F2") + "°";
2482 }
2483 else
2484 {
2485 //Error - display default label text
2486 ssMainLabelPositionXY.Text = ssMainLabelWorldXY.Text = "X: --- Y: ---";
2487 }
2488 }
2489 }
2490
2491 private void pbWorldMap_MouseLeave(object sender, EventArgs e)
2492 {
2493 ssMainLabelPositionXY.Text = ssMainLabelWorldXY.Text = "X: --- Y: ---";
2494 }
2495
2496 private void tabPageLog_Enter(object sender, EventArgs e)
2497 {
2498 //Enable / disable buttons according to port status
2499 try
2500 {
2501 foreach (Control tabControl in tabPageLog.Controls)
2502 {
2503 if (tabControl is Button && (String)tabControl.Tag == "ON_CLOSED_PORT_DISABLE_TAG")
2504 {
2505 if (this.InvokeRequired)
2506 {
2507 if (serialPortComPort.IsOpen)
2508 this.Invoke((MethodInvoker)delegate() { tabControl.Enabled = true; });
2509 else
2510 this.Invoke((MethodInvoker)delegate() { tabControl.Enabled = false; });
2511
2512 continue;
2513 }
2514
2515 if (serialPortComPort.IsOpen)
2516 tabControl.Enabled = true;
2517 else
2518 tabControl.Enabled = false;
2519 }
2520 }
2521 }
2522 catch (ObjectDisposedException)
2523 { /*Form main already disposed -> nothing to do*/}
2524 }
2525
2526 private void cameraToolStripMenuItem_Click(object sender, EventArgs e)
2527 {
2528 //Switch tab
2529 tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.CAMERA;
2530 }
2531
2532 private void settingsToolStripMenuItem2_Click(object sender, EventArgs e)
2533 {
2534 //Switch tab
2535 tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.SETTINGS;
2536 }
2537
2538 private void logToolStripMenuItem_Click(object sender, EventArgs e)
2539 {
2540 //Switch tab
2541 tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.LOG;
2542 }
2543
2544 private void tabControlMain_SelectedIndexChanged(object sender, EventArgs e)
2545 {
2546 //Disable all first, just for sure
2547 cameraToolStripMenuItem.Checked = false;
2548 settingsToolStripMenuItem2.Checked = false;
2549 logToolStripMenuItem.Checked = false;
2550
2551 toolStripButtonCamera.Checked = false;
2552 toolStripButtonSettings.Checked = false;
2553 toolStripButtonLog.Checked = false;
2554
2555 //Enable correct ones
2556 switch (tabControlMain.SelectedIndex)
2557 {
2558 default: //Camera -> default tab
2559 case (Int32)MainPageTabsIndexes.CAMERA:
2560
2561 cameraToolStripMenuItem.Checked = true;
2562 toolStripButtonCamera.Checked = true;
2563
2564 break;
2565
2566 case (Int32)MainPageTabsIndexes.SETTINGS:
2567
2568 settingsToolStripMenuItem2.Checked = true;
2569 toolStripButtonSettings.Checked = true;
2570
2571 break;
2572
2573 case (Int32)MainPageTabsIndexes.LOG:
2574
2575 logToolStripMenuItem.Checked = true;
2576 toolStripButtonLog.Checked = true;
2577
2578 break;
2579 }
2580 }
2581
2582 private void toolStripButtonCamera_Click(object sender, EventArgs e)
2583 {
2584 //Switch tab
2585 tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.CAMERA;
2586 }
2587
2588 private void toolStripButtonSettings_Click(object sender, EventArgs e)
2589 {
2590 //Switch tab
2591 tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.SETTINGS;
2592 }
2593
2594 private void toolStripButtonLog_Click(object sender, EventArgs e)
2595 {
2596 //Switch tab
2597 tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.LOG;
2598 }
2599
2600 private void buttonCalibrateHomography_Click(object sender, EventArgs e)
2601 {
2602 //Calculate homography using defined points
2603 cameraCalibrator.CalculateHomography();
2604 }
2605
2606 private void buttonLockMarks_Click(object sender, EventArgs e)
2607 {
2608 //Run method in separate thread
2609 new Thread(delegate() { MarkLock_ThreadRoutine(false); }).Start();
2610 }
2611
2612 private void firstToolStripMenuItem_Click(object sender, EventArgs e)
2613 {
2614 Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates
2615
2616 tcActionSwitch.SelectedTab = tabPageSelectColor;
2617
2618 rbSelectColor1.Checked = true;
2619
2620 pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
2621 }
2622
2623 private void secondToolStripMenuItem_Click(object sender, EventArgs e)
2624 {
2625 Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates
2626
2627 tcActionSwitch.SelectedTab = tabPageSelectColor;
2628
2629 rbSelectColor2.Checked = true;
2630
2631 pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
2632 }
2633
2634 private void upperLeftToolStripMenuItem_Click(object sender, EventArgs e)
2635 {
2636 Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates
2637
2638 tcActionSwitch.SelectedTab = tabPageCalibrate;
2639
2640 rbUL.Select();
2641
2642 pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
2643 }
2644
2645 private void upperRightToolStripMenuItem_Click(object sender, EventArgs e)
2646 {
2647 Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates
2648
2649 tcActionSwitch.SelectedTab = tabPageCalibrate;
2650
2651 rbUR.Select();
2652
2653 pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
2654 }
2655
2656 private void lowerRightToolStripMenuItem_Click(object sender, EventArgs e)
2657 {
2658 Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates
2659
2660 tcActionSwitch.SelectedTab = tabPageCalibrate;
2661
2662 rbLR.Select();
2663
2664 pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
2665 }
2666
2667 private void lowerLeftToolStripMenuItem_Click(object sender, EventArgs e)
2668 {
2669 Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates
2670
2671 tcActionSwitch.SelectedTab = tabPageCalibrate;
2672
2673 rbLL.Select();
2674
2675 pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
2676 }
2677
2678 private void deleteLogToolStripMenuItem_Click(object sender, EventArgs e)
2679 {
2680 buttonDelete_Click(sender, e);
2681 }
2682
2683 private void saveLogToFileToolStripMenuItem_Click(object sender, EventArgs e)
2684 {
2685 buttonSaveLogToFile_Click(sender, e);
2686 }
2687
2688 private void readConfig1ToolStripMenuItem_Click(object sender, EventArgs e)
2689 {
2690 if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
2691 {
2692 //Switch tabs and redraw tab
2693 tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
2694 tabPageLog.Refresh();
2695 }
2696
2697 buttonReadData1_Click(sender, e);
2698 }
2699
2700 private void readConfig2ToolStripMenuItem_Click(object sender, EventArgs e)
2701 {
2702 if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
2703 {
2704 //Switch tabs and redraw tab
2705 tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
2706 tabPageLog.Refresh();
2707 }
2708
2709 buttonReadData2_Click(sender, e);
2710 }
2711
2712 private void readWallfollowingRegulatorSettingsToolStripMenuItem_Click(object sender, EventArgs e)
2713 {
2714 if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
2715 {
2716 //Switch tabs and redraw tab
2717 tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
2718 tabPageLog.Refresh();
2719 }
2720
2721 buttonReadData3_Click(sender, e);
2722 }
2723
2724 private void readOdometryDataToolStripMenuItem_Click(object sender, EventArgs e)
2725 {
2726 if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
2727 {
2728 //Switch tabs and redraw tab
2729 tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
2730 tabPageLog.Refresh();
2731 }
2732
2733 buttonReadOdometryData_Click(sender, e);
2734 }
2735
2736 private void readPositionRegulatorDataToolStripMenuItem_Click(object sender, EventArgs e)
2737 {
2738 if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
2739 {
2740 //Switch tabs and redraw tab
2741 tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
2742 tabPageLog.Refresh();
2743 }
2744
2745 buttonReadRegulatorSettings_Click(sender, e);
2746 }
2747
2748 private void logWheelchairRunDataToFileToolStripMenuItem_Click(object sender, EventArgs e)
2749 {
2750 if (logWheelchairRunDataToFileToolStripMenuItem.Text == "Log to file")
2751 {
2752 if (!serialPortComPort.IsOpen)
2753 {
2754 textBoxData.AppendText("Port closed, log failed!" + Environment.NewLine);
2755
2756 return;
2757 }
2758
2759 try
2760 {
2761 //Start new log
2762 String path = @labelLogFilePath.Text + @"\" + System.DateTime.Today.Year.ToString("D4") + "-" + System.DateTime.Today.Month.ToString("D2") + "-"
2763 + System.DateTime.Today.Day.ToString("D2") + "__" + System.DateTime.Now.Hour.ToString("D2") + "-"
2764 + System.DateTime.Now.Minute.ToString("D2") + "-" + System.DateTime.Now.Second.ToString("D2") + ".csv";
2765
2766 swLogWriter = new StreamWriter(path, false, Encoding.ASCII);
2767
2768 //Write log start time
2769 swLogStartTime = DateTime.Now;
2770
2771 //Enable logging timer
2772 timerLog.Enabled = true;
2773
2774 logWheelchairRunDataToFileToolStripMenuItem.Text = "Stop log";
2775 }
2776 catch (IOException)
2777 {
2778 textBoxData.AppendText("Failed to initialize new log\n");
2779 }
2780 }
2781 else
2782 {
2783 //Disable timer
2784 timerLog.Enabled = false;
2785
2786 logWheelchairRunDataToFileToolStripMenuItem.Text = "Log to file";
2787
2788 logWheelchairRunDataToFileToolStripMenuItem.BackColor = SystemColors.Control;
2789
2790 ssLabelMCUPoll.BackColor = SystemColors.Control;
2791
2792 //Close log
2793 try
2794 {
2795 swLogWriter.Close();
2796 }
2797 catch (IOException)
2798 {
2799 textBoxData.AppendText("Failed to close log\n");
2800 }
2801 }
2802 }
2803
2804 private void logWheelchairRunDataToFileToolStripMenuItem_BackColorChanged(object sender, EventArgs e)
2805 {
2806 //Blink status label
2807 ssLabelLogToFile.BackColor = logWheelchairRunDataToFileToolStripMenuItem.BackColor;
2808 }
2809
2810 private void collapsibleGroupBoxInputSettings_CollapseBoxClickedEvent(object sender)
2811 {
2812 const UInt16 COMBOBOXES_DISTANCE = 17;
2813
2814 //Move log settings group box
2815 collapsibleGroupBoxLogSettings.Top = collapsibleGroupBoxInputSettings.Top + collapsibleGroupBoxInputSettings.Height + COMBOBOXES_DISTANCE;
2816 }
2817
2818 private void labelLogFilePath_TextChanged(object sender, EventArgs e)
2819 {
2820 //Copy text to toolStripTextBox
2821 toolStripTextBoxLogRunDataFilePath.Text = labelLogFilePath.Text;
2822 }
2823
2824 private void buttonChangeLogPath_Click(object sender, EventArgs e)
2825 {
2826 FolderBrowserDialog fbDialog = new FolderBrowserDialog();
2827
2828 fbDialog.SelectedPath = @labelLogFilePath.Text;
2829
2830 if (fbDialog.ShowDialog() == DialogResult.OK)
2831 {
2832 //Display path
2833 labelLogFilePath.Text = fbDialog.SelectedPath;
2834
2835 //Save to settings
2836 WheelchairControl.Properties.Settings.Default.Setting_LogPath = fbDialog.SelectedPath;
2837 WheelchairControl.Properties.Settings.Default.Save();
2838 }
2839 }
2840
2841 private void comboBoxFeatureDetectionMethod_SelectedIndexChanged(object sender, EventArgs e)
2842 {
2843 //Switch tabs
2844 switch (comboBoxFeatureDetectionMethod.SelectedIndex)
2845 {
2846 default:
2847 case (Byte)ComboboxDetectionMethodIndexes.REFERENCE_HSV:
2848
2849 wizardTabControlDetectionMethod.SelectedTab = tabPageHSV;
2850
2851 break;
2852
2853 case (Byte)ComboboxDetectionMethodIndexes.SURF_DETECTOR:
2854
2855 wizardTabControlDetectionMethod.SelectedTab = tabPageSurfDetector;
2856
2857 break;
2858 }
2859
2860 //Save settings
2861 WheelchairControl.Properties.Settings.Default.Setting_DetectionMethod = (Byte)comboBoxFeatureDetectionMethod.SelectedIndex;
2862 WheelchairControl.Properties.Settings.Default.Save();
2863 }
2864
2865 private void buttonLoadLeftModelImage_Click(object sender, EventArgs e)
2866 {
2867 OpenFileDialog ofDialog = new OpenFileDialog();
2868
2869 try
2870 {
2871 ofDialog.InitialDirectory = @WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath;
2872 }
2873 catch(Exception)
2874 { ofDialog.InitialDirectory = Environment.CurrentDirectory; }
2875
2876 if (ofDialog.ShowDialog() == DialogResult.OK)
2877 {
2878 try
2879 {
2880 //Load image
2881 using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@ofDialog.FileName))
2882 using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
2883 {
2884 ibModelImageLeft.Image = modelImageBGR;
2885
2886 numericUpDownLeftModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);
2887
2888 featureDetectorLeft = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageLeft.Image.Bitmap), (Byte)numericUpDownLeftModelReferenceHue.Value);
2889 }
2890
2891 //Save settings
2892 WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath = ofDialog.FileName;
2893 WheelchairControl.Properties.Settings.Default.Save();
2894 }
2895 catch (Exception)
2896 {
2897 //Empty image
2898 using (Image<Bgr, Byte> errorImage = new Image<Bgr, Byte>(ibModelImageLeft.Size))
2899 {
2900 ibModelImageLeft.Image = errorImage;
2901 numericUpDownLeftModelReferenceHue.Value = (Byte)numericUpDownLeftModelReferenceHue.Minimum;
2902 featureDetectorLeft = new FeatureDetector(errorImage, (Byte)numericUpDownLeftModelReferenceHue.Value);
2903 }
2904 }
2905 }
2906 }
2907
2908 private void buttonLoadRightModelImage_Click(object sender, EventArgs e)
2909 {
2910 OpenFileDialog ofDialog = new OpenFileDialog();
2911
2912 try
2913 {
2914 ofDialog.InitialDirectory = @WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath;
2915 }
2916 catch (Exception)
2917 { ofDialog.InitialDirectory = Environment.CurrentDirectory; }
2918
2919 if (ofDialog.ShowDialog() == DialogResult.OK)
2920 {
2921 try
2922 {
2923 //Load image
2924 using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath))
2925 using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
2926 {
2927 ibModelImageRight.Image = modelImageBGR;
2928
2929 numericUpDownRightModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);
2930
2931 featureDetectorRight = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageRight.Image.Bitmap), (Byte)numericUpDownRightModelReferenceHue.Value);
2932 }
2933
2934 //Save settings
2935 WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath = ofDialog.FileName;
2936 WheelchairControl.Properties.Settings.Default.Save();
2937 }
2938 catch (Exception)
2939 {
2940 //Empty image
2941 using (Image<Bgr, Byte> errorImage = new Image<Bgr, Byte>(ibModelImageRight.Size))
2942 {
2943 ibModelImageRight.Image = errorImage;
2944 numericUpDownRightModelReferenceHue.Value = (Byte)numericUpDownRightModelReferenceHue.Minimum;
2945 featureDetectorRight = new FeatureDetector(errorImage, (Byte)numericUpDownRightModelReferenceHue.Value);
2946 }
2947 }
2948 }
2949 }
2950
2951 private void toolStripMenuItemRed_Click(object sender, EventArgs e)
2952 {
2953 //Determine which numericUpDown sent the menu request and change its value
2954 try
2955 {
2956 ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
2957 ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
2958 NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;
2959
2960 nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Red;
2961 }
2962 catch (NullReferenceException)
2963 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
2964 }
2965
2966 private void toolStripMenuItemGreen_Click(object sender, EventArgs e)
2967 {
2968 //Determine which numericUpDown sent the menu request and change its value
2969 try
2970 {
2971 ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
2972 ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
2973 NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;
2974
2975 nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Green;
2976 }
2977 catch (NullReferenceException)
2978 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
2979 }
2980
2981 private void toolStripMenuItemBlue_Click(object sender, EventArgs e)
2982 {
2983 //Determine which numericUpDown sent the menu request and change its value
2984 try
2985 {
2986 ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
2987 ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
2988 NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;
2989
2990 nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Blue;
2991 }
2992 catch (NullReferenceException)
2993 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
2994 }
2995
2996 private void toolStripMenuItemCyan_Click(object sender, EventArgs e)
2997 {
2998 //Determine which numericUpDown sent the menu request and change its value
2999 try
3000 {
3001 ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
3002 ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
3003 NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;
3004
3005 nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Cyan;
3006 }
3007 catch (NullReferenceException)
3008 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
3009 }
3010
3011 private void toolStripMenuItemMagenta_Click(object sender, EventArgs e)
3012 {
3013 //Determine which numericUpDown sent the menu request and change its value
3014 try
3015 {
3016 ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
3017 ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
3018 NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;
3019
3020 nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Magenta;
3021 }
3022 catch (NullReferenceException)
3023 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
3024 }
3025
3026 private void toolStripMenuItemYellow_Click(object sender, EventArgs e)
3027 {
3028 //Determine which numericUpDown sent the menu request and change its value
3029 try
3030 {
3031 ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
3032 ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
3033 NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;
3034
3035 nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Yellow;
3036 }
3037 catch (NullReferenceException)
3038 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
3039 }
3040
3041 private void numericUpDownLeftModelReferenceHue_ValueChanged(object sender, EventArgs e)
3042 {
3043 //Try to create new feature detector
3044 try
3045 {
3046 using (Bitmap ibImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath))
3047 featureDetectorLeft = new FeatureDetector(new Image<Bgr, Byte>(ibImage), (Byte)numericUpDownLeftModelReferenceHue.Value);
3048 }
3049 catch (Exception)
3050 {
3051 //If feature detector was damaged, inform user
3052 if (featureDetectorLeft == null)
3053 MessageBox.Show("Feature detector error, please reload the model image or restart the application!", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
3054 }
3055 }
3056
3057 private void numericUpDownRightModelReferenceHue_ValueChanged(object sender, EventArgs e)
3058 {
3059 //Try to create new feature detector
3060 try
3061 {
3062 using (Bitmap ibImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath))
3063 featureDetectorRight = new FeatureDetector(new Image<Bgr, Byte>(ibImage), (Byte)numericUpDownRightModelReferenceHue.Value);
3064 }
3065 catch (Exception)
3066 {
3067 //If feature detector was damaged, inform user
3068 if (featureDetectorRight == null)
3069 MessageBox.Show("Feature detector error, please reload the model image or restart the application!", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
3070 }
3071 }
3072
3073 private void numericUpDownWorldMapScale_ValueChanged(object sender, EventArgs e)
3074 {
3075 //Change range
3076 tbWorldMapScale.Maximum = (Int32)numericUpDownWorldMapScale.Value;
3077
3078 //Save values
3079 WheelchairControl.Properties.Settings.Default.Setting_WorldMapMaxScale = (Byte)numericUpDownWorldMapScale.Value;
3080 WheelchairControl.Properties.Settings.Default.Save();
3081 }
3082
3083 private void comboBoxRenderingMode_SelectedIndexChanged(object sender, EventArgs e)
3084 {
3085 //Save settings
3086 WheelchairControl.Properties.Settings.Default.Setting_WorldMapRenderingMode = (Byte)comboBoxRenderingMode.SelectedIndex;
3087 WheelchairControl.Properties.Settings.Default.Save();
3088 }
3089
3090 private void checkBoxWatchdogResponseEnabled_CheckedChanged(object sender, EventArgs e)
3091 {
3092 //Save settings
3093 WheelchairControl.Properties.Settings.Default.Setting_WatchdogLoopbackEnabled = checkBoxWatchdogResponseEnabled.Checked;
3094 WheelchairControl.Properties.Settings.Default.Save();
3095 }
3096
3097 private void numericUpDownJpegCompression_ValueChanged(object sender, EventArgs e)
3098 {
3099 //Save settings
3100 WheelchairControl.Properties.Settings.Default.Setting_JpegCompression = (Byte)numericUpDownJpegCompression.Value;
3101 WheelchairControl.Properties.Settings.Default.Save();
3102 }
3103
3104 private void toolStripMenuItemCalibPointDone_Click(object sender, EventArgs e)
3105 {
3106 //Determine which radiobutton sent the menu request and change its value
3107 try
3108 {
3109 ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
3110 ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
3111 RadioButton rbSender = cmStrip.SourceControl as RadioButton;
3112
3113 //Convert values
3114 String[] splittedValues = toolStripTextBoxPoint.Text.Split(';');
3115
3116 float xCoord = float.Parse(splittedValues[0]);
3117 float yCoord = float.Parse(splittedValues[1]);
3118
3119 //Update proper value
3120 switch (rbSender.Name)
3121 {
3122 default:
3123 //Error
3124 return;
3125
3126 case "rbUL":
3127
3128 CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_LEFT]
3129 = new PointF(xCoord, yCoord);
3130
3131 break;
3132
3133 case "rbUR":
3134
3135 CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_RIGHT]
3136 = new PointF(xCoord, yCoord);
3137
3138
3139 break;
3140
3141 case "rbLR":
3142
3143 CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_RIGHT]
3144 = new PointF(xCoord, yCoord);
3145
3146 break;
3147
3148 case "rbLL":
3149
3150 CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_LEFT]
3151 = new PointF(xCoord, yCoord);
3152
3153 break;
3154 }
3155
3156 //Finally reload coordinates
3157 LoadCalibrationCoordinates();
3158 }
3159 catch (FormatException) { /*Conversion failed, do not update*/}
3160 catch (OverflowException) { /*Conversion failed, do not update*/}
3161 catch (NullReferenceException)
3162 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
3163 }
3164
3165 private void cmCalibrationCoordinates_Opened(object sender, EventArgs e)
3166 {
3167 //Determine which radiobutton sent the menu request and change its value
3168 try
3169 {
3170 ContextMenuStrip cmContainer = sender as ContextMenuStrip;
3171 RadioButton rbSender = cmContainer.SourceControl as RadioButton;
3172
3173 //Update with proper value
3174 switch (rbSender.Name)
3175 {
3176 default:
3177 //Error
3178 return;
3179
3180 case "rbUL":
3181
3182 toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_LEFT].X.ToString()
3183 + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_LEFT].Y.ToString();
3184
3185 toolStripMenuItemTitleCalibPoints.Text = "Modify upper left calibration point";
3186
3187 break;
3188
3189 case "rbUR":
3190
3191 toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_RIGHT].X.ToString()
3192 + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_RIGHT].Y.ToString();
3193
3194 toolStripMenuItemTitleCalibPoints.Text = "Modify upper right calibration point";
3195
3196 break;
3197
3198 case "rbLR":
3199
3200 toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_RIGHT].X.ToString()
3201 + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_RIGHT].Y.ToString();
3202
3203 toolStripMenuItemTitleCalibPoints.Text = "Modify lower right calibration point";
3204
3205 break;
3206
3207 case "rbLL":
3208
3209 toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_LEFT].X.ToString()
3210 + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_LEFT].Y.ToString();
3211
3212 toolStripMenuItemTitleCalibPoints.Text = "Modify lower left calibration point";
3213
3214 break;
3215 }
3216
3217 //Redraw menu
3218 cmCalibrationCoordinates.Refresh();
3219 }
3220 catch (NullReferenceException)
3221 { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
3222 catch (Exception)
3223 {
3224 //Use default text
3225 toolStripTextBoxPoint.Text = "Enter X [m]; Y [m]:";
3226 }
3227 }
3228
3229 private void tbWorldMapScale_ValueChanged(object sender, EventArgs e)
3230 {
3231 //Save settings
3232 WheelchairControl.Properties.Settings.Default.Setting_WorldMapScale = (Byte)tbWorldMapScale.Value;
3233 WheelchairControl.Properties.Settings.Default.Save();
3234 }
3235
3236 private void checkBoxDrawWheelchair_CheckedChanged(object sender, EventArgs e)
3237 {
3238 //Save settings
3239 WheelchairControl.Properties.Settings.Default.Setting_DrawWheelchair = checkBoxDrawWheelchair.Checked;
3240 WheelchairControl.Properties.Settings.Default.Save();
3241 }
3242
3243 private void tabControlJoyAutoParking_SelectedIndexChanged(object sender, EventArgs e)
3244 {
3245 //Check proper toolStripbutton
3246 switch (tabControlJoyAutoParking.SelectedIndex)
3247 {
3248 default:
3249 case (Byte)JoyAutoParkingSelesctionIndex.JOYSTICK:
3250
3251 toolStripButtonJoystick.Checked = true;
3252 toolStripButtonAutoParking.Checked = false;
3253
3254 break;
3255
3256 case (Byte)JoyAutoParkingSelesctionIndex.AUTOPARKING:
3257
3258 toolStripButtonAutoParking.Checked = true;
3259 toolStripButtonJoystick.Checked = false;
3260
3261 break;
3262 }
3263 }
3264
3265 private void toolStripButtonJoystick_Click(object sender, EventArgs e)
3266 {
3267 //Choose joystick tab
3268 tabControlJoyAutoParking.SelectedIndex = (Byte)JoyAutoParkingSelesctionIndex.JOYSTICK;
3269 }
3270
3271 private void toolStripButtonAutoParking_Click(object sender, EventArgs e)
3272 {
3273 //Choose autoparking tab
3274 tabControlJoyAutoParking.SelectedIndex = (Byte)JoyAutoParkingSelesctionIndex.AUTOPARKING;
3275 }
3276
3277 private void buttonSTARTAutoParking_Click(object sender, EventArgs e)
3278 {
3279 //Start logging
3280 logWheelchairRunDataToFileToolStripMenuItem_Click(sender, e);
3281
3282 //Activate thread
3283 AutoParkingThread = new Thread(AutoParkingThreadMethod);
3284 AutoParkingThread.IsBackground = true;
3285 AutoParkingThread.Priority = ThreadPriority.Lowest;
3286 AutoParkingThread.Start();
3287 }
3288
3289 private void checkBoxUserResponseDemanded_CheckedChanged(object sender, EventArgs e)
3290 {
3291 //Save settings
3292 WheelchairControl.Properties.Settings.Default.Setting_ConfirmMarksDetection = checkBoxUserResponseDemanded.Checked;
3293 WheelchairControl.Properties.Settings.Default.Save();
3294 }
3295
3296 #endregion
3297 }
3298 }