private void RunZ_Click(object sender, EventArgs e)
{
//搜索点
double radius = Convert.ToDouble(Radius.Text);
double stepLength = Convert.ToDouble(StepLength.Text);
double StartLineY = Convert.ToDouble((radius * 2) % stepLength);
int InterSpaceCount =int.Parse( ((radius*2) / stepLength).ToString() );
double y;
double bottomLength;
List<double[]> XYPoint = new List<double[]>();
// List<PointF> pt = new List<PointF>();
//运动控制,采用点位运动的形式
Axis = 1;
rtn = GT_ClrSts(carNumber, Axis, 8);//清除1到8轴状态
TTrapPrm trap = new TTrapPrm();
//获取参数
double acc = Convert.ToDouble(Acceleration.Text);
double dec = Convert.ToDouble(Deceleration.Text);
short SmoothTime = Convert.ToInt16(SmoothingTime.Text);
//设置参数
trap.acc = acc;
trap.dec = dec;
trap.smoothTime = SmoothTime;
trap.velStart = 10;
// X轴
rtn = GT_SetTrapPrm(carNumber, Axis, ref trap);
CommandPrint("GT_SetTrapPrm", rtn);
// Y轴
rtn = GT_SetTrapPrm(carNumber, (short)(Axis+1), ref trap);
CommandPrint("GT_SetTrapPrm", rtn);
//设置XY轴目标速度
double vel = Convert.ToDouble(Speed.Text);
rtn = GT_SetVel(carNumber, Axis, vel);
CommandPrint("GT_SetVel", rtn);
rtn = GT_SetVel(carNumber, (short)(Axis + 1), vel);
CommandPrint("GT_SetVel", rtn);
//搜索点
for (int i = 0; i < InterSpaceCount; i++)
{
double[] site = new double[2];
double[] sites = new double[2];
if (StartLineY == 0)
{
site[0] = 0;
site[1] = 0;
XYPoint.Add(site);
// StartLineY += InterSpace;
StartLineY += stepLength;
}
else
{
if (StartLineY < stepLength)
{
StartLineY = stepLength;
}
y = StartLineY - radius;
bottomLength = Math.Sqrt((radius * radius) - (y * y));
bottomLength = Math.Round(bottomLength, 3);
Console.WriteLine($"bottomLength: {bottomLength}");
site[0] = bottomLength;
site[1] = y;
XYPoint.Add(site);
sites[0] = bottomLength;
sites[1] = y;
XYPoint.Add(sites);
// StartLineY += InterSpace;
StartLineY += stepLength;
if (i % 2 == 0)
{
Console.WriteLine($"i: {i}");
//位置2,X轴
rtn = GT_SetPos(carNumber, Axis,(int)XYPoint[XYPoint.Count - 1][0]);
Console.WriteLine($"X轴移动到位置2: {(int)XYPoint[XYPoint.Count - 1][0]}");
//启动点位运动
rtn = GT_Update(carNumber, 1 << (Axis - 1));
CommandPrint("X轴GT_Update", rtn);
//位置2,Y轴
rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 1][1]);
Console.WriteLine($"Y轴移动到位置2: {(int)XYPoint[XYPoint.Count - 1][1]}");
rtn = GT_Update(carNumber, 1 << (Axis));
CommandPrint("Y轴GT_Update", rtn);
MoveDetect((int)XYPoint[XYPoint.Count - 1][0], (int)XYPoint[XYPoint.Count - 1][1]);
//位置1,X轴
rtn = GT_SetPos(carNumber, Axis, (int)XYPoint[XYPoint.Count - 2][0]);
Console.WriteLine($"X轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][0]}");
rtn = GT_Update(carNumber, 1 << (Axis - 1));
//位置1,Y轴
rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 2][1]);
Console.WriteLine($"Y轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][1]}");
rtn = GT_Update(carNumber, 1 << (Axis));
CommandPrint("Y轴GT_Update", rtn);
MoveDetect((int)XYPoint[XYPoint.Count - 2][0], (int)XYPoint[XYPoint.Count - 2][1]);
}
else
{
Console.WriteLine($"i: {i}");
//位置1,X轴
rtn = GT_SetPos(carNumber, Axis, (int)XYPoint[XYPoint.Count - 2][0]);
Console.WriteLine($"X轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][0]}");
//启动点位运动
rtn = GT_Update(carNumber, 1 << (Axis - 1));
CommandPrint("X轴GT_Update", rtn);
//位置1,Y轴
rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 2][1]);
Console.WriteLine($"Y轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][1]}");
rtn = GT_Update(carNumber, 1 << (Axis));
CommandPrint("Y轴GT_Update", rtn);
MoveDetect((int)XYPoint[XYPoint.Count - 2][0], (int)XYPoint[XYPoint.Count - 2][1]);
//位置2,X轴
rtn = GT_SetPos(carNumber, Axis, (int)XYPoint[XYPoint.Count - 1][0]);
Console.WriteLine($"X轴移动到位置2: { (int)XYPoint[XYPoint.Count - 1][0]}");
//启动点位运动
rtn = GT_Update(carNumber, 1 << (Axis - 1));
CommandPrint("X轴GT_Update", rtn);
//位置2,Y轴
rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 1][1]);
Console.WriteLine($"Y轴移动到位置2: { (int)XYPoint[XYPoint.Count - 1][1]}");
rtn = GT_Update(carNumber, 1 << (Axis));
CommandPrint("Y轴GT_Update", rtn);
MoveDetect((int)XYPoint[XYPoint.Count - 1][0], (int)XYPoint[XYPoint.Count - 1][1]);
}
}
}
}
使用固高运动卡在圆形里走倒T型运动
2024-07-22 17:16:04 17 阅读