Beispiel #1
0
        private void goWithPP2(Point res, Point des, ConPort myConPort, DrPort myDrPort, UrgPort myUrgPort)//点对点行走
        {
            // 设置行进方向最小速度和最大速速度
            int minGo = 10, minShift = 10, minRot = 2;
            int maxGo = 60, maxShift = 40, maxRot = 20;
            // 设置角度、距离误差范围
            double angRound = 0.008;
            double disRound = 0.0075;
            // 角度、距离比例控制
            double angP = 120;   // 10/0.008  = 250
            double disP = 650;   // 60/0.0075 = 13333
            // 当前位置点信息
            Point nowPos = myDrPort.getPoint();

            //
            // 行进控制部分开始
            //
            // 1.旋转AGV对准方向
            //
            if (Math.Abs(nowPos.w - des.w) > 4 * angRound)
            {
                while (Math.Abs(nowPos.w - des.w) > angRound)
                {
                    // 比例控制速度
                    int curRot = (int)(angP * (des.w - nowPos.w));
                    // 限制速度在合理范围
                    curRot = Navigation.getLimit(curRot, maxRot, minRot, true, true);
                    // 执行转弯
                    myConPort.controlDirectAGV(0, 0, curRot);
                    Thread.Sleep(100);
                    nowPos = myDrPort.getPoint();
                }
            }
            //
            int goSpeed = 0;
            int shSpeed = 0;

            if (Math.Abs(res.y - des.y) < Math.Abs(res.x - des.x))
            {
                while (Math.Abs(des.x - nowPos.x) > disRound)
                {
                    Navigation.getDefaultSpeed(res, des, nowPos, nowPos.w + Math.PI / 2, ref goSpeed, ref shSpeed);
                    myConPort.controlDirectAGV(goSpeed, shSpeed, 0);
                    Thread.Sleep(100);
                    nowPos = myDrPort.getPoint();
                }
            }
            else
            {
                while (Math.Abs(des.y - nowPos.y) > disRound)
                {
                    Navigation.getDefaultSpeed(res, des, nowPos, nowPos.w + Math.PI / 2, ref goSpeed, ref shSpeed);
                    myConPort.controlDirectAGV(goSpeed, shSpeed, 0);
                    Thread.Sleep(100);
                    nowPos = myDrPort.getPoint();
                }
            }


            /*
             * //Thread.Sleep(600);
             * // set speed value
             * int goSpeed = 0;
             * int shiftSpeed = 0;
             * int rotateSpeed = 3;
             * int setGo = 60;
             * int setShift = 40;
             * int setRot = 20;
             * // set begin point and now point
             * Point save = myDrPort.getPoint();
             * Point now = myDrPort.getPoint();
             *
             * Redraw_Curr_P(now);  //绘出当前坐标点
             * // first rotate to correct direction
             * // absolute
             *
             * // change des to res
             * while (Math.Abs(now.w - des.w) > 0.008) {
             *  int curRotSpeed = rotateSpeed;
             *  if (Math.Abs(now.w - des.w) < 0.1) {
             *      curRotSpeed = (int)(rotateSpeed * 0.2);
             *      if (curRotSpeed == 0) {
             *          curRotSpeed = 1;
             *      }
             *  }
             *  if (des.w > now.w) {
             *      myConPort.controlDirectAGV(goSpeed, shiftSpeed, curRotSpeed);
             *  }
             *  else {
             *      myConPort.controlDirectAGV(goSpeed, shiftSpeed, -curRotSpeed);
             *  }
             *  System.Threading.Thread.Sleep(100);
             *  now = myDrPort.getPoint();
             * }
             *
             * // second judge how to move to correct location
             * rotateSpeed = 0;
             * int direction = 1;
             * double tmpW = des.w + Math.PI / 2.0;
             * double length = Math.Sqrt(Math.Pow(res.x - des.x, 2) + Math.Pow(res.y - des.y, 2));
             * if (Math.Sqrt((Math.Pow(res.x + length * Math.Cos(tmpW) - des.x, 2))
             + Math.Pow(res.y + length * Math.Sin(tmpW) - des.y, 2)) < length) {
             +  goSpeed = setGo;
             +  direction = 1;
             + }
             + else if (Math.Sqrt((Math.Pow(res.x + length * Math.Cos(tmpW + Math.PI / 2) - des.x, 2))
             + Math.Pow(res.y + length * Math.Sin(tmpW + Math.PI / 2) - des.y, 2)) < length) {
             +  shiftSpeed = setShift;
             +  direction = 2;
             + }
             + else if (Math.Sqrt((Math.Pow(res.x + length * Math.Cos(tmpW - Math.PI / 2) - des.x, 2))
             + Math.Pow(res.y + length * Math.Sin(tmpW - Math.PI / 2) - des.y, 2)) < length) {
             +  shiftSpeed = -setShift;
             +  direction = 3;
             + }
             + else {
             +  goSpeed = -setGo;
             +  direction = 4;
             + }
             + // third move!
             + Thread.Sleep(1000);
             + double toMoveX = des.x - res.x;
             + double toMoveY = des.y - res.y;
             + // relative
             +
             + // absolute
             + int curGoSpeed = goSpeed;
             + int curShSpeed = shiftSpeed;
             + if (Math.Abs(toMoveX) < Math.Abs(toMoveY)) {
             +  while (Math.Abs(des.y - now.y) > 0.0075) {
             +      if (Math.Abs(des.y - now.y) < 0.08) {
             +          curGoSpeed = (int)(goSpeed * 0.3);
             +          curShSpeed = (int)(shiftSpeed * 0.3);
             +      }
             +      if (myUrgPort.CanGo()) {
             +          myConPort.controlDirectAGV(curGoSpeed, curShSpeed, rotateSpeed);
             +      }
             +      System.Threading.Thread.Sleep(100);
             +      now = myDrPort.getPoint();
             +      Redraw_Curr_P(now);  //绘出当前坐标点
             +  }
             +
             + }
             + else {
             +  while (Math.Abs(des.x - now.x) > 0.0075) {
             +      if (Math.Abs(des.x - now.x) < 0.08) {
             +          curGoSpeed = (int)(goSpeed * 0.3);
             +          curShSpeed = (int)(shiftSpeed * 0.3);
             +      }
             +      if (myUrgPort.CanGo()) {
             +          myConPort.controlDirectAGV(curGoSpeed, curShSpeed, rotateSpeed);
             +      }
             +      System.Threading.Thread.Sleep(100);
             +      now = myDrPort.getPoint();
             +      Redraw_Curr_P(now);  //绘出当前坐标点
             +  }
             + }
             * */
        }
Beispiel #2
0
        private void goWithRadar(Point res, Point des, ConPort myConPort, DrPort myDrPort, UrgPort myUrgPort, int speed)//激光雷达行走
        {
            double toMove = 0;
            bool   flagX  = false;
            bool   flagY  = false;
            Point  start  = myDrPort.getPoint();
            Point  now    = myDrPort.getPoint();

            Redraw_Curr_P(now);  //绘出当前坐标点
            if (Math.Abs(des.x - res.x) < Math.Abs(des.y - res.y))
            {
                toMove = des.y - res.y;
                flagY  = true;
            }
            else
            {
                toMove = des.x - res.x;
                flagX  = true;
            }
            while ((flagX && Math.Abs(toMove - (now.x - start.x)) > 0.1) ||
                   (flagY && Math.Abs(toMove - (now.y - start.y)) > 0.1))
            {
                myUrgPort.GetAngleAndDis();
                // add 20151113 for stop directly
                if (myUrgPort.CanGo())
                {
                    myConPort.UrgControl(speed, 90 - myUrgPort.angle, 0 - myUrgPort.dis);
                }
                System.Threading.Thread.Sleep(100);
                now = myDrPort.getPoint();
                Redraw_Curr_P(now);  //绘出当前坐标点
            }

            //while (Math.Abs(now.w - des.w) > 0.01)
            //{
            //    int rotateS = 40;
            //    if (des.w > now.w)
            //    {
            //        myConPort.controlDirectAGV(0, 0, rotateS);
            //    }
            //    else
            //    {
            //        myConPort.controlDirectAGV(0, 0, -rotateS);
            //    }
            //    System.Threading.Thread.Sleep(100);
            //    now = myDrPort.getPoint();
            //    Redraw_Curr_P(now);  //绘出当前坐标点
            //}
        }
Beispiel #3
0
        private void goWithPP(Point res, Point des, ConPort myConPort, DrPort myDrPort, UrgPort myUrgPort)//点对点行走
        {
            //Thread.Sleep(600);
            // set speed value
            int goSpeed     = 0;
            int shiftSpeed  = 0;
            int rotateSpeed = 3;
            int setGo       = 60;
            int setShift    = 40;
            int setRot      = 20;
            // set begin point and now point
            Point save = myDrPort.getPoint();
            Point now  = myDrPort.getPoint();

            //if (res.x == 0 && res.y == 0) {
            //    now.w = 0;
            //    des.w = 0;
            //}
            Redraw_Curr_P(now);  //绘出当前坐标点
            // first rotate to correct direction
            // relative
            //double toRotate = des.w - res.w;
            //while (Math.Abs((now.w - save.w) - toRotate) > 0.01){
            //    if (toRotate > 0){
            //        myConPort.controlDirectAGV(goSpeed, shiftSpeed, rotateSpeed);
            //    }
            //    else{
            //        myConPort.controlDirectAGV(goSpeed, shiftSpeed, -rotateSpeed);
            //    }
            //    System.Threading.Thread.Sleep(100);
            //    now = myDrPort.getPoint();
            //    Redraw_Curr_P(now);  //绘出当前坐标点
            //}
            // absolute

            // change des to res
            while (Math.Abs(now.w - des.w) > 0.008)
            {
                int curRotSpeed = rotateSpeed;
                if (Math.Abs(now.w - des.w) < 0.1)
                {
                    curRotSpeed = (int)(rotateSpeed * 0.2);
                    if (curRotSpeed == 0)
                    {
                        curRotSpeed = 1;
                    }
                }
                if (des.w > now.w)
                {
                    myConPort.controlDirectAGV(goSpeed, shiftSpeed, curRotSpeed);
                }
                else
                {
                    myConPort.controlDirectAGV(goSpeed, shiftSpeed, -curRotSpeed);
                }
                System.Threading.Thread.Sleep(100);
                now = myDrPort.getPoint();
            }

            // second judge how to move to correct location
            rotateSpeed = 0;
            double tmpW   = des.w + Math.PI / 2.0;
            double length = Math.Sqrt(Math.Pow(res.x - des.x, 2) + Math.Pow(res.y - des.y, 2));

            if (Math.Sqrt((Math.Pow(res.x + length * Math.Cos(tmpW) - des.x, 2))
                          + Math.Pow(res.y + length * Math.Sin(tmpW) - des.y, 2)) < length)
            {
                goSpeed = setGo;
            }
            else if (Math.Sqrt((Math.Pow(res.x + length * Math.Cos(tmpW + Math.PI / 2) - des.x, 2))
                               + Math.Pow(res.y + length * Math.Sin(tmpW + Math.PI / 2) - des.y, 2)) < length)
            {
                shiftSpeed = setShift;
            }
            else if (Math.Sqrt((Math.Pow(res.x + length * Math.Cos(tmpW - Math.PI / 2) - des.x, 2))
                               + Math.Pow(res.y + length * Math.Sin(tmpW - Math.PI / 2) - des.y, 2)) < length)
            {
                shiftSpeed = -setShift;
            }
            else
            {
                goSpeed = -setGo;
            }
            // third move!
            Thread.Sleep(1000);
            double toMoveX = des.x - res.x;
            double toMoveY = des.y - res.y;
            // relative
            //if (Math.Abs(toMoveX) < Math.Abs(toMoveY)) {
            //    while (Math.Abs(now.y - save.y - toMoveY) > 0.05) {
            //        myUrgPort.GetAngleAndDis();
            //        if (myUrgPort.CanGo()) {
            //            myConPort.controlDirectAGV(goSpeed, shiftSpeed, rotateSpeed);
            //        }
            //        myConPort.controlDirectAGV(goSpeed, shiftSpeed, rotateSpeed);
            //        System.Threading.Thread.Sleep(100);
            //        now = myDrPort.getPoint();
            //    }
            //} else {
            //    while (Math.Abs(now.x - save.x - toMoveX) > 0.05) {
            //        myUrgPort.GetAngleAndDis();
            //        if (myUrgPort.CanGo()) {
            //            myConPort.controlDirectAGV(goSpeed, shiftSpeed, rotateSpeed);
            //        }
            //        myConPort.controlDirectAGV(goSpeed, shiftSpeed, rotateSpeed);
            //        System.Threading.Thread.Sleep(100);
            //        now = myDrPort.getPoint();
            //    }
            //}

            // absolute
            int curGoSpeed = goSpeed;
            int curShSpeed = shiftSpeed;

            if (Math.Abs(toMoveX) < Math.Abs(toMoveY))
            {
                while (Math.Abs(des.y - now.y) > 0.0075)
                {
                    if (Math.Abs(des.y - now.y) < 0.08)
                    {
                        curGoSpeed = (int)(goSpeed * 0.3);
                        curShSpeed = (int)(shiftSpeed * 0.3);
                    }
                    if (myUrgPort.CanGo())
                    {
                        myConPort.controlDirectAGV(curGoSpeed, curShSpeed, rotateSpeed);
                    }
                    System.Threading.Thread.Sleep(100);
                    now = myDrPort.getPoint();
                    Redraw_Curr_P(now);  //绘出当前坐标点
                }
            }
            else
            {
                while (Math.Abs(des.x - now.x) > 0.0075)
                {
                    if (Math.Abs(des.x - now.x) < 0.08)
                    {
                        curGoSpeed = (int)(goSpeed * 0.3);
                        curShSpeed = (int)(shiftSpeed * 0.3);
                    }
                    if (myUrgPort.CanGo())
                    {
                        myConPort.controlDirectAGV(curGoSpeed, curShSpeed, rotateSpeed);
                    }
                    System.Threading.Thread.Sleep(100);
                    now = myDrPort.getPoint();
                    Redraw_Curr_P(now);  //绘出当前坐标点
                }
            }
        }