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