Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

..

Browse Source
This commit is contained in:
chi
2025-06-23 17:38:24 +09:00
parent 3fcbbfe354
commit ba18564719
5 changed files with 253 additions and 444 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
Cs_HMI/SubProject/AGVControl/Models/AGV.cs
View File

@@ -2,6 +2,7 @@ using System;
using System.Drawing;
using System.Collections.Generic;
using System.Linq;
using System.Security.Permissions;
namespace AGVControl.Models
{
@@ -12,65 +13,99 @@ namespace AGVControl.Models
Stop = 2
}
public struct movehistorydata
public class CRFIDData
{
public UInt16 rfid { get; set; }
public Point Position { get; set; }
public override string ToString()
{
return $"RFID:{rfid},P:{Position.X},{Position.Y}";
}
}
public class movehistorydata : CRFIDData
{
public Direction direction { get; set; }
public override string ToString()
{
return $"RFID:{rfid},DIR:{direction}";
return $"RFID:{rfid},DIR:{direction},P:{Position.X},{Position.Y}";
}
}
public class AGV
{
public Point CurrentPosition { get; set; }
public uint CurrentRFID { get; set; }
public float BatteryLevel { get; set; } = 0f;
/// <summary>
/// AGV에서 방향값이 수산됩니다.
/// RFID 번호
/// </summary>
public Direction CurrentDirection { get; set; }
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 List<Point> CurrentPath { get; set; } = new List<Point>();
/// <summary>
/// 경로검색으로 입력된 경로
/// </summary>
public List<RFIDPoint> CurrentPath { get; set; } = new List<RFIDPoint>();
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>();
CurrentPath = new List<RFIDPoint>();
PlannedPath = new List<Point>();
PathRFIDs = new List<string>();
CurrentDirection = Direction.Forward;
TargetPosition = Point.Empty;
TargetRFID = 0;
CurrentRFID = new RFIDPoint();
TargetRFID = new RFIDPoint();
TargetDirection = Direction.Forward;
BodyAngle = null;
}
public void Move()
{
if (CurrentPath.Count > 0)
{
CurrentPosition = CurrentPath[0];
CurrentPath.RemoveAt(0);
}
// BodyAngle = null;
}
// 이동 경로에 새로운 RFID 추가
public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
{
@@ -78,18 +113,16 @@ namespace AGVControl.Models
if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue)
return;
MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction }) ;
PositionHistory.Add(position);
MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction, Position = position });
// 기록 크기 제한
if (MovementHistory.Count > HISTORY_SIZE)
{
MovementHistory.RemoveAt(0);
PositionHistory.RemoveAt(0);
}
//최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다.
if(MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction)
if (MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction)
{
var lastTwo = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToArray(); // [9, 10]
MovementHistory.Clear();
@@ -129,7 +162,7 @@ namespace AGVControl.Models
return true; // 검증 불가능한 경우
// 최근 두 RFID 값 가져오기
var recentRFIDs = MovementHistory.Skip( MovementHistory.Count - 2).Take(2).ToList();
var recentRFIDs = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToList();
if (recentRFIDs.Count < 2)
return true;
@@ -144,15 +177,9 @@ namespace AGVControl.Models
if (actualDirection.Value != expectedDirection)
{
// AGV 모터 방향을 실제 이동 방향으로 정정
CurrentDirection = actualDirection.Value;
CurrentAGVDirection = actualDirection.Value;
TargetDirection = actualDirection.Value;
// 몸체 방향도 180도 회전 (결정된 경우에만)
if (BodyAngle.HasValue)
{
BodyAngle = (BodyAngle.Value + 180) % 360;
}
return false; // 정정됨을 알림
}
@@ -188,77 +215,7 @@ namespace AGVControl.Models
}
}
// 경로상 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