| 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
WorldMap.cs
1 ﻿using System;
2 using System.Collections.Generic;
3 using System.Linq;
4 using System.Text;
5 using System.Drawing;
6
7 using MyImageProcessingClasses;
8
9 namespace World
10 {
14 public static class Car
15 {
19 public const float CAR_LENGTH = 3.5f;
20
24 public const Int32 DRIVER_TO_REAR_DISTANCE_CM = 250;
25
29 public const float PLATFORM_LENGTH = 1.0f;
30
34 public static readonly Color CAR_COLOR = Color.FromArgb(128, Color.Red);
35
39 public static readonly Color PLATFORM_COLOR = Color.FromArgb(128, Color.LightBlue);
40
44 public static PointF[] CarRectangle = new PointF[4];
45
49 public static PointF[] PlatformRectangle = new PointF[4];
50 }
51
55 public class TrackPoint
56 {
60 private PointF coordinates = new PointF();
61
65 public PointF Coordinates
66 {
67 get
68 {
69 return coordinates;
70 }
71
72 set
73 {
74 this.coordinates = value;
75 }
76 }
77
81 public TrackPoint()
82 { }
83
88 public TrackPoint(PointF coordinatesIn)
89 {
90 coordinates = coordinatesIn;
91 }
92 }
93
97 public class Mark
98 {
102 private PointF coordinates = new PointF();
103
107 private WorldMap.MarkPosition position;
108
112 private Color markColor = new Color();
113
117 public WorldMap.MarkPosition Position
118 {
119 get
120 {
121 return position;
122 }
123
124 set
125
126 {
127 position = value;
128 }
129 }
130
134 public PointF Coordinates
135 {
136 get
137 {
138 return coordinates;
139 }
140
141 set
142 {
143 this.coordinates = value;
144 }
145 }
146
150 public Color MarkColor
151 {
152 get
153 {
154 return markColor;
155 }
156
157 set
158 {
159 this.markColor = value;
160 }
161 }
162
166 public Mark()
167 { }
168
175 public Mark(PointF coordinatesIn, Color colorIn, WorldMap.MarkPosition positionIn)
176 {
177 coordinates = coordinatesIn;
178 markColor = colorIn;
179 position = positionIn;
180 }
181 }
182
190 [System.Runtime.Remoting.Contexts.Synchronization]
191 public class WheelchairModel : ContextBoundObject
192 {
193 #region Constants
194
195
196
197 public static readonly List<Brush> WHEELCHAIR_COLOR = new List<Brush>() { new SolidBrush(Color.Black), new SolidBrush(Color.Red) };
198
202 public const Int32 USER_RESPONSE_TIMEOUT_MS = 30000; //30 seconds
203
207 public const Int16 VDES = -40;
208
212 public const Byte Y0DES = 70;
213
217 public const Byte TARGET_COORD_ALLOWED_ERROR_CM = 10;
218
222 public const UInt16 MARK_DETECT_ROTATION_ANGLE_MRAD = 523; //523 mrad = 30°
223
227 public enum UserResponseCode : byte
228 {
232 END = 0,
233
237 LOOK_FURTHER,
238
242 MARKS_FOUND
243 }
244
248 public enum AutoParkingPhase : byte
249 {
253 WAITING_FOR_START = 0,
254
258 WALL_FOLLOWING,
259
263 GOING_BACKWARDS,
264
268 MARKS_TRACKING,
269
273 PATH_FOLLOWING,
274
278 END
279 }
280
284 public enum PhaseError : byte
285 {
289 ERROR = 0,
290
294 NO_ERROR
295 }
296
300 private const Int32 WHEELCHAIR_SIDE_SIZE_MM = 600;
301
302 #endregion
303
304 #region Private variables
305
309 private UserResponseCode userResponse = UserResponseCode.END;
310
314 private Boolean regulationInProgress = false;
315
319 private AutoParkingPhase currentPhase = AutoParkingPhase.WAITING_FOR_START;
320
324 private Int32 xCoordinate = 0;
325
329 private Int32 yCoordinate = 0;
330
334 private Int32 phi = 0;
335
336 #endregion
337
338 #region Public variables and properties
339
343 public Int32 X
344 { get { return xCoordinate; } }
345
349 public Int32 Y
350 { get { return yCoordinate; } }
351
355 public Int32 Phi
356 { get { return phi; } }
357
361 public AutoParkingPhase CurrentPhase
362 {
363 get { return currentPhase; }
364 set { currentPhase = value; }
365 }
366
370 public Boolean IsRegulationActive
371 {
372 get { return regulationInProgress; }
373 set { regulationInProgress = value; }
374 }
375
380 public UserResponseCode UserResponse
381 {
382 get { return userResponse; }
383 set { userResponse = value; }
384 }
385
386 #endregion
387
391 public WheelchairModel()
392 { /*Default constructor, private variables are initialized in declaration*/}
393
398 public PointF[] GetSurroundingRectangle()
399 {
400 List<PointF> tempList = new List<PointF>();
401 Int32 wheelchairDiagonalHalf = (Int32)(WHEELCHAIR_SIDE_SIZE_MM * Math.Sqrt(2.0) * 0.5);
402 Double piQuart = Math.PI / 4.0;
403
404 //Add upper left point
405 tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 - piQuart) * wheelchairDiagonalHalf),
406 yCoordinate + (Int32)(Math.Cos(phi / 1000.0 - piQuart) * wheelchairDiagonalHalf)));
407
408 //Add upper right point
409 tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 + piQuart) * wheelchairDiagonalHalf),
410 yCoordinate + (Int32)(Math.Cos(phi / 1000.0 + piQuart) * wheelchairDiagonalHalf)));
411
412 //Add lower right point
413 tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 + (3 * piQuart)) * wheelchairDiagonalHalf),
414 yCoordinate + (Int32)(Math.Cos(phi / 1000.0 + ( 3 * piQuart)) * wheelchairDiagonalHalf)));
415
416 //Add lower left point
417 tempList.Add(new PointF(xCoordinate + (Int32)(Math.Sin(phi / 1000.0 + (5 * piQuart)) * wheelchairDiagonalHalf),
418 yCoordinate + (Int32)(Math.Cos(phi / 1000.0 + (5 * piQuart)) * wheelchairDiagonalHalf)));
419
420 return tempList.ToArray();
421 }
422
428 public PointF[] GetDefaultSurroundingRectangle()
429 {
430 List<PointF> tempList = new List<PointF>();
431 Int32 wheelchairDiagonalHalf = (Int32)(WHEELCHAIR_SIDE_SIZE_MM * Math.Sqrt(2.0) * 0.5);
432 Double piQuart = Math.PI / 4.0;
433
434 //Add upper left point
435 tempList.Add(new PointF((Int32)(Math.Sin(-piQuart) * wheelchairDiagonalHalf),
436 (Int32)(Math.Cos(-piQuart) * wheelchairDiagonalHalf)));
437
438 //Add upper right point
439 tempList.Add(new PointF((Int32)(Math.Sin( piQuart) * wheelchairDiagonalHalf),
440 (Int32)(Math.Cos(piQuart) * wheelchairDiagonalHalf)));
441
442 //Add lower right point
443 tempList.Add(new PointF((Int32)(Math.Sin((3 * piQuart)) * wheelchairDiagonalHalf),
444 (Int32)(Math.Cos((3 * piQuart)) * wheelchairDiagonalHalf)));
445
446 //Add lower left point
447 tempList.Add(new PointF((Int32)(Math.Sin((5 * piQuart)) * wheelchairDiagonalHalf),
448 (Int32)(Math.Cos((5 * piQuart)) * wheelchairDiagonalHalf)));
449
450 return tempList.ToArray();
451 }
452
459 public void SetCoordinates(Int32 X, Int32 Y, Int32 Phi)
460 {
461 xCoordinate = X;
462 yCoordinate = Y;
463 phi = Phi;
464 }
465 }
466
470 public class WorldMap
471 {
475 public enum MarkPosition : byte
476 {
480 LEFT = 0,
481
485 RIGHT = 1,
486
490 MARKS_COUNT
491 };
492
496 private List<Mark> marks;
497
501 private List<TrackPoint> trackPoints;
502
506 public List<Mark> Marks
507 {
508 get
509 {
510 return marks;
511 }
512 }
513
517 public List<TrackPoint> TrackPoints
518 {
519 get
520 {
521 return trackPoints;
522 }
523 }
524
528 public WorldMap()
529 {
530 //Marks
531 marks = new List<Mark>((Int32)MarkPosition.MARKS_COUNT);
532
533 for (Byte i = 0; i < (Byte)MarkPosition.MARKS_COUNT; i++)
534 marks.Add(new Mark());
535
536 //Track points
537 trackPoints = new List<TrackPoint>();
538 }
539
546 public void ModifyMark(MarkPosition position, PointF coordinates, Color color)
547 {
548 //Modify marks coordinates
549 marks[(Int32)position].Coordinates = coordinates;
550 marks[(Int32)position].MarkColor = color;
551
552 //Recalculate environment coordinates
553 RecalculateTrack();
554 RecalculateCarRectangle();
555 RecalculatePlatformRectangle();
556 }
557
561 private void RecalculateTrack()
562 {
563 //Car angle, width and marks' centre
564 Double angle = Math.Atan2((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y),
565 (marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X));
566
567 Double width = Math.Sqrt((Math.Pow((marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X), 2.0))
568 + (Math.Pow((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y), 2.0)));
569
570 PointF centre = new PointF((marks[(Int32)MarkPosition.LEFT].Coordinates.X + marks[(Int32)MarkPosition.RIGHT].Coordinates.X) / 2.0f,
571 (marks[(Int32)MarkPosition.LEFT].Coordinates.Y + marks[(Int32)MarkPosition.RIGHT].Coordinates.Y) / 2.0f);
572
573 trackPoints.Clear();
574
575 trackPoints.Add(new TrackPoint(new PointF((float)(centre.X - Math.Sin(angle) * Car.PLATFORM_LENGTH / 2.0),
576 (float)(centre.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH / 2.0))));
577
578 trackPoints.Add(new TrackPoint(new PointF((float)(centre.X - Math.Sin(angle) * Car.PLATFORM_LENGTH * 1.5),
579 (float)(centre.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH * 1.5))));
580
581 trackPoints.Add(new TrackPoint(new PointF((float)(trackPoints[1].Coordinates.X + Math.Cos(angle) * width),
582 (float)(trackPoints[1].Coordinates.Y + Math.Sin(angle) * width))));
583 }
584
588 private void RecalculateCarRectangle()
589 {
590 //Car angle
591 Double angle = Math.Atan2((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y),
592 (marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X));
593
594 //Rectangle corners
595 Car.CarRectangle[3] = new PointF(marks[(Int32)MarkPosition.RIGHT].Coordinates.X, marks[(Int32)MarkPosition.RIGHT].Coordinates.Y);
596 Car.CarRectangle[2] = new PointF(marks[(Int32)MarkPosition.LEFT].Coordinates.X, marks[(Int32)MarkPosition.LEFT].Coordinates.Y);
597
598 Car.CarRectangle[1] = new PointF((float)(marks[(Int32)MarkPosition.LEFT].Coordinates.X + Math.Sin(angle) * Car.CAR_LENGTH),
599 (float)(marks[(Int32)MarkPosition.LEFT].Coordinates.Y - Math.Cos(angle) * Car.CAR_LENGTH));
600
601 Car.CarRectangle[0] = new PointF((float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.X + Math.Sin(angle) * Car.CAR_LENGTH),
602 (float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.Y - Math.Cos(angle) * Car.CAR_LENGTH));
603 }
604
608 private void RecalculatePlatformRectangle()
609 {
610 //Car angle
611 Double angle = Math.Atan2((marks[(Int32)MarkPosition.LEFT].Coordinates.Y - marks[(Int32)MarkPosition.RIGHT].Coordinates.Y),
612 (marks[(Int32)MarkPosition.LEFT].Coordinates.X - marks[(Int32)MarkPosition.RIGHT].Coordinates.X));
613
614 //Rectangle corners
615 Car.PlatformRectangle[0] = new PointF(marks[(Int32)MarkPosition.RIGHT].Coordinates.X, marks[(Int32)MarkPosition.RIGHT].Coordinates.Y);
616 Car.PlatformRectangle[1] = new PointF(marks[(Int32)MarkPosition.LEFT].Coordinates.X, marks[(Int32)MarkPosition.LEFT].Coordinates.Y);
617
618 Car.PlatformRectangle[2] = new PointF((float)(marks[(Int32)MarkPosition.LEFT].Coordinates.X - Math.Sin(angle) * Car.PLATFORM_LENGTH),
619 (float)(marks[(Int32)MarkPosition.LEFT].Coordinates.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH));
620
621 Car.PlatformRectangle[3] = new PointF((float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.X - Math.Sin(angle) * Car.PLATFORM_LENGTH),
622 (float)(marks[(Int32)MarkPosition.RIGHT].Coordinates.Y + Math.Cos(angle) * Car.PLATFORM_LENGTH));
623 }
624 }
625 }