public void StartPVT(Motors motors, string adress,double unit,int timevalue1,int timevalue2)
{
#region
//计算轨迹位置，速度和时间间隔序列
//原始数据
string[] ral = File.ReadAllLines(adress, Encoding.Default); //相对目录是在bin/Debug下，所以要回溯到上两级目录
int lineCounter = ral.Length; //获取步态数据行数
string[] col = (ral[0] ?? string.Empty).Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
int colCounter = col.Length; //获取步态数据列数
double[,] pos0 = new double[lineCounter, colCounter]; //原始位置数据
double[,] vel = new double[lineCounter, colCounter]; //速度
int[] times = new int[lineCounter]; //时间间隔
for (int i = 0; i < lineCounter; i++)
{
if(i<lineCounter-300)
{
times[i] = timevalue1; //【设置】时间间隔
}
else
{
times[i] = timevalue2; //【设置】时间间隔
}
string[] str = (ral[i] ?? string.Empty).Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
for (int j = 0; j < colCounter; j++)
{
pos0[i, j] = double.Parse(str[j]) / (unit / motors.RATIO) * motors.userUnits[j] * -1;
}
}
times[lineCounter - 1] = 0;
#region
//一次扩充
//int richLine = lineCounter * 2 - 1;
//double[,] pos1 = new double[richLine, colCounter]; //一次扩充位置数据
//for (int i = 0; i < richLine; i++)
//{
// for (int j = 0; j < colCounter; j++)
// {
// if (i % 2 == 0)//偶数位存放原始数据
// {
// pos1[i, j] = pos0[i / 2, j];
// }
// else//奇数位存放扩充数据
// {
// pos1[i, j] = (pos0[i / 2 + 1, j] + pos0[i / 2, j]) / 2.0;
// }
// }
//}
////二次扩充
//int rich2Line = richLine * 2 - 1;
//double[,] pos2 = new double[rich2Line, colCounter]; //二次扩充位置数据
//double[,] vel = new double[rich2Line, colCounter]; //速度
//int[] times = new int[rich2Line]; //时间间隔
//for (int i = 0; i < rich2Line; i++)
//{
// times[i] =3; //【设置】时间间隔
// for (int j = 0; j < colCounter; j++)
// {
// if (i % 2 == 0)//偶数位存放原始数据
// {
// pos2[i, j] = pos1[i / 2, j];
// }
// else//奇数位存放扩充数据
// {
// pos2[i, j] = (pos1[i / 2 + 1, j] + pos1[i / 2, j]) / 2.0;
// }
// }
//}
//三次扩充
//int rich3Line = rich2Line * 2 - 1;
//double[,] pos3 = new double[rich2Line, colCounter]; //三次扩充位置数据
//for (int i = 0; i < rich3Line; i++)
//{
// for (int j = 0; j < colCounter; j++)
// {
// if (i % 2 == 0)//偶数位存放原始数据
// {
// pos3[i, j] = pos2[i / 2, j];
// }
// else//奇数位存放扩充数据
// {
// pos3[i, j] = (pos2[i / 2 + 1, j] + pos2[i / 2, j]) / 2.0;
// }
// //pos3[i, j] = pos3[i, j] - pos3[0, j];
// }
//}
#endregion
for (int i = 0; i < lineCounter - 1; i++)
{
for (int j = 0; j < colCounter; j++)
{
vel[i, j] = (pos0[i + 1, j] - pos0[i, j]) * 1000.0 / (times[i]);
}
}
vel[lineCounter - 1, 0] = 0;
vel[lineCounter - 1, 1] = 0;
vel[lineCounter - 1, 2] = 0;
vel[lineCounter - 1, 3] = 0;
#endregion
for (int i = 0; i < motors.motor_num; i++)//开始步态前各电机回到轨迹初始位置
{
motors.profileSettingsObj = motors.ampObj[i].ProfileSettings;
motors.profileSettingsObj.ProfileAccel = (motors.ampObj[i].VelocityLoopSettings.VelLoopMaxAcc);
motors.profileSettingsObj.ProfileDecel = (motors.ampObj[i].VelocityLoopSettings.VelLoopMaxDec);
motors.profileSettingsObj.ProfileVel = (motors.ampObj[i].VelocityLoopSettings.VelLoopMaxVel * 0.8);
motors.profileSettingsObj.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP; //PVT模式下的控制模式类型
motors.ampObj[i].ProfileSettings = motors.profileSettingsObj;
motors.ampObj[i].MoveAbs(pos0[0, i]); //PVT模式开始后先移动到各关节初始位置
motors.ampObj[i].WaitMoveDone(10000); //等待各关节回到初始位置的最大时间
}
motors.Linkage.TrajectoryInitialize(pos0, vel, times, 500); //开始步态
//motors.ampObj[0].WaitMoveDone(10000);
//motors.ampObj[1].WaitMoveDone(10000);
//motors.ampObj[2].WaitMoveDone(10000);
//motors.ampObj[3].WaitMoveDone(10000);
}
private const int PARA_NUM = 1; //每个电机写入参数个数
#endregion
public bool writeStart(StatusBar statusBarIn, TextBlock statusInfoTextBlockIn, Motors motorsIn)
{
statusBar = statusBarIn;
statusInfoTextBlock = statusInfoTextBlockIn;
motors = motorsIn;
ExcelApp = new Excel.Application();
if (ExcelApp == null)
{
statusBar.Background = new SolidColorBrush(Color.FromArgb(255, 230, 20, 20));
statusInfoTextBlock.Text = "该计算机可能未安装Excel！";
return(false);
}
ExcelApp.Visible = true;
ExcelApp.Application.DisplayAlerts = false;
ExcelWorkbooks = ExcelApp.Workbooks;
try
{
ExcelWorkbook = ExcelWorkbooks.Add(misValue);
ExcelWorksheet = (Excel.Worksheet)ExcelWorkbook.Worksheets["Sheet1"];
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 1], ExcelWorksheet.Cells[1, PARA_NUM]].Merge();
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 1], ExcelWorksheet.Cells[1, PARA_NUM]].Interior.ColorIndex = 39;
ExcelWorksheet.Cells[1, 1] = "电机1左膝";
ExcelWorksheet.Cells[2, 1] = "关节角度";
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, PARA_NUM + 1], ExcelWorksheet.Cells[1, 2 * PARA_NUM]].Merge();
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, PARA_NUM + 1], ExcelWorksheet.Cells[1, 2 * PARA_NUM]].Interior.ColorIndex = 35;
ExcelWorksheet.Cells[1, PARA_NUM + 1] = "电机2左髋";
ExcelWorksheet.Cells[2, PARA_NUM + 1] = "关节角度";
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 2 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 3 * PARA_NUM]].Merge();
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 2 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 3 * PARA_NUM]].Interior.ColorIndex = 37;
ExcelWorksheet.Cells[1, 2 * PARA_NUM + 1] = "电机3右髋";
ExcelWorksheet.Cells[2, 2 * PARA_NUM + 1] = "关节角度";
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 3 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 4 * PARA_NUM]].Merge();
ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 3 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 4 * PARA_NUM]].Interior.ColorIndex = 36;
ExcelWorksheet.Cells[1, 3 * PARA_NUM + 1] = "电机4右膝";
ExcelWorksheet.Cells[2, 3 * PARA_NUM + 1] = "关节角度";
excelCnt = 3;
timer = new DispatcherTimer();
timer.Interval = TimeSpan.FromMilliseconds(10);
timer.Tick += new EventHandler(writeExcel_Tick);
timer.Start();
}
catch
{
statusBar.Background = new SolidColorBrush(Color.FromArgb(255, 230, 20, 20));
statusInfoTextBlock.Text = "打开文件时发生错误！";
writeStop();
return(false);
}
return(true);
}
// 求取从初始位置到第一个位置的电机转角变化量的绝对值
public void start_Standup2(Motors motor)
{
double[] DeltaP1 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP1[i] = System.Math.Abs(KeyPos1[i] - 0);
}
//获取变化量的绝对值的最大值
double MaxDeltaP1 = DeltaP1.Max();
//设置各电机运动参数，并在位置模式下运动到第一个下蹲位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP1[i] / MaxDeltaP1; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos1[i]);
}
//cm.MotorAmp_Array[9].WaitMoveDone(100000);
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP2 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP2[i] = System.Math.Abs(KeyPos2[i] - KeyPos1[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP2 = DeltaP2.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP2[i] / MaxDeltaP2; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos2[i]);
}
//cm.MotorAmp_Array[9].WaitMoveDone(100000);
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP3 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP3[i] = System.Math.Abs(KeyPos3[i] - KeyPos2[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP3 = DeltaP3.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚落地位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP3[i] / MaxDeltaP3; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos3[i]);
}
//cm.MotorAmp_Array[9].WaitMoveDone(100000);
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP4 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP4[i] = System.Math.Abs(KeyPos4[i] - KeyPos3[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP4 = DeltaP4.Max();
//设置各电机运动参数,并在位置模式下左移重心
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP4[i] / MaxDeltaP4; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos4[i]);
}
//MotorAmp_Array[9].WaitMoveDone(100000);
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP5 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP5[i] = System.Math.Abs(KeyPos5[i] - KeyPos4[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP5 = DeltaP5.Max();
//设置各电机运动参数,并在位置模式下左移重心
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = 0 * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos5[i]);
}
Task task = Task.Run(() => {
Thread.Sleep(200);
});
task.Wait();
//Thread.Sleep(2000);
//motor.ampObj[3].WaitMoveDone(5000);
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP6 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP6[i] = System.Math.Abs(KeyPos6[i] - KeyPos5[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP6 = DeltaP6.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP6[i] / MaxDeltaP6; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos6[i]);
}
//cm.MotorAmp_Array[9].WaitMoveDone(100000);
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP7 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP7[i] = System.Math.Abs(KeyPos7[i] - KeyPos6[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP7 = DeltaP7.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP7[i] / MaxDeltaP7; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos7[i]);
}
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP8 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP8[i] = System.Math.Abs(KeyPos8[i] - KeyPos7[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP8 = DeltaP8.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP8[i] / MaxDeltaP8; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos8[i]);
}
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP9 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP9[i] = System.Math.Abs(KeyPos9[i] - KeyPos8[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP9 = DeltaP9.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP9[i] / MaxDeltaP9; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos9[i]);
}
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP10 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP10[i] = System.Math.Abs(KeyPos10[i] - KeyPos9[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP10 = DeltaP10.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP10[i] / MaxDeltaP10; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos10[i]);
}
//求取相对前一位置的电机转角变化量的绝对值
double[] DeltaP11 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP11[i] = System.Math.Abs(KeyPos11[i] - KeyPos10[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP11 = DeltaP11.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP11[i] / MaxDeltaP11; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos11[i]);
}
double[] DeltaP12 = new double[4];
for (int i = 0; i < motor.motor_num; i++)
{
DeltaP12[i] = System.Math.Abs(KeyPos12[i] - KeyPos11[i]);
}
//获取变化量的绝对值的最大值
double MaxDeltaP12 = DeltaP12.Max();
//设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
for (int i = 0; i < motor.motor_num; i++)
{
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP12[i] / MaxDeltaP12; //单位为°/s
profileParameters.ProfileAccel = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
profileParameters.ProfileDecel = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
profileParameters.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
motor.ampObj[i].ProfileSettings = profileParameters;
motor.ampObj[i].MoveAbs(KeyPos12[i]);
}
}
