This commit is contained in:
chi
2025-06-30 13:48:19 +09:00
parent 02e71d7446
commit 00dd50192b
29 changed files with 2397 additions and 1136 deletions

View File

@@ -1,248 +1,242 @@
using System;
using System.Drawing;
using System.Collections.Generic;
using System.Linq;
using System.Security.Permissions;
using System.Windows.Forms;
namespace AGVControl.Models
{
public enum Direction
{
Forward = 0,
Backward = 1,
Stop = 2
}
//public class CRFIDData
//{
// public UInt16 Value { get; set; }
// public Point Location { get; set; }
// public override string ToString()
// {
// return $"RFID:{Value},P:{Location.X},{Location.Y}";
// }
//}
public class movehistorydata : RFIDPoint
{
public Direction Direction { get; set; }
public override string ToString()
{
return $"RFID:{Value},DIR:{Direction},P:{Location.X},{Location.Y}";
}
}
public class AGV
{
/// <summary>
/// RFID 번호
/// </summary>
public RFIDPoint CurrentRFID { get; set; }
/// <summary>
/// 목적지가 셋팅된경우 해당 값
/// </summary>
public RFIDPoint TargetRFID { get; set; }
/// <summary>
/// 배터리잔량(%)
/// </summary>
public float BatteryLevel { get; set; } = 0f;
/// <summary>
/// 배터리온도(board)
/// </summary>
public double BatteryTemp1 { get; set; } = 0;
/// <summary>
/// 배터리온도(cell)
/// </summary>
public double BatteryTemp2 { get; set; } = 0;
/// <summary>
/// AGV
/// </summary>
public Direction CurrentAGVDirection { get; set; }
/// <summary>
/// AGV모터 방향
/// 외부에서 값이 상시 업데이트 됩니다.
/// </summary>
public Direction CurrentMOTDirection { get; set; }
/// <summary>
/// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다.
/// </summary>
public Direction TargetDirection { get; set; } = Direction.Stop;
public bool IsMoving { get; set; }
public bool IsMarkCheck { get; set; }
public bool IsTargetDirectionMatch { get; set; }
/// <summary>
/// 메인경로
/// 경로검색으로 입력된 경로
/// </summary>
public List<RFIDPoint> MainPath { get; set; } = new List<RFIDPoint>();
/// <summary>
/// 메인경로외에 거쳐가는 중간 경로
/// </summary>
public List<RFIDPoint> SubPath { get; set; }
public List<string> PathRFIDs { get; set; }
// 이동 경로 기록을 위한 새로운 속성들
public List<movehistorydata> MovementHistory { get; } = new List<movehistorydata>();
public const int HISTORY_SIZE = 10; // 최근 4개 위치 기록
public AGV()
{
MainPath = new List<RFIDPoint>();
SubPath = new List<RFIDPoint>();
PathRFIDs = new List<string>();
CurrentRFID = new RFIDPoint();
TargetRFID = new RFIDPoint();
TargetDirection = Direction.Forward;
// BodyAngle = null;
}
// 이동 경로에 새로운 RFID 추가
public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
{
// 중복 RFID가 연속으로 들어오는 경우 무시
if (MovementHistory.Count > 0 && MovementHistory.Last().Value == rfidValue)
return;
MovementHistory.Add(new movehistorydata { Value = rfidValue, Direction = direction, Location = position });
// 기록 크기 제한
if (MovementHistory.Count > HISTORY_SIZE)
{
MovementHistory.RemoveAt(0);
}
//최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다.
if (MovementHistory.Count > 2 && MovementHistory.First().Direction != MovementHistory.Last().Direction)
{
var lastTwo = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToArray(); // [9, 10]
MovementHistory.Clear();
MovementHistory.AddRange(lastTwo);
}
}
// 연결 정보 기반 실제 이동 방향 계산
public Direction? CalculateActualDirectionByConnection(uint currentRFID, uint previousRFID, List<RFIDConnection> connections)
{
if (connections == null || connections.Count == 0)
return null;
// 이전 RFID에서 현재 RFID로의 연결 확인
var connection = connections.FirstOrDefault(c =>
(c.P1.Value == previousRFID && c.P2.Value == currentRFID) ||
(c.P1.Value == currentRFID && c.P2.Value == previousRFID));
if (connection == null)
return null; // 연결되지 않은 경로
// 연결 방향에 따라 실제 이동 방향 결정
if (connection.P1.Value == previousRFID && connection.P2.Value == currentRFID)
{
return Direction.Forward; // Start -> End 방향으로 이동
}
else
{
return Direction.Backward; // End -> Start 방향으로 이동
}
}
// 연결 정보 기반 방향 불일치 검증 및 정정
public bool ValidateAndCorrectDirectionByConnection(Direction expectedDirection, List<RFIDConnection> connections)
{
if (MovementHistory.Count < 2 || connections == null)
return true; // 검증 불가능한 경우
// 최근 두 RFID 값 가져오기
var recentRFIDs = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToList();
if (recentRFIDs.Count < 2)
return true;
var previousRFID = recentRFIDs[0];
var currentRFID = recentRFIDs[1];
var actualDirection = CalculateActualDirectionByConnection(currentRFID.Value, previousRFID.Value, connections);
if (!actualDirection.HasValue)
return true; // 연결 정보로 방향 판단 불가
// 방향이 일치하지 않는 경우
if (actualDirection.Value != expectedDirection)
{
// AGV 모터 방향을 실제 이동 방향으로 정정
CurrentAGVDirection = actualDirection.Value;
TargetDirection = actualDirection.Value;
return false; // 정정됨을 알림
}
return true; // 방향 일치
}
// RFID 순서 기반 실제 이동 방향 계산 (기존 메서드 - 호환성 유지)
public Direction? CalculateActualDirectionByRFID()
{
if (MovementHistory.Count < 2)
return null;
// 최근 두 RFID 값으로부터 실제 이동 방향 계산
var recentRFIDs = MovementHistory.Skip(Math.Max(0, MovementHistory.Count - 2)).Take(2).ToList();
if (recentRFIDs.Count < 2)
return null;
var prevRFID = recentRFIDs[0];
var currentRFID = recentRFIDs[1];
// RFID 값의 증가/감소로 방향 판단
if (currentRFID.Value > prevRFID.Value)
{
return Direction.Forward; // RFID 값이 증가하면 전진
}
else if (currentRFID.Value < prevRFID.Value)
{
return Direction.Backward; // RFID 값이 감소하면 후진
}
else
{
return null; // 같은 RFID 값이면 방향 판단 불가
}
}
}
public class PathNode
{
public Point Location { get; set; }
public string RFID { get; set; }
public double G { get; set; } // 시작점에서 현재 노드까지의 비용
public double H { get; set; } // 현재 노드에서 목표점까지의 예상 비용
public double F => G + H; // 총 비용
public PathNode Parent { get; set; }
public PathNode(Point location, string rfid)
{
Location = location;
RFID = rfid;
G = 0;
H = 0;
Parent = null;
}
}
using System;
using System.Drawing;
using System.Collections.Generic;
using System.Linq;
using System.Security.Permissions;
using System.Windows.Forms;
namespace AGVControl.Models
{
//public class CRFIDData
//{
// public UInt16 Value { get; set; }
// public Point Location { get; set; }
// public override string ToString()
// {
// return $"RFID:{Value},P:{Location.X},{Location.Y}";
// }
//}
public class movehistorydata : RFIDPoint
{
public Direction Direction { get; set; }
public override string ToString()
{
return $"RFID:{Value},DIR:{Direction},P:{Location.X},{Location.Y}";
}
}
public class AGV
{
/// <summary>
/// RFID 번호
/// </summary>
public RFIDPoint CurrentRFID { get; set; }
/// <summary>
/// 목적지가 셋팅된경우 해당 값
/// </summary>
public RFIDPoint TargetRFID { get; set; }
/// <summary>
/// 배터리잔량(%)
/// </summary>
public float BatteryLevel { get; set; } = 0f;
/// <summary>
/// 배터리온도(board)
/// </summary>
public double BatteryTemp1 { get; set; } = 0;
/// <summary>
/// 배터리온도(cell)
/// </summary>
public double BatteryTemp2 { get; set; } = 0;
/// <summary>
/// AGV
/// </summary>
public Direction CurrentAGVDirection { get; set; }
/// <summary>
/// AGV모터 방향
/// 외부에서 값이 상시 업데이트 됩니다.
/// </summary>
public Direction CurrentMOTDirection { get; set; }
/// <summary>
/// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다.
/// </summary>
public Direction TargetDirection { get; set; } = Direction.Stop;
public bool IsMoving { get; set; }
public bool IsMarkCheck { get; set; }
public bool IsTargetDirectionMatch { get; set; }
/// <summary>
/// 메인경로
/// 경로검색으로 입력된 경로
/// </summary>
public List<RFIDPoint> MainPath { get; set; } = new List<RFIDPoint>();
/// <summary>
/// 메인경로외에 거쳐가는 중간 경로
/// </summary>
public List<RFIDPoint> SubPath { get; set; }
public List<string> PathRFIDs { get; set; }
// 이동 경로 기록을 위한 새로운 속성들
public List<movehistorydata> MovementHistory { get; } = new List<movehistorydata>();
public const int HISTORY_SIZE = 10; // 최근 4개 위치 기록
public AGV()
{
MainPath = new List<RFIDPoint>();
SubPath = new List<RFIDPoint>();
PathRFIDs = new List<string>();
CurrentRFID = new RFIDPoint();
TargetRFID = new RFIDPoint();
TargetDirection = Direction.Forward;
// BodyAngle = null;
}
// 이동 경로에 새로운 RFID 추가
public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
{
// 중복 RFID가 연속으로 들어오는 경우 무시
if (MovementHistory.Count > 0 && MovementHistory.Last().Value == rfidValue)
return;
MovementHistory.Add(new movehistorydata { Value = rfidValue, Direction = direction, Location = position });
// 기록 크기 제한
if (MovementHistory.Count > HISTORY_SIZE)
{
MovementHistory.RemoveAt(0);
}
//최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다.
if (MovementHistory.Count > 2 && MovementHistory.First().Direction != MovementHistory.Last().Direction)
{
var lastTwo = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToArray(); // [9, 10]
MovementHistory.Clear();
MovementHistory.AddRange(lastTwo);
}
}
// 연결 정보 기반 실제 이동 방향 계산
public Direction? CalculateActualDirectionByConnection(uint currentRFID, uint previousRFID, List<RFIDConnection> connections)
{
if (connections == null || connections.Count == 0)
return null;
// 이전 RFID에서 현재 RFID로의 연결 확인
var connection = connections.FirstOrDefault(c =>
(c.P1.Value == previousRFID && c.P2.Value == currentRFID) ||
(c.P1.Value == currentRFID && c.P2.Value == previousRFID));
if (connection == null)
return null; // 연결되지 않은 경로
// 연결 방향에 따라 실제 이동 방향 결정
if (connection.P1.Value == previousRFID && connection.P2.Value == currentRFID)
{
return Direction.Forward; // Start -> End 방향으로 이동
}
else
{
return Direction.Backward; // End -> Start 방향으로 이동
}
}
// 연결 정보 기반 방향 불일치 검증 및 정정
public bool ValidateAndCorrectDirectionByConnection(Direction expectedDirection, List<RFIDConnection> connections)
{
if (MovementHistory.Count < 2 || connections == null)
return true; // 검증 불가능한 경우
// 최근 두 RFID 값 가져오기
var recentRFIDs = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToList();
if (recentRFIDs.Count < 2)
return true;
var previousRFID = recentRFIDs[0];
var currentRFID = recentRFIDs[1];
var actualDirection = CalculateActualDirectionByConnection(currentRFID.Value, previousRFID.Value, connections);
if (!actualDirection.HasValue)
return true; // 연결 정보로 방향 판단 불가
// 방향이 일치하지 않는 경우
if (actualDirection.Value != expectedDirection)
{
// AGV 모터 방향을 실제 이동 방향으로 정정
CurrentAGVDirection = actualDirection.Value;
TargetDirection = actualDirection.Value;
return false; // 정정됨을 알림
}
return true; // 방향 일치
}
// RFID 순서 기반 실제 이동 방향 계산 (기존 메서드 - 호환성 유지)
public Direction? CalculateActualDirectionByRFID()
{
if (MovementHistory.Count < 2)
return null;
// 최근 두 RFID 값으로부터 실제 이동 방향 계산
var recentRFIDs = MovementHistory.Skip(Math.Max(0, MovementHistory.Count - 2)).Take(2).ToList();
if (recentRFIDs.Count < 2)
return null;
var prevRFID = recentRFIDs[0];
var currentRFID = recentRFIDs[1];
// RFID 값의 증가/감소로 방향 판단
if (currentRFID.Value > prevRFID.Value)
{
return Direction.Forward; // RFID 값이 증가하면 전진
}
else if (currentRFID.Value < prevRFID.Value)
{
return Direction.Backward; // RFID 값이 감소하면 후진
}
else
{
return null; // 같은 RFID 값이면 방향 판단 불가
}
}
}
public class PathNode
{
public Point Location { get; set; }
public string RFID { get; set; }
public double G { get; set; } // 시작점에서 현재 노드까지의 비용
public double H { get; set; } // 현재 노드에서 목표점까지의 예상 비용
public double F => G + H; // 총 비용
public PathNode Parent { get; set; }
public PathNode(Point location, string rfid)
{
Location = location;
RFID = rfid;
G = 0;
H = 0;
Parent = null;
}
}
}

View File

@@ -1,4 +1,5 @@
using AGVControl.Models;
using System;
namespace AGVControl
{
@@ -9,6 +10,48 @@ namespace AGVControl
public string Reason { get; set; }
public AGVActionReasonCode ReasonCode { get; set; }
public AGVMoveState MoveState { get; set; } // RUN 또는 STOP
}
public AgvSpeed? MoveSpeed { get; set; }
public AgvRunDirection? MoveDiv { get; set; }
public UInt32 Idx { get; set; }
public bool Changed { get; set; }
// override object.Equals
public bool Equals(AGVActionPrediction obj)
{
// null 체크
if (obj == null) return false;
// 참조가 같으면 true
if (ReferenceEquals(this, obj)) return true;
// 핵심 속성들만 비교 (Idx, Changed 제외)
if (obj.Direction != this.Direction) return false;
if (obj.ReasonCode != this.ReasonCode) return false;
if (obj.MoveState != this.MoveState) return false;
if (obj.MoveSpeed != this.MoveSpeed) return false;
if (obj.MoveDiv != this.MoveDiv) return false;
// NextRFID 비교 (null 체크 포함)
if (obj.NextRFID == null || this.NextRFID == null)
{
if (obj.NextRFID != this.NextRFID) return false; // 하나만 null이면 false
}
else
{
if (obj.NextRFID.Value != this.NextRFID.Value) return false;
}
// Reason 비교 (null 체크 포함)
if (obj.Reason == null || this.Reason == null)
{
if (obj.Reason != this.Reason) return false; // 하나만 null이면 false
}
else
{
if (obj.Reason != this.Reason) return false;
}
return true;
}
}
}

View File

@@ -1,9 +1,75 @@
using System.Drawing;
using System;
using System.Collections.Generic;
using System.Windows.Forms;
namespace AGVControl.Models
{
{
/// <summary>
/// 도로특성정보
/// </summary>
public class RoadInformation
{
/// <summary>
/// 시작지점
/// </summary>
public RFIDPoint P1 { get; set; }
/// <summary>
/// 종료지점
/// </summary>
public RFIDPoint P2 { get; set; }
/// <summary>
/// 도로의 사용여부
/// </summary>
public bool Enable { get; set; }
/// <summary>
/// AGV의 이동방향(리프트방향)
/// 목적지 방향과의 일치를 위해 해당 방향을 설정할 수 있따
/// </summary>
public Direction? LiftDirection { get; set; }
/// <summary>
/// AGV이동시 속도 (high, middle, low)
/// </summary>
public AgvSpeed? MoveSpeed { get; set; }
/// <summary>
/// AGV이동시 방향모드(좌분기, 전진, 우분기)
/// </summary>
public AgvRunDirection? MoveDirection { get; set; }
public RoadInformation()
{
P1 = null;
P2 = null;
LiftDirection = null;
MoveSpeed = null;
MoveDirection = null;
Enable = false;
}
/// <summary>
/// 값이 설정되어있는지
/// </summary>
public bool HasValue
{
get
{
if (P1 == null || P2 == null) return false;
if (LiftDirection == null && MoveSpeed == null && MoveDirection == null) return false;
return true;
}
}
}
public class MagnetLine
{
public Point StartPoint { get; set; }

View File

@@ -1,14 +1,195 @@
using AGVControl.Models;
using AR;
using System.Collections.Specialized;
using System.ComponentModel;
using System.Diagnostics.Tracing;
namespace AGVControl
{
public class RFIDConnection
{
/// <summary>
/// 시작지점
/// </summary>
public RFIDPoint P1 { get; set; }
/// <summary>
/// 종료지점
/// </summary>
public RFIDPoint P2 { get; set; }
public bool DisableP1_to_P2 { get; set; }
public bool DisableP2_to_P1 { get; set; }
public float Distance { get; set; }
/// <summary>
/// 도로의 사용여부
/// </summary>
public bool EnableP { get; set; }
public bool EnableN { get; set; }
/// <summary>
/// AGV의 이동방향(리프트방향)
/// 목적지 방향과의 일치를 위해 해당 방향을 설정할 수 있따
/// </summary>
public Direction? LiftDirectionP { get; set; }
public Direction? LiftDirectionN { get; set; }
/// <summary>
/// AGV이동시 속도 (high, middle, low)
/// </summary>
public AgvSpeed? MoveSpeedP { get; set; }
public AgvSpeed? MoveSpeedN { get; set; }
/// <summary>
/// AGV이동시 방향모드(좌분기, 전진, 우분기)
/// </summary>
public AgvRunDirection? MoveDirectionP { get; set; }
public AgvRunDirection? MoveDirectionN { get; set; }
/// <summary>
/// 파일저장 및 불러오기시 사용하는 문자열로 반환
/// </summary>
[Browsable(false)]
public string DataFileString
{
get
{
var str_enbP = EnableP ? "1" : "0";
var str_liftP = "";
if (LiftDirectionP != null) str_liftP = ((int)LiftDirectionP).ToString();
var str_movespeedP = "";
if (MoveSpeedP != null) str_movespeedP = ((int)MoveSpeedP).ToString();
var str_movdirP = "";
if (MoveDirectionP != null) str_movdirP = ((int)MoveDirectionP).ToString();
var str_enbN = EnableP ? "1" : "0";
var str_liftN = "";
if (LiftDirectionN != null) str_liftN = ((int)LiftDirectionN).ToString();
var str_movespeedN = "";
if (MoveSpeedN != null) str_movespeedN = ((int)MoveSpeedN).ToString();
var str_movdirN = "";
if (MoveDirectionN != null) str_movdirN = ((int)MoveDirectionN).ToString();
var PStr= $"{P1.Location.X},{P1.Location.Y},{P2.Location.X},{P2.Location.Y}," + //location
$"{P1.Value},{P2.Value}," + //rfid values
$"{str_enbP};{str_liftP};{str_movespeedP};{str_movdirP}," +
$"{str_enbN};{str_liftN};{str_movespeedN};{str_movdirN}";
return $"{PStr}";
}
set
{
var buf = value.Split(',');
if (buf.Length >= 2)
{
var p1x = int.Parse(buf[0]);
var p1y = int.Parse(buf[1]);
var p2x = int.Parse(buf[2]);
var p2y = int.Parse(buf[3]);
var p1v = uint.Parse(buf[4]);
var p2v = uint.Parse(buf[5]);
if (P1 == null) P1 = new RFIDPoint();
P1.Location = new System.Drawing.Point(p1x, p1y);
P1.Value = p1v;
if (P2 == null) P2 = new RFIDPoint();
P2.Location = new System.Drawing.Point(p2x, p2y);
P2.Value = p2v;
if (buf[6].Contains(";")) //양방향 정보
{
var StrP = buf[6].Split(';');
var StrN = buf[7].Split(';');
//Positive
this.EnableP = StrP[0] == "1";
if (StrP[1].isEmpty()) LiftDirectionP = null;
else LiftDirectionP = (Direction)int.Parse(StrP[1]);
if (StrP[2].isEmpty()) MoveSpeedP = null;
else MoveSpeedP = (AgvSpeed)int.Parse(StrP[2]);
if (StrP[3].isEmpty()) MoveDirectionP = null;
else MoveDirectionP = (AgvRunDirection)int.Parse(StrP[3]);
//Negative
this.EnableN = StrN[0] == "1";
if (StrN[1].isEmpty()) LiftDirectionN = null;
else LiftDirectionN = (Direction)int.Parse(StrN[1]);
if (StrN[2].isEmpty()) MoveSpeedN = null;
else MoveSpeedN = (AgvSpeed)int.Parse(StrN[2]);
if (StrN[3].isEmpty()) MoveDirectionN = null;
else MoveDirectionN = (AgvRunDirection)int.Parse(StrN[3]);
}
else
{
this.EnableP = buf[6] == "1";
if (buf[7].isEmpty()) LiftDirectionP = null;
else LiftDirectionP = (Direction)int.Parse(buf[7]);
if (buf[8].isEmpty()) MoveSpeedP = null;
else MoveSpeedP = (AgvSpeed)int.Parse(buf[8]);
if (buf[9].isEmpty()) MoveDirectionP = null;
else MoveDirectionP = (AgvRunDirection)int.Parse(buf[9]);
this.EnableN = this.EnableP;
this.LiftDirectionN = this.LiftDirectionP;
this.MoveSpeedN = this.MoveSpeedP;
this.MoveDirectionN = this.MoveDirectionP;
}
}
}
}
public RFIDConnection(string dataline = "")
{
P1 = null;
P2 = null;
LiftDirectionP = null;
MoveSpeedP = null;
MoveDirectionP = null;
EnableP = false;
LiftDirectionN = null;
MoveSpeedN = null;
MoveDirectionP = null;
EnableP = false;
if (dataline.isEmpty() == false) DataFileString = dataline;
}
/// <summary>
/// 값이 설정되어있는지
/// </summary>
public bool HasValue
{
get
{
if (P1 == null || P2 == null) return false;
if (LiftDirectionP == null && MoveSpeedP == null && MoveDirectionP == null &&
LiftDirectionN == null && MoveSpeedN == null && MoveDirectionN == null) return false;
return true;
}
}
public override bool Equals(object obj)
@@ -27,7 +208,7 @@ namespace AGVControl
public override string ToString()
{
//연결정보를 확인
return $"{P1.Value} ↔ {P2.Value},P1-2:{(DisableP1_to_P2 ? "X" : "O")},P2-1:{(DisableP2_to_P1 ? "X" : "O")}";
return $"{P1.Value} ↔ {P2.Value},ENB:{(EnableP ? "O" : "X")}|{(EnableN ? "O" : "X")}";
}
}
}

View File

@@ -1,6 +1,7 @@
using System.Drawing;
using System;
using System.Collections.Generic;
using System.ComponentModel;
namespace AGVControl.Models
{
public class RFIDPoint
@@ -11,7 +12,9 @@ namespace AGVControl.Models
public bool IsBidirectional { get; set; } // 양방향 연결 여부
public bool IsRotatable { get; set; } // 회전 가능 여부
public Direction? FixedDirection { get; set; } // 고정 방향(없으면 null)
public bool IsTerminal { get; set; } // 종단 여부
public bool IsTerminal { get; set; } // 종단 여부
[Browsable(false)]
public RectangleF Bounds { get; set; }
public void Clear()
{

View File

@@ -6,12 +6,32 @@ using System.Threading.Tasks;
namespace AGVControl
{
public enum Direction
{
Forward = 0,
Backward = 1,
Stop = 2
}
public enum AGVMoveState
{
Stop = 0,
Run
}
public enum AgvSpeed
{
High,
Middle,
Low,
}
public enum AgvRunDirection
{
Straight,
Left,
Right,
}
public enum AGVActionReasonCode
{