private void moveMotor(int speed, int acceleration, int distance)
{
double vel, accel, dist;
// test speed
vel = Convert.ToDouble(speed + 10) * 10;
accel = Convert.ToDouble(acceleration) * 100;
dist = Convert.ToDouble(distance) * 10;
ProfileSettings = xAxisAmp.ProfileSettings; // read profile settings from amp
//Set the profile type for move
ProfileSettings.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
//Set the profile trajectory values
ProfileSettings.ProfileAccel = accel;
ProfileSettings.ProfileDecel = accel;
ProfileSettings.ProfileVel = vel;
// Update the amplier's profile settigns
xAxisAmp.ProfileSettings = ProfileSettings;
xAxisAmp.Enable(); // Enable Motor
xAxisAmp.MoveAbs(dist); // Execute the move
}
public void motors_Init() //电机初始化
{
canObj = new canOpenObj(); //实例化网络接口
profileSettingsObj = new ProfileSettingsObj(); //实例化驱动器属性
canObj.BitRate = CML_BIT_RATES.BITRATE_1_Mbit_per_sec; //设置CAN传输速率为1M/s
canObj.Initialize(); //网络接口初始化
ampObj = new AmpObj[MOTOR_NUM]; //实例化四个驱动器（盘式电机）
ampsettingsObj = new ampSettingsObj();
ampsettingsObj.enableOnInit = true;
ampsettingsObj.guardTime = 1000;
ampsettingsObj.lifeFactor = 1000;
for (int i = 0; i < MOTOR_NUM; i++) //初始化四个驱动器
{
ampObj[i] = new AmpObj();
//ampObj[i].Initialize(canObj, (short)(i + 1));
ampObj[i].InitializeExt(canObj, (short)(i + 1), ampsettingsObj);
ampObj[i].HaltMode = CML_HALT_MODE.HALT_DECEL; //选择通过减速来停止电机的方式
ampObj[i].CountsPerUnit = 1; //电机转一圈编码器默认计数25600次，设置为4后转一圈计数6400次
userUnits[i] = ampObj[i].MotorInfo.ctsPerRev / ampObj[i].CountsPerUnit; //用户定义单位，counts/圈
}
Linkage = new LinkageObj();
Linkage.Initialize(ampObj);
Linkage.SetMoveLimits(200000, 3000000, 3000000, 200000);
motorsTimer = new DispatcherTimer();
motorsTimer.Tick += new EventHandler(motorsTimer_Tick); //Tick是超过计时器间隔时发生事件，此处为Tick增加了一个叫ShowCurTimer的取当前时间并扫描串口的委托
motorsTimer.Interval = TimeSpan.FromMilliseconds(100);; //设置刻度之间的时间值，设定为1秒（即文本框会1秒改变一次输出文本）
motorsTimer.Start();
}
///*/
private void doMoveButton_Click(object sender, EventArgs e)
{
try
{
Double Distance;
Distance = Convert.ToDouble(DistanceTextBox.Text);
ProfileSettings = xAxisAmp.ProfileSettings; // read profile settings from amp
//Set the profile type for move
ProfileSettings.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
//Set the profile trajectory values
ProfileSettings.ProfileAccel = Convert.ToDouble(AccelTextBox.Text);
ProfileSettings.ProfileDecel = Convert.ToDouble(DecelTextBox.Text);
ProfileSettings.ProfileVel = Convert.ToDouble(VelocityTextBox.Text);
// Update the amplier's profile settigns
xAxisAmp.ProfileSettings = ProfileSettings;
// Execute the move
xAxisAmp.MoveRel(Distance);
}
catch (Exception ex)
{
doMoveButton.Enabled = true;
HomeAxisButton.Enabled = true;
DisplayError(ex);
}
}
public void start_Sitdown2(Motors motor) // 位置模式控制(非PVT模式)执行坐下动作
{
#region
double[,] KeyPos = { { 5, -20, 20, -5 },
{ 14, -52.7, 52.7, -14 },
{ 30.7, -78.5, 78.5, -30.7 },
{ 64, -102.8, 102.8, -64 },
{ 87, -120, 120, -87 },
{ 91, -113, 113, -91 },
{ 100, -90, 90, -100 } }; // 坐下动作的位置点(4列)（单位：°）
double[,] KeyPos_s = new double[7, 4]; // 对KeyPos角度值的编码值
#endregion
ProfileSettingsObj profileParameters = new ProfileSettingsObj(); //用于设置电机参数
//double MotorVelocity = 230; // 电机速度(越大越快)
//double MotorAcceleration = 100; // 电机加速度(越大越快)
//double MotorDeceleration = 100; // 电机减速度(越大越快)
double MotorVelocity = 280; // 电机速度(越大越快)
double MotorAcceleration = 150; // 电机加速度(越大越快)
double MotorDeceleration = 150; // 电机减速度(越大越快)
double[,] DeltaP = new double[7, 4];
for (int s = 0; s < 7; s++)
{
angletocoder.AngleToCoder((KeyPos[s, 0].ToString()), (KeyPos[s, 1].ToString()), (KeyPos[s, 2].ToString()), (KeyPos[s, 3].ToString()), ref KeyPos_s[s, 0], ref KeyPos_s[s, 1], ref KeyPos_s[s, 2], ref KeyPos_s[s, 3]);
for (int j = 0; j < motor.motor_num; j++) // 求每个坐下动作位置点的差值(点与点之间的路程)
{
if (s == 0)
{
DeltaP[s, j] = Math.Abs(KeyPos_s[s, j] - 0);
}
else
{
DeltaP[s, j] = Math.Abs(KeyPos_s[s, j] - KeyPos_s[s - 1, j]);
}
}
for (int i = 0; i < motor.motor_num; i++) // 求每个电机坐下动作位置点间差值的最大值（以便后续归一化）
{
double MaxDeltaP = DeltaP[s, 0];
if (MaxDeltaP < DeltaP[s, i])
{
MaxDeltaP = DeltaP[s, i];
}
//profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 640 * 4 * 160 / 360 * DeltaP[s, i] / MaxDeltaP; //单位为°/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(KeyPos_s[s, i]); // 位置控制模式，让电机按以上速度，加速度移动到KeyPos_s的位置
}
}
//motors.Linkage.WaitMoveDone(10000);
motor.ampObj[0].WaitMoveDone(10000);
motor.ampObj[1].WaitMoveDone(10000);
motor.ampObj[2].WaitMoveDone(10000);
motor.ampObj[3].WaitMoveDone(10000);
}
public void motors_Init()//电机初始化
{
try
{
canObj = new canOpenObj(); //实例化网络接口
profileSettingsObj = new ProfileSettingsObj(); //实例化驱动器属性
//canObj.BitRate = CML_BIT_RATES.BITRATE_500_Kbit_per_sec;
canObj.BitRate = CML_BIT_RATES.BITRATE_1_Mbit_per_sec; //设置CAN传输速率为1M/s
canObj.Initialize(); //网络接口初始化
ampObj = new AmpObj[MOTOR_NUM]; //实例化四个驱动器（盘式电机）
ampsetingsObj = new ampSettingsObj();
ampsetingsObj.enableOnInit = true;
ampsetingsObj.guardTime = 2000; // 防止时间过长工控机电机失能
//ampsetingsObj.heartbeatPeriod = 1000;
//ampsetingsObj.heartbeatPeriod = 300;
ampsetingsObj.lifeFactor = 1000;
for (int i = 0; i < MOTOR_NUM; i++)//初始化四个驱动器
{
ampObj[i] = new AmpObj();
//ampObj[i].Initialize(canObj, (short)(i + 1));
ampObj[i].InitializeExt(canObj, (short)(i + 1), ampsetingsObj);
ampObj[i].HaltMode = CML_HALT_MODE.HALT_DECEL; //选择通过减速来停止电机的方式
ampObj[i].CountsPerUnit = 1; //电机转一圈编码器默认计数25600次，设置为4后转一圈计数6400次
userUnits[i] = ampObj[i].MotorInfo.ctsPerRev / ampObj[i].CountsPerUnit; //用户定义单位，counts/圈
}
//ampObj[0].PositionActual = -1;
//ampObj[1].PositionActual = -2;
//ampObj[2].PositionActual = -2;
//ampObj[3].PositionActual = -1;
Linkage = new LinkageObj();
linkageSettingsObj = new LinkageSettingsObj();
linkageSettingsObj.moveAckTimeout = 2000; // 代替WaitMoveDone
//Linkage.Initialize(ampObj);
Linkage.InitializeExt(ampObj, linkageSettingsObj); //电机联动初始化
Linkage.SetMoveLimits(200000, 3000000, 3000000, 200000); // 设置电机速度，加速度等极限值
EventObj = Linkage.CreateEvent(CML_LINK_EVENT.LINKEVENT_TRJDONE | CML_LINK_EVENT.LINKEVENT_MOVEDONE | CML_LINK_EVENT.LINKEVENT_ABORT, CML_EVENT_CONDITION.CML_EVENT_ANY);
EventObj.EventNotify += new eventObj.EventHandler(EventObj_EventNotify);
motorsTimer = new DispatcherTimer();
motorsTimer.Tick += new EventHandler(motorsTimer_Tick); //Tick是超过计时器间隔时发生事件，此处为Tick增加了一个叫ShowCurTimer的取当前时间并扫描串口的委托
motorsTimer.Interval = TimeSpan.FromMilliseconds(100);; //设置刻度之间的时间值，设定为1秒（即文本框会1秒改变一次输出文本）
motorsTimer.Start();
}
catch (Exception)
{
MessageBox.Show("驱动器初始化错误");
}
}
public CopleyAmpC()
{
connecter = CopleyConnecter.GetInstance();
motor = new CopleyAmpObj();
home = new HomeSettingsObj();
proJog = new ProfileSettingsObj();
e = new CML_AMP_EVENT();
status = new CML_EVENT_STATUS();
param = new CopleyAxisParams();
err = new CML_AMP_FAULT();
}
public void start_Sitdown2(Motors motor)
{
#region
double[,] KeyPos = { { -56888.88889, 227555.5556, -227555.5556, 56888.88889 },
{ -159288.8889, 599608.8889, -599608.8889, 159288.8889 },
{ -349297.7778, 893155.5556, -893155.5556, 349297.7778 },
{ -728177.7778, 1169635.556, -1169635.556, 728177.7778 },
{ -989866.6667, 1365333.333, -1365333.333, 989866.6667 },
{ -1035377.778, 1285688.889, -1285688.889, 1035377.778 },
{ -1137777.778, 1196942.222, -1196942.222, 1137777.778 } };
#endregion
ProfileSettingsObj profileParameters = new ProfileSettingsObj(); //用于设置电机参数
double MotorVelocity = 72;
double MotorAcceleration = 20;
double MotorDeceleration = 20;
double[,] DeltaP = new double[7, 4];
for (int s = 0; s < 7; s++)
{
for (int j = 0; j < motor.motor_num; j++)
{
if (s == 0)
{
DeltaP[s, j] = Math.Abs(KeyPos[s, j] - 0);
}
else
{
DeltaP[s, j] = Math.Abs(KeyPos[s, j] - KeyPos[s - 1, j]);
}
}
for (int i = 0; i < motor.motor_num; i++)
{
double MaxDeltaP = DeltaP[s, 0];
if (MaxDeltaP < DeltaP[s, i])
{
MaxDeltaP = DeltaP[s, i];
}
profileParameters = motor.ampObj[i].ProfileSettings;
profileParameters.ProfileVel = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP[s, i] / MaxDeltaP; //单位为°/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(KeyPos[s, i]);
}
}
}
private void moveMotor(int velocity, int acceleration, int distance)
{
double vel, accel, dist;
vel = Convert.ToDouble(velocity) * 1000;
accel = Convert.ToDouble(acceleration) * 1000;
dist = Convert.ToDouble(distance) * 1000;
ProfileSettings = xAxisAmp.ProfileSettings; // read profile settings from amp
//Set the profile type for move
ProfileSettings.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP;
//Set the profile trajectory values
ProfileSettings.ProfileAccel = accel;
ProfileSettings.ProfileDecel = accel;
ProfileSettings.ProfileVel = vel;
// Update the amplier's profile settigns
xAxisAmp.ProfileSettings = ProfileSettings;
// Execute the move
xAxisAmp.MoveRel(dist);
}
// 求取从初始位置到第一个位置的电机转角变化量的绝对值
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(1000);
});
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]);
}
}
public void Form1_Load(object sender, EventArgs e)
{
try
{
//Initialize code here
xAxisAmp = new AmpObj();
ProfileSettings = new ProfileSettingsObj();
//***************************************************
//
// CANOpen Network
//
//***************************************************
canOpen = new canOpenObj();
//
//**************************************************************************
//* The next two lines of code are optional. If no bit rate is specified,
//* then the default bit rate (1 Mbit per second) is used. If no port name
//* is specified, then CMO will use the first supported CAN card found and
//* use channel 0 of that card.
//**************************************************************************
// Set the bit rate to 1 Mbit per second
canOpen.BitRate = CML_BIT_RATES.BITRATE_1_Mbit_per_sec;
// Indicate that channel 0 of a Copley CAN card should be used
canOpen.PortName = "copley1";
//// Indicate that channel 0 of a KVaser card should be used
//canOpen.PortName = "kvaser0";
//// Indicate that channel 0 of an IXXAT card should be used
//canOpen.PortName = "IXXAT0";
//// Indicate that channel 0 of an IXXAT card (V3.x or newer drivers) should be used
//canOpen.PortName = "IXXATV30";
//***************************************************
//* Initialize the CAN card and network
//***************************************************
canOpen.Initialize();
//***************************************************
//* Initialize the amplifier
//***************************************************
xAxisAmp.Initialize(canOpen, X_AXIS_CAN_ADDRESS);
//Code to replace vb handle statements
JogPosButton.MouseUp += JogPosButton_MouseUp;
JogPosButton.MouseDown += JogPosButton_MouseDown;
JogNegButton.MouseUp += JogNegButton_MouseUp;
JogNegButton.MouseDown += JogNegButton_MouseDown;
//***************************************************
//*
//* EtherCAT Network
//*
//***************************************************
//ecatObj = new EcatObj();
//
//***************************************************
//* The next line of code is optional. The port name is the IP address of
//* the Ethernet adapter. Alternatively, a shortcut name “eth” can be used
//* in conjunction with the adapter number. For example “eth0” for the first
//* Ethernet adapter, “eth1” for the second adapter. If no port name is
//* supplied, it will default to “eth0”.
//**************************************************************************
//// Indicate that the first Ethernet adapter is to be used
//ecatObj.PortName = "eth0";
//// Specify an IP address
//ecatObj.PortName = "192.168.1.1";
//
//***************************************************
//* Initialize the EtherCAT network
//***************************************************
//ecatObj.Initialize();
//
//***************************************************
//* Initialize the amplifier
//***************************************************
//xAxisAmp.InitializeEcat(ecatObj, ECAT_NODE);
//
// Read velocity loop settings from the amp, use these as reasonble starting
// points for the trajectory limits.
VelocityTextBox.Text = Convert.ToString((xAxisAmp.VelocityLoopSettings.VelLoopMaxVel) / 10);
AccelTextBox.Text = Convert.ToString((xAxisAmp.VelocityLoopSettings.VelLoopMaxAcc) / 10);
DecelTextBox.Text = Convert.ToString((xAxisAmp.VelocityLoopSettings.VelLoopMaxDec) / 10);
// Initialize home object with amplifier home settings
Home = xAxisAmp.HomeSettings;
Timer1.Start();
}
catch (Exception ex)
{
DisplayError(ex);
}
}
