352 lines
12 KiB
C#
352 lines
12 KiB
C#
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 AgvDir 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 Speed
|
|
/// </summary>
|
|
public AgvSpeed CurrentSpeed { get; set; }
|
|
|
|
/// <summary>
|
|
/// AGV STS
|
|
/// </summary>
|
|
public AgvSts CurrentSTS { get; set; }
|
|
|
|
/// <summary>
|
|
/// AGV Motor Direction
|
|
/// </summary>
|
|
public AgvDir Current_Motor_Direction { get; set; }
|
|
|
|
/// <summary>
|
|
/// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다.
|
|
/// </summary>
|
|
public AgvDir? TargetDirection { get; set; }
|
|
|
|
/// <summary>
|
|
/// AGV.System1.agv_Run
|
|
/// </summary>
|
|
public bool IsMoving { get; set; }
|
|
/// <summary>
|
|
/// AGV.System1.Mark1_Check | Mark2_Check
|
|
/// </summary>
|
|
public bool IsMarkCheck { get; set; }
|
|
|
|
/// <summary>
|
|
/// 이동대상과 AGV의 머리방향이 일치하는지?
|
|
/// </summary>
|
|
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 = AgvDir.Forward;
|
|
// BodyAngle = null;
|
|
}
|
|
|
|
|
|
// 이동 경로에 새로운 RFID 추가
|
|
public void AddToMovementHistory(UInt16 rfidValue, Point position, AgvDir 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);
|
|
}
|
|
|
|
// 위치 업데이트 시 자동으로 히스토리 파일에 저장
|
|
SaveHistoryOnUpdate();
|
|
}
|
|
|
|
// 연결 정보 기반 실제 이동 방향 계산
|
|
public AgvDir? 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 AgvDir.Forward; // Start -> End 방향으로 이동
|
|
}
|
|
else
|
|
{
|
|
return AgvDir.Backward; // End -> Start 방향으로 이동
|
|
}
|
|
}
|
|
|
|
// 연결 정보 기반 방향 불일치 검증 및 정정
|
|
public bool ValidateAndCorrectDirectionByConnection(AgvDir 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 AgvDir? 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 AgvDir.Forward; // RFID 값이 증가하면 전진
|
|
}
|
|
else if (currentRFID.Value < prevRFID.Value)
|
|
{
|
|
return AgvDir.Backward; // RFID 값이 감소하면 후진
|
|
}
|
|
else
|
|
{
|
|
return null; // 같은 RFID 값이면 방향 판단 불가
|
|
}
|
|
}
|
|
|
|
// 위치 히스토리 파일 저장 (최근 3개만 저장)
|
|
public void SavePositionHistory(string filePath)
|
|
{
|
|
try
|
|
{
|
|
// 최근 3개의 히스토리만 저장
|
|
var recentHistory = MovementHistory.Skip(Math.Max(0, MovementHistory.Count - 3)).ToList();
|
|
|
|
var lines = new List<string>();
|
|
lines.Add($"# AGV Position History - {DateTime.Now:yyyy-MM-dd HH:mm:ss}");
|
|
lines.Add("# Format: RFID,Direction,X,Y,Timestamp");
|
|
|
|
foreach (var history in recentHistory)
|
|
{
|
|
lines.Add($"{history.Value},{history.Direction},{history.Location.X},{history.Location.Y},{DateTime.Now:yyyy-MM-dd HH:mm:ss}");
|
|
}
|
|
|
|
System.IO.File.WriteAllLines(filePath, lines);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// 로그 기록 (실제 환경에서는 로깅 시스템 사용)
|
|
System.Diagnostics.Debug.WriteLine($"SavePositionHistory Error: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
// 위치 히스토리 파일 로드
|
|
public bool LoadPositionHistory(string filePath)
|
|
{
|
|
try
|
|
{
|
|
if (!System.IO.File.Exists(filePath))
|
|
return false;
|
|
|
|
var lines = System.IO.File.ReadAllLines(filePath);
|
|
MovementHistory.Clear();
|
|
|
|
foreach (var line in lines)
|
|
{
|
|
// 주석 라인 건너뛰기
|
|
if (line.StartsWith("#") || string.IsNullOrWhiteSpace(line))
|
|
continue;
|
|
|
|
var parts = line.Split(',');
|
|
if (parts.Length >= 4)
|
|
{
|
|
if (UInt16.TryParse(parts[0], out UInt16 rfidValue) &&
|
|
Enum.TryParse<AgvDir>(parts[1], out AgvDir direction) &&
|
|
int.TryParse(parts[2], out int x) &&
|
|
int.TryParse(parts[3], out int y))
|
|
{
|
|
MovementHistory.Add(new movehistorydata
|
|
{
|
|
Value = rfidValue,
|
|
Direction = direction,
|
|
Location = new Point(x, y)
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
return MovementHistory.Count > 0;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"LoadPositionHistory Error: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// 시작 시 위치 히스토리 자동 로드
|
|
public void LoadHistoryOnStartup()
|
|
{
|
|
string historyFilePath = System.IO.Path.Combine(
|
|
System.IO.Path.GetDirectoryName(System.Reflection.Assembly.GetExecutingAssembly().Location),
|
|
"agv_position_history.dat"
|
|
);
|
|
|
|
LoadPositionHistory(historyFilePath);
|
|
}
|
|
|
|
// 위치 업데이트 시 자동 저장
|
|
public void SaveHistoryOnUpdate()
|
|
{
|
|
string historyFilePath = System.IO.Path.Combine(
|
|
System.IO.Path.GetDirectoryName(System.Reflection.Assembly.GetExecutingAssembly().Location),
|
|
"agv_position_history.dat"
|
|
);
|
|
|
|
SavePositionHistory(historyFilePath);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
} |