PuAiSim/Assets/machineArmBehavior.cs

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

public class machineArmBehavior : MonoBehaviour
{
    // 非空闲，第一位2表示已就位，1表示运行中。
    //0 idle, x1 park x forward, x2 park y downward, x3 park y upward, x4 park put car and y reset
    //x6 fetch x forward, x7 fetch y downward, x8 fetch y downward 
    public GameObject ctrlr;
    public int state;
    public float xSpeed;
    public float ySpeed;
    public bool park;
    public int terminalIndex;
    //private const float minX = 0;
    //private const float maxX = 22.8f;
    private float[] termX = { 1.2f, 6.2f, 8.7f, 13.7f, 16.2f, 21.2f };
    private Vector3 currPos;
    private Transform armFront, roller;
    public GameObject myCar;

    // Start is called before the first frame update
    void Start()
    {
        state = 0;
        park = true;
        terminalIndex = -1;
        xSpeed = 6.0f;
        ySpeed = 6.0f;
        armFront = transform.Find("arm_front");
        roller = transform.Find("roller");
    }

    /// <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 = 0; i < termX.Length; i++)
            {
                if (diff > Mathf.Abs(transform.position.x - termX[i]))
                {
                    diff = Mathf.Abs(transform.position.x - termX[i]);
                    index = i;
                }
            }
            return index;
        }
    }

    /// <summary>
    /// move arm
    /// </summary>
    /// <param name="axis"> 0x 1y </param>
    /// <param name="diff"></param>
    /// <param name=""></param>
    public void ArmMove(int axis, float diff, int toState, GameObject car = null)
    {
        if (axis == 0)
        {
            if (Mathf.Abs(diff) < Time.deltaTime * 15)
            {
                state = toState;
            }
            else if (diff < 0)
            {
                for (int i = 0; i < termX.Length; i++)
                {
                    // 找到其左边第一个终端
                    if (currPos.x < termX[i])
                    {
                        // 未上锁等待
                        if (ctrlr.GetComponent<controller>().doorLock[i] != 1)
                        {
                            Debug.Log("机械手等待锁门");
                            return;
                        }
                    }
                }
                transform.Translate(Vector3.right * xSpeed * Time.deltaTime);
            }
            else
            {
                for (int i = termX.Length - 1; i >= 0; i--)
                {
                    // 找到其右边第一个终端
                    if (currPos.x > termX[i])
                    {
                        // 未上锁等待
                        if (ctrlr.GetComponent<controller>().doorLock[i] != 1)
                        {
                            Debug.Log("机械手等待锁门");
                            return;
                        }
                    }
                }
                transform.Translate(Vector3.left * xSpeed * Time.deltaTime);
            }
        }
        else if (axis == 1)
        {
            if (Mathf.Abs(diff) < 0.1)
            {
                state = toState;
            }
            else
            {
                if (diff < 0)
                {
                    roller.localScale += new Vector3(0f, ySpeed * Time.deltaTime);
                    roller.position -= new Vector3(0f, ySpeed * Time.deltaTime);
                }
                else
                {
                    roller.localScale -= new Vector3(0f, ySpeed * Time.deltaTime);
                    roller.position += new Vector3(0f, ySpeed * Time.deltaTime);
                }
                if (car != null)
                {
                    car.GetComponent<Transform>().position = new Vector3(currPos.x, currPos.y - 1.25f - roller.localScale.y * 2 - 1.5f, currPos.z);
                }
            }
        }
    }

    // Update is called once per frame
    void Update()
    {
        if (armFront == null || roller == null)
            return;
        currPos = GetComponent<Transform>().position;
        switch (state)
        {
            // 空闲状态
            case 0:
                if (terminalIndex > 0)
                {
                    if (terminalIndex < 7)
                        state = 11;
                    else
                        state = 15;
                }
                break;

            // 停车状态
            case 11://x轴平移
                float diffX = currPos.x - termX[(terminalIndex - 1) % 6];
                ArmMove(0, diffX, 21);
                break;
            case 21:
                state = 12;
                break;
            case 12:// y轴高度下降
                float diffY = roller.localScale.y - 4;
                ArmMove(1, diffY, 22);
                break;
            case 22:// 创建车模
                state = 13;
                myCar = ctrlr.GetComponent<controller>().CreateCar(new Vector3(currPos.x, currPos.y - 1.25f - roller.localScale.y * 2 - 1.5f, currPos.z));
                break;
            case 13:// y轴高度上升，移动车模
                diffY = roller.localScale.y - 2;
                ArmMove(1, diffY, 23, myCar);
                break;
            case 23:// 判断是否下方已存在等待的中跑车
                GameObject[] transfers = ctrlr.GetComponent<controller>().transferVehicles;
                for (int i = 0; i < transfers.Length; i++)
                {
                    Vector3 temp = transfers[i].GetComponent<Transform>().position;
                    transferBehavior tb = transfers[i].GetComponent<transferBehavior>();
                    if (tb.state == 4 && tb.cmd == 0 && Mathf.Abs(temp.y - 3) < 0.25f && Mathf.Abs(temp.x - currPos.x) < 1.5f && Mathf.Abs(temp.z - currPos.z) < 0.25f)
                    {
                        tb.cmd = 1;// 发送接车指令
                        state = 14;
                    }
                }
                break;
            case 14:// 收起y轴，保持车辆位置
                diffY = roller.localScale.y - 1;
                ArmMove(1, diffY, 24);
                break;
            case 24:// 恢复空闲，复位编号，需controller手动控制
                terminalIndex = -1;
                state = 0;
                myCar = null;
                break;

            // 取车状态
            case 15:// 移动x轴
                int index = (terminalIndex - 1) % 6 + 1;
                diffX = currPos.x - termX[index - 1];
                ArmMove(0, diffX, 25);
                break;
            case 25:
                // 等中跑车到达且状态为4
                transfers = ctrlr.GetComponent<controller>().transferVehicles;
                for (int i = 0; i < transfers.Length; i++)
                {
                    Vector3 temp = transfers[i].GetComponent<Transform>().position;
                    if (Mathf.Abs(temp.y - 3) < 0.25f && Mathf.Abs(temp.x - currPos.x) < 2f && Mathf.Abs(temp.z - currPos.z) < 0.25f)
                    {
                        if (transfers[i].GetComponent<transferBehavior>().state == 4)
                        {
                            state = 160;
                            break;
                        }
                    }
                }
                break;
            case 160:// 放下y轴，抓车, 由中跑车更新车辆句柄
                diffY = roller.localScale.y - 2;
                ArmMove(1, diffY, 160, myCar);
                if (myCar != null)
                {
                    state = 161;
                }
                break;
            case 161:
                diffY = roller.localScale.y - 1.5f;//收一点高度，让中跑车离开
                ArmMove(1, diffY, 26, myCar);
                break;
            case 26:// 等待中跑车移开
                transfers = ctrlr.GetComponent<controller>().transferVehicles;
                for (int i = 0; i < transfers.Length; i++)
                {
                    Vector3 temp = transfers[i].GetComponent<Transform>().position;
                    if (Mathf.Abs(temp.y - 3) < 0.25f && Mathf.Abs(temp.x - currPos.x) < 2f && Mathf.Abs(temp.z - currPos.z) < 0.25f)
                    {
                        return;
                    }
                }
                state = 17;
                break;
            case 17:// 放下y轴
                diffY = roller.localScale.y - 4;
                ArmMove(1, diffY, 27, myCar);
                break;
            case 27:
                state = 18;
                break;
            case 18:// 收起y轴,移动到旁边
                index = (terminalIndex - 1) % 6 + 1;
                diffY = roller.localScale.y - 1;
                ArmMove(1, diffY, 18);
                // 移动机械手到旁边，锁条件由controller调度
                if (index == 1)
                {
                    diffX = transform.position.x - (-1.5f);
                    ArmMove(0, diffX, 28);
                }else if(index == 6)
                {
                    diffX = transform.position.x - 24f;
                    ArmMove(0, diffX, 28);
                }
                // 右边已锁住
                else if (ctrlr.GetComponent<controller>().doorLock[index - 2] == 1 && ctrlr.GetComponent<controller>().doors[index - 2].GetComponent<doorBehavior>().close && !ctrlr.GetComponent<controller>().doors[index - 2].GetComponent<doorBehavior>().moving)
                {
                    diffX = transform.position.x - termX[index - 2];
                    ArmMove(0, diffX, 28);
                }
                // 左边已锁住
                else if (ctrlr.GetComponent<controller>().doorLock[index] == 1 && ctrlr.GetComponent<controller>().doors[index].GetComponent<doorBehavior>().close && !ctrlr.GetComponent<controller>().doors[index].GetComponent<doorBehavior>().moving)
                {
                    diffX = transform.position.x - termX[index];
                    ArmMove(0, diffX, 28);
                }
                break;
            case 28:// 待车辆离开后清除车模，恢复空闲，复位编号
                index = (terminalIndex - 1) % 6 + 1;
                // 机械手取车开门
                if (/*ctrlr.GetComponent<controller>().doorLock[index - 1] == 0 && */ctrlr.GetComponent<controller>().doors[index - 1].GetComponent<doorBehavior>().close && !ctrlr.GetComponent<controller>().doors[index - 1].GetComponent<doorBehavior>().moving)
                {
                    ctrlr.GetComponent<controller>().doors[index - 1].GetComponent<doorBehavior>().close = false;
                }
                // 移动车模到车库外
                if (myCar.transform.position.z > 8f)
                {
                    ctrlr.GetComponent<controller>().DestroyCar(myCar);
                    terminalIndex = -1;
                    state = 0;
                }else if (myCar.transform.position.z > 2.6f || (!ctrlr.GetComponent<controller>().doors[index - 1].GetComponent<doorBehavior>().close ))
                {
                    myCar.transform.Translate(Vector3.back * xSpeed * Time.deltaTime);
                }
                // 移动出去则关门
                if(myCar.transform.position.z > 2.6f)
                {
                    ctrlr.GetComponent<controller>().doors[index - 1].GetComponent<doorBehavior>().close = true;
                }
                break;

            default: break;
        }
    }
}