..
This commit is contained in:
chi
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user