private void GetBackInfo(ref int shiftSpeed, ref int rotatSpeed, IConPort myconport)
{
SonicModel sm = myconport.Measure_Sonic();
int right = sm.S[4];
int left = sm.S[7];
//if (right < 80) right = 230;
//if (left < 80) left = 230;
double angle;
double dis;
double length = right + left + 400;//左右距离+车宽
if (right > 350 && left > 350)
{
rotatSpeed = 0;
shiftSpeed = 0;
}
else if (right > 350)
{
shiftSpeed = (int)(0.2 * (200 - left));
rotatSpeed = (int)(3 * (200 - left));
}
else if (left > 350)
{
shiftSpeed = (int)(0.2 * (right - 200));
rotatSpeed = (int)(3 * (right - 200));
}
else
{
if (length < 800)
{
angle = 0;
}
else
{
angle = Math.Acos(800 / length);
}
if (right >= left)
{
rotatSpeed = (int)(5 * (angle * 180 / 3.14159));
}
else if (right < left)
{
rotatSpeed = -(int)(5 * (angle * 180 / 3.14159));
}
if (length > 800)
{
shiftSpeed = (int)(0.2 * (right - left));
}
}
}
public SonicModel Measure_Sonic()
{
// 等待设置完毕
while (config.IsSetting)
{
;
}
config.IsGetting = true;
// 填充数据
int[] temp = new int[8];
for (int i = 0; i < 8; i++)
{
temp[i] = config.UltraSonic[i];
}
SonicModel sonicData = new SonicModel(temp);
// 关闭等待
config.IsGetting = false;
return(sonicData);
}
private int getRotateSpeed(IConPort conPort, IUrgPort urgPort, IDrPort drPort)
{
List <CONFIG.URG_POINT> pointsH = getUrgPoint(85, 95, urgPort);
double minH = double.MaxValue;
for (int i = 0; i < pointsH.Count; i++)
{
double y = pointsH[i].y;
if (y < minH)
{
minH = y;
}
}
if (minH < 1000)
{
return(0);
}
// 取点
List <CONFIG.URG_POINT> pointsL = getUrgPoint(120, 180, urgPort);
List <CONFIG.URG_POINT> pointsR = getUrgPoint(0, 60, urgPort);
// 交换并取绝对坐标
for (int i = 0; i < pointsL.Count; i++)
{
CONFIG.URG_POINT point = pointsL[i];
double tempx = Math.Abs(point.x);
double tempy = Math.Abs(point.y);
point.x = tempy; point.y = tempx; pointsL[i] = point;
}
for (int i = 0; i < pointsR.Count; i++)
{
CONFIG.URG_POINT point = pointsR[i];
double tempx = Math.Abs(point.x);
double tempy = Math.Abs(point.y);
point.x = tempy; point.y = tempx; pointsR[i] = point;
}
// 一米之内
for (int i = pointsL.Count - 1; i >= 0; i--)
{
if (pointsL[i].y > 1000)
{
pointsL.RemoveAt(i);
}
}
for (int i = pointsR.Count - 1; i >= 0; i--)
{
if (pointsR[i].y > 1000)
{
pointsR.RemoveAt(i);
}
}
// 拟合左右两边障碍物信息
pointsL = SortPoints(pointsL);
pointsR = SortPoints(pointsR);
pointsL = getFitPoints(pointsL);
pointsR = getFitPoints(pointsR);
double[] KAB_L = getFitLine(pointsL);
double[] KAB_R = getFitLine(pointsR);
// 点数量不够
bool acceptL = pointsL.Count > 10;
bool acceptR = pointsR.Count > 10;
if (!acceptL && !acceptR)
{
return(0);
}
// 控制策略
int RotateSpeed = 0;
if (acceptL && acceptR)
{
double current = 0;
double target = (KAB_L[1] - KAB_R[1]) / 2;
RotateSpeed = (int)PDcontroller(current, target, ref config.PD_R);
}
if (acceptL && !acceptR)
{
double current = 0;
double target = KAB_L[1];
RotateSpeed = (int)PDcontroller(current, target, ref config.PD_R);
}
if (!acceptL && acceptR)
{
double current = 0;
double target = KAB_R[1];
RotateSpeed = -(int)PDcontroller(current, target, ref config.PD_R);
}
// 判断是否允许旋转
double permitRotateDistance = 100; //100
SonicModel sonic = conPort.Measure_Sonic();
//foreach (int s in sonic.S) { if (s < permitRotateDistance) { return 0; } }
// 限速
if (RotateSpeed > config.MaxRotateSpeed)
{
return(config.MaxRotateSpeed);
}
if (RotateSpeed < -config.MaxRotateSpeed)
{
return(-config.MaxRotateSpeed);
}
return(RotateSpeed);
}
private int getTranslateSpeed(double keepLeft, IConPort conPort, IUrgPort urgPort, IDrPort drPort)
{
// 取数据
SonicModel sonic = conPort.Measure_Sonic();
double distanceL = 0, distanceR = 0;
if (sonic.S[0] < 1000)
{
distanceL = sonic.S[0];
}
if (sonic.S[7] < 1000)
{
if (distanceL == 0)
{
distanceL = sonic.S[7];
}
else
{
distanceL = Math.Min(distanceL, sonic.S[7]);
}
}
if (sonic.S[3] < 1000)
{
distanceR = sonic.S[3];
}
if (sonic.S[4] < 1000)
{
if (distanceR == 0)
{
distanceR = sonic.S[4];
}
else
{
distanceR = Math.Min(distanceR, sonic.S[4]);
}
}
if (distanceL != 0)
{
distanceL += 225;
}
if (distanceR != 0)
{
distanceR += 225;
}
if (distanceL == 0)
{
distanceL = double.MaxValue;
}
if (distanceR == 0)
{
distanceR = double.MaxValue;
}
// 不适用超声波数据
distanceL = double.MaxValue;
distanceR = double.MaxValue;
// 加点前瞻
List <CONFIG.URG_POINT> pointsL = getUrgPoint(120, 180, urgPort);
List <CONFIG.URG_POINT> pointsR = getUrgPoint(0, 60, urgPort);
double minL = double.MaxValue, minR = double.MaxValue;
for (int i = 0; i < pointsL.Count; i++)
{
double x = Math.Abs(pointsL[i].x);
if (x < minL)
{
minL = x;
}
}
for (int i = 0; i < pointsR.Count; i++)
{
double x = Math.Abs(pointsR[i].x);
if (x < minR)
{
minR = x;
}
}
distanceL = Math.Min(distanceL, minL);
distanceR = Math.Min(distanceR, minR);
if (distanceL == 0 && distanceR == 0)
{
return(0);
}
// 判断通道宽度
double AisleWidth = 800;
// 获取控制
int TranslateSpeed = 0;
// 最短距离模式
double acceptDistance = 320;
bool acceptL = distanceL < acceptDistance;
bool acceptR = distanceR < acceptDistance;
if (acceptL && acceptR)
{
double current = distanceL;
double target = (distanceL + distanceR) / 2;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (acceptL && !acceptR)
{
double current = distanceL;
double target = (distanceL < AisleWidth) ? (distanceL + distanceR) : 180; //Math.Abs(keepLeft);
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (!acceptL && acceptR)
{
double current = distanceR;
double target = (distanceL < AisleWidth) ? (distanceL + distanceR) : 180; //Math.Abs(keepLeft);
TranslateSpeed = -(int)PDcontroller(current, target, ref config.PD_T);
}
// 限速
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
if (acceptL || acceptR)
{
return(TranslateSpeed);
}
// 数据能否使用
if (0 < distanceL && distanceL < AisleWidth)
{
acceptL = true;
}
if (0 < distanceR && distanceR < AisleWidth)
{
acceptR = true;
}
// 数据无效模式
if (!acceptL && !acceptR)
{
return(0);
}
if (!acceptL)
{
double current = distanceR;
double target = AisleWidth / 2;
TranslateSpeed = -(int)PDcontroller(current, target, ref config.PD_T);
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
return(TranslateSpeed);
}
if (!acceptR)
{
double current = distanceL;
double target = AisleWidth / 2;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
return(TranslateSpeed);
}
// 左中右模式
if (keepLeft == 0)
{
double current = distanceL;
double target = (distanceL + distanceR) / 2;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (keepLeft > 0)
{
double current = distanceL;
double target = keepLeft;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (keepLeft < 0)
{
double current = distanceR;
double target = -keepLeft;
TranslateSpeed = -(int)PDcontroller(current, target, ref config.PD_T);
}
// 限速
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
return(TranslateSpeed);
}