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); //绘出当前坐标点 //} }
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); //绘出当前坐标点 } } }