toan / Robofork

Download as
Browse Code ยป

Commit 57fdd8bb89a2ef94ba5b32f1de5d16a8639413e9

Authored by toan
1 parent 1fe3e8a873
Exists in master

RM2063 : Commit demo

Showing 3 changed files with 62 additions and 18 deletions Side-by-side Diff

releases/RM2063/20170319_RM2063.rar
Diff comments   View file @ 57fdd8b
No preview for this file type
sources/RoboforkApp/Controls/DesignerCanvas.cs
Diff comments   View file @ 57fdd8b
... ... @@ -3027,7 +3027,7 @@
3027 3027 PointMap pMap = new PointMap();
3028 3028 pMap.pointMap_X = node.X;
3029 3029 pMap.pointMap_Y = node.Y;
3030   - pMap.speed_Map = 5;
  3030 + pMap.speed_Map = 1;
3031 3031  
3032 3032 //Backup List Node Insert of fork
3033 3033 if (IndexNodeInsert_List.Count> 0)
... ... @@ -3080,7 +3080,7 @@
3080 3080 PointMap pMap = new PointMap();
3081 3081 pMap.pointMap_X = node.X;
3082 3082 pMap.pointMap_Y = node.Y;
3083   - pMap.speed_Map = 5;
  3083 + pMap.speed_Map = 1;
3084 3084  
3085 3085 if (IndexNodeInsert_List.Count > 0)
3086 3086 {
sources/RoboforkApp/Controls/ScheduleCanvas.cs
Diff comments   View file @ 57fdd8b
... ... @@ -18,11 +18,11 @@
18 18 const double RADIUS_NODE = 25;
19 19 public simulationRobo simulation;
20 20 private VehicleModelList vehicleModelList;
21   - private Point startPoint;
22   - private Point endPoint;
  21 + private Point _startPoint;
  22 + private Point _endPoint;
23 23 private bool _isGoal = false;
24 24 private double _speed;
25   - private int index;
  25 + private int _index;
26 26  
27 27 private List<ucNode> _lstNode;
28 28  
... ... @@ -45,11 +45,18 @@
45 45 //Init data
46 46 this._lstNode = lstNode;
47 47 this.vehicleModelList = vehicleModel.VehicleModelList[vehicleIndex];
48   - this.startPoint = new Point(Canvas.GetLeft(_lstNode[index]) + RADIUS_NODE, COOR_Y);
49   - this.endPoint = new Point(Canvas.GetLeft(_lstNode[index + 1]) + RADIUS_NODE, COOR_Y);
50   - this._speed = vehicleModelList.pointMapList[index].speed_Schedule;
51   - this.index += 1;
52   - if (index == _lstNode.Count - 1)
  48 + this._startPoint = new Point(Canvas.GetLeft(_lstNode[_index]) + RADIUS_NODE, COOR_Y);
  49 + this._endPoint = new Point(Canvas.GetLeft(_lstNode[_index + 1]) + RADIUS_NODE, COOR_Y);
  50 +
  51 + //Get speed
  52 + double scheDis = GetDistance(_startPoint, _endPoint);
  53 + double mapDis = GetDistance(new Point(vehicleModelList.pointMapList[_index].pointMap_X, vehicleModelList.pointMapList[_index].pointMap_Y),
  54 + new Point(vehicleModelList.pointMapList[_index + 1].pointMap_X, vehicleModelList.pointMapList[_index + 1].pointMap_Y + 1));
  55 + this._speed = GetSpeed(mapDis, scheDis, vehicleModelList.pointMapList[_index].speed_Map);
  56 +
  57 + //Check next node is goal
  58 + this._index += 1;
  59 + if (_index == _lstNode.Count - 1)
53 60 {
54 61 _isGoal = true;
55 62 }
... ... @@ -64,7 +71,7 @@
64 71 private void RoboSimulation()
65 72 {
66 73 simulation = new simulationRobo();
67   - simulation.storyBoard = CreatPathAnimation(startPoint, endPoint, _speed); //pathAnimationStoryboard;
  74 + simulation.storyBoard = CreatPathAnimation(_startPoint, _endPoint, _speed);
68 75 this.Children.Add(simulation);
69 76 }
70 77  
71 78  
... ... @@ -119,13 +126,19 @@
119 126 // If not end node
120 127 if (!isGoal)
121 128 {
122   - this._speed = vehicleModelList.pointMapList[index].speed_Schedule;
123   - this.index += 1;
124 129 this.Children.Remove(simulation);
125   - this.startPoint = endPoint;
126   - this.endPoint = new Point(Canvas.GetLeft(_lstNode[index]) + RADIUS_NODE, COOR_Y);
127   -
128   - if (this.index == this._lstNode.Count - 1)
  130 + this._startPoint = _endPoint;
  131 + this._endPoint = new Point(Canvas.GetLeft(_lstNode[_index + 1]) + RADIUS_NODE, COOR_Y);
  132 +
  133 + //Get speed
  134 + double scheDis = GetDistance(_startPoint, _endPoint);
  135 + double mapDis = GetDistance(new Point(vehicleModelList.pointMapList[_index].pointMap_X, vehicleModelList.pointMapList[_index].pointMap_Y),
  136 + new Point(vehicleModelList.pointMapList[_index + 1].pointMap_X, vehicleModelList.pointMapList[_index + 1].pointMap_Y + 1));
  137 + this._speed = GetSpeed(mapDis, scheDis, vehicleModelList.pointMapList[_index].speed_Map);
  138 +
  139 + //Check next node is goal
  140 + this._index += 1;
  141 + if (this._index == this._lstNode.Count - 1)
129 142 {
130 143 this._isGoal = true;
131 144 }
132 145  
... ... @@ -134,9 +147,40 @@
134 147 }
135 148  
136 149 // Reset data when finish
137   - this.index = 0;
  150 + this._index = 0;
138 151 this._speed = 0;
139 152 this._isGoal = false;
  153 + }
  154 +
  155 + /// <summary>
  156 + /// Get speed on schedule
  157 + /// </summary>
  158 + /// <param name="mapDis">Distance the line on Map</param>
  159 + /// <param name="scheDis">Distance the line on Schedule</param>
  160 + /// <param name="mapSpeed">Speed the fork on Map</param>
  161 + /// <returns>Speed the fork on schedule</returns>
  162 + private double GetSpeed(double mapDis, double scheDis, double mapSpeed)
  163 + {
  164 + if (mapDis == 0)
  165 + return 0.0;
  166 +
  167 + return mapSpeed * (scheDis / mapDis);
  168 + }
  169 +
  170 + /// <summary>
  171 + /// Get distance between two point
  172 + /// </summary>
  173 + /// <param name="point1">Point 1</param>
  174 + /// <param name="point2">Point 2</param>
  175 + /// <returns>Distance between two point</returns>
  176 + private double GetDistance(Point point1, Point point2)
  177 + {
  178 + //pythagorean theorem c^2 = a^2 + b^2
  179 + //thus c = square root(a^2 + b^2)
  180 + double a = (double)(point2.X - point1.X);
  181 + double b = (double)(point2.Y - point1.Y);
  182 +
  183 + return Math.Sqrt(a * a + b * b);
140 184 }
141 185 }
142 186 }