282 lines
10 KiB
C#
282 lines
10 KiB
C#
using System;
|
|
using System.Drawing;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
|
|
namespace AGVControl.Models
|
|
{
|
|
public enum Direction
|
|
{
|
|
Forward = 0,
|
|
Backward = 1,
|
|
Stop = 2
|
|
}
|
|
|
|
public struct movehistorydata
|
|
{
|
|
public UInt16 rfid { get; set; }
|
|
public Direction direction { get; set; }
|
|
|
|
public override string ToString()
|
|
{
|
|
return $"RFID:{rfid},DIR:{direction}";
|
|
}
|
|
}
|
|
|
|
public class AGV
|
|
{
|
|
public Point CurrentPosition { get; set; }
|
|
public uint CurrentRFID { get; set; }
|
|
public float BatteryLevel { get; set; } = 0f;
|
|
/// <summary>
|
|
/// AGV에서 방향값이 수산됩니다.
|
|
/// </summary>
|
|
public Direction CurrentDirection { get; set; }
|
|
|
|
/// <summary>
|
|
/// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다.
|
|
/// </summary>
|
|
public Direction TargetDirection { get; set; } = Direction.Stop;
|
|
public bool IsMoving { get; set; }
|
|
public List<Point> CurrentPath { get; set; } = new List<Point>();
|
|
public List<Point> PlannedPath { get; set; }
|
|
public List<string> PathRFIDs { get; set; }
|
|
public Point TargetPosition { get; set; }
|
|
public uint TargetRFID { get; set; }
|
|
public float? BodyAngle { get; set; } = null;
|
|
public float MotorAngle { get; set; } = 0f;
|
|
// 이동 경로 기록을 위한 새로운 속성들
|
|
public List<movehistorydata> MovementHistory { get; } = new List<movehistorydata>();
|
|
public List<Point> PositionHistory { get; } = new List<Point>();
|
|
public const int HISTORY_SIZE = 4; // 최근 4개 위치 기록
|
|
|
|
public AGV()
|
|
{
|
|
CurrentPath = new List<Point>();
|
|
PlannedPath = new List<Point>();
|
|
PathRFIDs = new List<string>();
|
|
CurrentDirection = Direction.Forward;
|
|
TargetPosition = Point.Empty;
|
|
TargetRFID = 0;
|
|
TargetDirection = Direction.Forward;
|
|
BodyAngle = null;
|
|
}
|
|
|
|
public void Move()
|
|
{
|
|
if (CurrentPath.Count > 0)
|
|
{
|
|
CurrentPosition = CurrentPath[0];
|
|
CurrentPath.RemoveAt(0);
|
|
}
|
|
}
|
|
|
|
// 이동 경로에 새로운 RFID 추가
|
|
public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
|
|
{
|
|
// 중복 RFID가 연속으로 들어오는 경우 무시
|
|
if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue)
|
|
return;
|
|
|
|
MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction }) ;
|
|
PositionHistory.Add(position);
|
|
|
|
// 기록 크기 제한
|
|
if (MovementHistory.Count > HISTORY_SIZE)
|
|
{
|
|
MovementHistory.RemoveAt(0);
|
|
PositionHistory.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.StartRFID == previousRFID && c.EndRFID == currentRFID) ||
|
|
(c.IsBidirectional && c.StartRFID == currentRFID && c.EndRFID == previousRFID));
|
|
|
|
if (connection == null)
|
|
return null; // 연결되지 않은 경로
|
|
|
|
// 연결 방향에 따라 실제 이동 방향 결정
|
|
if (connection.StartRFID == previousRFID && connection.EndRFID == 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.rfid, previousRFID.rfid, connections);
|
|
if (!actualDirection.HasValue)
|
|
return true; // 연결 정보로 방향 판단 불가
|
|
|
|
// 방향이 일치하지 않는 경우
|
|
if (actualDirection.Value != expectedDirection)
|
|
{
|
|
// AGV 모터 방향을 실제 이동 방향으로 정정
|
|
CurrentDirection = actualDirection.Value;
|
|
TargetDirection = actualDirection.Value;
|
|
|
|
// 몸체 방향도 180도 회전 (결정된 경우에만)
|
|
if (BodyAngle.HasValue)
|
|
{
|
|
BodyAngle = (BodyAngle.Value + 180) % 360;
|
|
}
|
|
|
|
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.rfid > prevRFID.rfid)
|
|
{
|
|
return Direction.Forward; // RFID 값이 증가하면 전진
|
|
}
|
|
else if (currentRFID.rfid < prevRFID.rfid)
|
|
{
|
|
return Direction.Backward; // RFID 값이 감소하면 후진
|
|
}
|
|
else
|
|
{
|
|
return null; // 같은 RFID 값이면 방향 판단 불가
|
|
}
|
|
}
|
|
|
|
// 경로상 RFID 순서 기반 예상 방향 계산
|
|
public Direction? CalculateExpectedDirectionByRFID()
|
|
{
|
|
if (CurrentPath == null || CurrentPath.Count < 2)
|
|
return null;
|
|
|
|
// 현재 위치의 RFID 찾기
|
|
var currentRFID = CurrentRFID;
|
|
if (currentRFID == 0)
|
|
return null;
|
|
|
|
// 경로상 다음 RFID 찾기
|
|
int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition);
|
|
if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1)
|
|
return null;
|
|
|
|
// 다음 위치의 RFID 찾기 (MapControl에서 RFID 정보 필요)
|
|
// 이 부분은 MapControl에서 처리하도록 수정 필요
|
|
return null;
|
|
}
|
|
|
|
// 실제 이동 방향 계산 (기존 메서드 - 호환성 유지)
|
|
public Direction? CalculateActualDirection()
|
|
{
|
|
if (MovementHistory.Count < 2)
|
|
return null;
|
|
|
|
// 최근 두 위치로부터 실제 이동 방향 계산
|
|
var recentPositions = PositionHistory.Skip(Math.Max(0, PositionHistory.Count - 2)).Take(2).ToList();
|
|
if (recentPositions.Count < 2)
|
|
return null;
|
|
|
|
var prevPos = recentPositions[0];
|
|
var currentPos = recentPositions[1];
|
|
|
|
// X축 이동이 더 큰 경우
|
|
if (Math.Abs(currentPos.X - prevPos.X) > Math.Abs(currentPos.Y - prevPos.Y))
|
|
{
|
|
return currentPos.X > prevPos.X ? Direction.Forward : Direction.Backward;
|
|
}
|
|
// Y축 이동이 더 큰 경우
|
|
else
|
|
{
|
|
return currentPos.Y > prevPos.Y ? Direction.Forward : Direction.Backward;
|
|
}
|
|
}
|
|
|
|
// 경로상 예상 방향 계산
|
|
public Direction? CalculateExpectedDirection()
|
|
{
|
|
if (CurrentPath == null || CurrentPath.Count < 2)
|
|
return null;
|
|
|
|
int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition);
|
|
if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1)
|
|
return null;
|
|
|
|
var currentPos = CurrentPath[currentIdx];
|
|
var nextPos = CurrentPath[currentIdx + 1];
|
|
|
|
// X축 이동이 더 큰 경우
|
|
if (Math.Abs(nextPos.X - currentPos.X) > Math.Abs(nextPos.Y - currentPos.Y))
|
|
{
|
|
return nextPos.X > currentPos.X ? Direction.Forward : Direction.Backward;
|
|
}
|
|
// Y축 이동이 더 큰 경우
|
|
else
|
|
{
|
|
return nextPos.Y > currentPos.Y ? Direction.Forward : Direction.Backward;
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
} |