PuAiSim/Assets/scanBoardBehavior.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class scanBoardBehavior : MonoBehaviour
{
    public GameObject ctrlr;
    public GameObject arm;
    public int targetTerm;
    /// <summary>
    /// 待controller清除
    /// </summary>
    public bool scanned;
    public bool avoiding;
    public float speedX;
    // 0空闲 1检查门状态加锁 2运动
    public int state;
    private static float[] termPosX = { -1.5f, 1.2f, 6.2f, 8.7f, 13.7f, 16.2f, 21.2f, 24f };

    /// <summary>
    /// 获取当前位置终端号，1-6
    /// </summary>
    /// <returns></returns>
    public int GetPos()
    {
        float dist = 99;
        int index = -1;
        for (int i = 0; i < termPosX.Length; i++)
        {
            if(Mathf.Abs(transform.position.x - termPosX[i]) < dist)
            {
                dist = Mathf.Abs(transform.position.x - termPosX[i]);
                index = i;
            }
        }
        return index;
    }

    /// <summary>
    ///  获取离扫描板最近终端编号0-5
    /// </summary>
    /// <returns></returns>
    public int GetNearestTerm()
    {
        // 已靠边
        if (transform.position.x < -1 || transform.position.x > 23)
        {
            return -1;
        }
        else
        {
            float diff = 99f;
            int index = -1;
            for (int i = 1; i < termPosX.Length-1; i++)
            {
                if (diff > Mathf.Abs(transform.position.x - termPosX[i]))
                {
                    diff = Mathf.Abs(transform.position.x - termPosX[i]);
                    index = i;
                }
            }
            return index-1;
        }
    }

    public void BoardMove(float diff, int toState)
    {
        if (diff > 0)
        {
            for (int i = termPosX.Length - 2; i >= 1; i--)
            {
                // 找到其右边第一个终端
                if (transform.position.x > termPosX[i])
                {
                    // 未上锁等待
                    if (ctrlr.GetComponent<controller>().doorLock[i - 1] != 1)
                    {
                        Debug.Log("扫描板等待锁门");
                        return;
                    }
                }
            }
            //diff = transform.position.x - termPosX[0];
            transform.Translate(Vector3.left * speedX * Time.deltaTime);
        }
        else
        {
            for (int i = 1; i < termPosX.Length - 1; i++)
            {
                // 找到其左边第一个终端
                if (transform.position.x < termPosX[i])
                {
                    // 未上锁等待
                    if (ctrlr.GetComponent<controller>().doorLock[i - 1] != 1)
                    {
                        Debug.Log("扫描板等待锁门");
                        return;
                    }
                }
            }
            //diff = transform.position.x - termPosX[termPosX.Length-1];
            transform.Translate(Vector3.right * speedX * Time.deltaTime);
        }
        if (Mathf.Abs(diff) < speedX * 0.1f)
        {
            state = toState;

        }
    }

    // Start is called before the first frame update
    void Start()
    {
        targetTerm = -1;
        speedX = 6f;
        state = 0;
        scanned = false;
        avoiding = false;
    }

    // Update is called once per frame
    void Update()
    {
        if (ctrlr.GetComponent<controller>().doors.Length != 6)
            return;
        // 与机械手存在碰撞风险，暂停扫描工作，专心避让
        if(arm.GetComponent<machineArmBehavior>().state == 11 || arm.GetComponent<machineArmBehavior>().state == 12 || arm.GetComponent<machineArmBehavior>().state == 13 || 
            arm.GetComponent<machineArmBehavior>().state == 26 || arm.GetComponent<machineArmBehavior>().state == 17 || arm.GetComponent<machineArmBehavior>().state == 18)
        {
            if (Mathf.Abs(transform.position.x - arm.GetComponent<machineArmBehavior>().transform.position.x) < 4f)
            {
                int term = GetNearestTerm();
                float diff = 0;
                if (term != -1)
                {
                    // 往右
                    diff = transform.position.x - termPosX[term];
                }
                BoardMove(diff, state);
                avoiding = true;
                return;
            }
            else
            {
                avoiding = false;
            }
        }
        switch (state)
        {
            case 0:
                if (targetTerm == -1)
                {
                    //if (Mathf.Abs(transform.position.x - termPosX[0]) < Mathf.Abs(transform.position.x - termPosX[termPosX.Length - 1]))
                    //{
                    //    //diff = transform.position.x - termPosX[0];
                    //    //if (diff < speedX * 0.05f)
                    //    //{
                    //        transform.Translate(Vector3.left * speedX * Time.deltaTime);
                    //    //}
                    //}
                    //else
                    //{
                    //    //diff = 
                    //    transform.Translate(Vector3.right * speedX * Time.deltaTime);
                    //}
                }
                else
                {
                    state = 1;
                }
                break;
            case 1:
                float diff = transform.position.x - termPosX[targetTerm];
                int occupiedPlace = -1;
                // 限定扫描范围
                for (int i = 0; i < termPosX.Length; i++)
                {
                    if (diff > 0)
                    {
                        if (transform.position.x <= (termPosX[i] + 0.25f))
                        {
                            occupiedPlace = i; break;
                        }
                    }
                    else
                    {
                        if (transform.position.x > (termPosX[i] - 0.25f))
                        {
                            occupiedPlace = i;
                        }
                    }
                }
                if (occupiedPlace < 1) occupiedPlace = 1;
                if (occupiedPlace > 6) occupiedPlace = 6;
                int targetClamp = targetTerm;
                targetClamp = targetClamp < 1 ? 1 : targetClamp;
                targetClamp = targetClamp > 6 ? 6 : targetClamp;
                // 等待关门，门上锁，检查上锁情况
                if (diff > 0)
                {
                    for (int i = targetClamp; i <= occupiedPlace; i++)
                    {
                        // 未关门，停止动作等待
                        if (!ctrlr.GetComponent<controller>().doors[i - 1].GetComponent<doorBehavior>().close) return;
                        //else if (ctrlr.GetComponent<controller>().doorLock[i - 1] == 0)
                        //    ctrlr.GetComponent<controller>().doorLock[i - 1] = 1;
                    }
                    for (int i = targetClamp; i <= occupiedPlace; i++)
                    {
                        if (!ctrlr.GetComponent<controller>().doors[i - 1].GetComponent<doorBehavior>().close || !(ctrlr.GetComponent<controller>().doorLock[i - 1] == 1)) return;
                    }
                }
                else
                {
                    for (int i = occupiedPlace; i <= targetClamp; i++)
                    {
                        // 未关门，停止动作等待
                        if (!ctrlr.GetComponent<controller>().doors[i - 1].GetComponent<doorBehavior>().close) return;
                        //else if (ctrlr.GetComponent<controller>().doorLock[i - 1] == 0)
                        //    ctrlr.GetComponent<controller>().doorLock[i - 1] = 1;
                    }
                    for (int i = occupiedPlace; i <= targetClamp; i++)
                    {
                        if (!ctrlr.GetComponent<controller>().doors[i - 1].GetComponent<doorBehavior>().close || !(ctrlr.GetComponent<controller>().doorLock[i - 1] == 1)) return;
                    }
                }
                // 等待机械手运动结束
                int armState = arm.GetComponent<machineArmBehavior>().state;
                if (armState == 0)
                {
                    state = 2;
                }
                break;
            case 2:
                diff = transform.position.x - termPosX[targetTerm];
                BoardMove(diff,3);
                break;
            case 3:
                targetTerm = -1;
                state = 0;
                scanned = true;
                break;
            default: break;
        }
    }
}