📄 stewartwin_dlg.cpp
字号:
void CCStewartWinAppDlg::OnClearError()
{
DisplyMotorStatus();
UpdateData(false);
for( int i = 0 ; i < MOTION_NUMBER - 1 ;i++)
meiObjects->clearError(meiObjects->MpiMotion( i ));
if( GoHomeFlage == 0 ) //正常运行
{
meiObjects->clearError(meiObjects->MpiMotion( 6 ));
}
}
void CCStewartWinAppDlg::OnClearEncoder()
{
for (int index = 0; index < AXIS_NUMBER; index++)
{
meiObjects->ClearEncoder( meiObjects->MpiAxis( index ) );
}
}
//==============================================================================
int CCStewartWinAppDlg::AllLegAtHome(void)
{
int index;
int temp ;
long returnValue;
double ActPositon;
temp = 0;
ReadIO( );
if( DriverStatusOO.m_Limit_WH_NEG1 )
{
temp ++ ;
}
if( DriverStatusOO.m_Limit_WH_NEG2 )
{
temp ++ ;
}
if( DriverStatusOO.m_Limit_WH_NEG3 )
{
temp ++ ;
}
if( DriverStatusOO.m_Limit_WH_NEG4 )
{
temp ++ ;
}
if( DriverStatusOO.m_Limit_WH_NEG5 )
{
temp ++ ;
}
if( DriverStatusOO.m_Limit_WH_NEG6 )
{
temp ++ ;
}
if( temp == 6 )
AtHomeFlage = 1;
/* for( index = 0; index < AXIS_NUMBER; index++)//index < numberOfAxes
{
returnValue = mpiAxisActualPositionGet( meiObjects->MpiAxis( index ) , &ActPositon);
if( ActPositon != 0.0 )
{
AtHomeFlage = 0; //不在家
break;
}
}
*/
if( temp == 6 )
return 1; //在家
else
return 0; //不在家
}
//==============================================================================
int CCStewartWinAppDlg::OnGoHome()
{
// TODO: Add your control notification handler code here
long returnValue;
int index;
int i;
int temp = 0;
usTime NusTime_oo;
//meiObjects->MapAxisToMotion(1);
if( !( meiObjects->CheckAmpEnable() ) )
{
MessageBox("请先使能伺服放大器！！！");
return(0);
}
meiObjects->MapAxisToMotion(0);
for( int j = 0 ; j < 20 ; j++)
{
for( i = 0 ; i < 6 ; i ++ )
{
if( j % 2 == 0 )
meiObjects->simpleTrapMove( meiObjects->MpiMotion( i ) , -80000 , 5000 , 10000 , 20000 );
else
meiObjects->simpleTrapMove( meiObjects->MpiMotion( i ) , 0 , 5000 , 10000 , 20000 );
}
meiObjects->waitForMotionDone();
NusTime_oo.MySleep( 1000000 );//1000ms
MessageBox("MotionDone！！！");
}
/* for( int pointJ=0 ;pointJ < MOTOR_NUMBER ;pointJ++)
{
Direct[pointJ] = -1;
BuCangFlage[pointJ] = 0;
}
AtHomeFlage = AllLegAtHome( );
//
meiObjects->MapAxisToMotion(0);
//
if( AtHomeFlage == 0 )//不在家
{
//meiObjects->simpleTrapMove( meiObjects->MpiMotion( 0) , 800000 , 3000 , 10000 , 20000 );
for( i = 0 ; i < AXIS_NUMBER ; i ++ )
meiObjects->simpleTrapMove( meiObjects->MpiMotion( i ) , 1000000 , 3000 , 10000 , 20000 );
GoHomeFlage = 1;
}
temp = 0;
bool LegsAtHomeFlage[6] = {false ,false ,false ,false ,false ,false};
do
{
ReadIO( );
if( LegsAtHomeFlage[0] == false && DriverStatusOO.m_Limit_WH_NEG1 )
{
//meiObjects->ClearEncoder( meiObjects->MpiAxis( 0 ) );
LegsAtHomeFlage[0] = true ;
}
if( LegsAtHomeFlage[1] == false && DriverStatusOO.m_Limit_WH_NEG2 )
{
//meiObjects->ClearEncoder( meiObjects->MpiAxis( 1 ) );
LegsAtHomeFlage[1] = true ;
}
if( LegsAtHomeFlage[2] == false && DriverStatusOO.m_Limit_WH_NEG3 )
{
//meiObjects->ClearEncoder( meiObjects->MpiAxis( 2 ) );
LegsAtHomeFlage[2] = true ;
}
if( LegsAtHomeFlage[3] == false && DriverStatusOO.m_Limit_WH_NEG4 )
{
//meiObjects->ClearEncoder( meiObjects->MpiAxis( 3 ) );
LegsAtHomeFlage[3] = true ;
}
if( LegsAtHomeFlage[4] == false && DriverStatusOO.m_Limit_WH_NEG5 )
{
// meiObjects->ClearEncoder( meiObjects->MpiAxis( 4 ) );
LegsAtHomeFlage[4] = true ;
}
if( LegsAtHomeFlage[5] == false && DriverStatusOO.m_Limit_WH_NEG6 )
{
//meiObjects->ClearEncoder( meiObjects->MpiAxis( 5 ) );
LegsAtHomeFlage[5] = true ;
}
if( LegsAtHomeFlage[0] && LegsAtHomeFlage[1]
&& LegsAtHomeFlage[2] && LegsAtHomeFlage[3]
&& LegsAtHomeFlage[4] && LegsAtHomeFlage[5] )
{
AtHomeFlage = 1;
}
}while ( AtHomeFlage == 0 );//不在家
MessageBox("肢腿归零结束 ！");
//MessageBox("at home");
//meiPlatformSleep(500);
// Change the LIMIT_HW_NEGS configuration //
meiObjects->limitConfigure( MOTOR_NUMBER, //电机的数目//
MPIEventTypeLIMIT_HW_NEG, // -HW limit //
MPIActionNONE, // None active on limit //
FALSE, // Active High FALSE //
FALSE, // Use velocity as a trigger qualifier //
EVENT_DURATION);
NusTime_oo.MySleep( 1000000 );//1000ms
for ( i = 0; i < AXIS_NUMBER; i++)
{
meiObjects->ClearEncoder( meiObjects->MpiAxis( i ) );
}
NusTime_oo.MySleep( 50000 );//500ms
for( i = 0 ; i < MOTION_NUMBER - 1 ;i++)
meiObjects->clearError(meiObjects->MpiMotion( i ));
NusTime_oo.MySleep( 50000 );//500ms
/* //前往stewart平台的圆点( 0 ,0 ,-630 ,0 ,0 ,0)
//meiObjects->simpleTrapMove( meiObjects->MpiMotion( 0 ) , -8192 * 18 , 3000 , 10000 , 20000 );
for( i = 0 ; i < AXIS_NUMBER ; i ++ )
meiObjects->simpleTrapMove( meiObjects->MpiMotion( i ) , -8192 * 18 , 3000 , 10000 , 20000 );
// Change the LIMIT_HW_NEGS configuration //
NusTime_oo.MySleep( 1000000 );//1s
do
{
ReadIO();
}while ( DriverStatusOO.m_Limit_WH_NEG1
|| DriverStatusOO.m_Limit_WH_NEG2
|| DriverStatusOO.m_Limit_WH_NEG3
|| DriverStatusOO.m_Limit_WH_NEG4
|| DriverStatusOO.m_Limit_WH_NEG5
|| DriverStatusOO.m_Limit_WH_NEG6 );//在家
AtHomeFlage = 0; //不在家
// meiObjects->waitForMotionDone();
meiObjects->limitConfigure( MOTOR_NUMBER, //电机的数目//
MPIEventTypeLIMIT_HW_NEG, // -HW limit //
MPIActionE_STOP, // E-STOP on limit //
FALSE, // Active High //
FALSE, // Use velocity as a trigger qualifier //
EVENT_DURATION);
while( meiObjects->CheckAxieAtTarget(0) != 1)
{
;
}
//
PlatHomeTrace();
OnRun() ;
do
{
ReadIO();
}while ( DriverStatusOO.m_Limit_WH_NEG1
|| DriverStatusOO.m_Limit_WH_NEG2
|| DriverStatusOO.m_Limit_WH_NEG3
|| DriverStatusOO.m_Limit_WH_NEG4
|| DriverStatusOO.m_Limit_WH_NEG5
|| DriverStatusOO.m_Limit_WH_NEG6 );//在家
AtHomeFlage = 0; //不在家
meiObjects->limitConfigure( MOTOR_NUMBER, //电机的数目//
MPIEventTypeLIMIT_HW_NEG, // -HW limit //
MPIActionE_STOP, // E-STOP on limit //
FALSE, // Active High //
FALSE, // Use velocity as a trigger qualifier //
EVENT_DURATION);
while( meiObjects->CheckAxieAtTarget(6) != 1)
{
;
}
MessageBox("平台归零结束 !");
for ( i = 0; i < AXIS_NUMBER; i++ )
{
meiObjects->ClearEncoder( meiObjects->MpiAxis( i ) );
}
*/
return(1);
}
//========================================================
//返回值为0，则没有错误发送，为1 则有错误发送。
bool CCStewartWinAppDlg::DisplyMotionStatus(void)
{
MPIState MotionStatusFlage[ MOTION_NUMBER ];
CString MotionStatus[ MOTION_NUMBER ];
meiObjects->CheckMotionStatus( MotionStatusFlage );
int i;
for( i = 0 ; i < MOTION_NUMBER-1 ; i ++)
{
switch (MotionStatusFlage[i]) /* Check state and take the appropriate action */
{
case MPIStateIDLE: /* IDLE is OK, start a move. */
{
MotionStatus[i] = _T("空闲");
break;
}
case MPIStateSTOPPING: /* STOPPING is OK, modify a move. */
{
MotionStatus[i] = _T("停止");
break;
}
case MPIStateMOVING: /* MOVING is OK, modify a move. */
{
MotionStatus[i] = _T("运动");
break;
}
case MPIStateERROR: /* ERROR is not OK. */
default: /* Don't know what happened. */
{
m_MotionStatus1 = _T("错误");
m_MotionStatus2 = _T("错误");
m_MotionStatus3 = _T("错误");
m_MotionStatus4 = _T("错误");
m_MotionStatus5 = _T("错误");
m_MotionStatus6 = _T("错误");
UpdateData(false);
return(1);
break;
}
}
}
m_MotionStatus1 = MotionStatus[0];
m_MotionStatus2 = MotionStatus[1];
m_MotionStatus3 = MotionStatus[2];
m_MotionStatus4 = MotionStatus[3];
m_MotionStatus5 = MotionStatus[4];
m_MotionStatus6 = MotionStatus[5];
return(0);
}
//========================================================。
void CCStewartWinAppDlg::DisplyMotorStatus(void)
{
MPIAction LimitActions[MOTOR_NUMBER];
MPI_BOOL LimitDirections[MOTOR_NUMBER];
long encoderPhases[MOTOR_NUMBER];
CString LimitAction[MOTOR_NUMBER];
meiObjects->CheckLimitConfigure( LimitActions , LimitDirections , encoderPhases);
for( int i =0 ; i < MOTOR_NUMBER ; i ++)
{
switch (LimitActions[i] )
{
case MPIActionE_STOP:
LimitAction[i] = _T("E_STOP");
break;
case MPIActionNONE:
LimitAction[i] = _T("NONE");
break;
case MPIActionSTOP:
LimitAction[i] = _T("STOP");
break;
case MPIActionABORT:
LimitAction[i] = _T("ABORT");
break;
default :
LimitAction[i] = _T(" !error !");
break;
}
}
DriverStatusOO.m_LimitDirection1 = (bool)LimitDirections[0];
DriverStatusOO.m_LimitDirection2 = (bool)LimitDirections[1];
DriverStatusOO.m_LimitDirection3 = (bool)LimitDirections[2];
DriverStatusOO.m_LimitDirection4 = (bool)LimitDirections[3];
DriverStatusOO.m_LimitDirection5 = (bool)LimitDirections[4];
DriverStatusOO.m_LimitDirection6 = (bool)LimitDirections[5];
DriverStatusOO.m_LimitAction1 = LimitAction[0] ;
DriverStatusOO.m_LimitAction2 = LimitAction[1] ;
DriverStatusOO.m_LimitAction3 = LimitAction[2] ;
DriverStatusOO.m_LimitAction4 = LimitAction[3] ;
DriverStatusOO.m_LimitAction5 = LimitAction[4] ;
DriverStatusOO.m_LimitAction6 = LimitAction[5] ;
DriverStatusOO.m_EncoderPhase1 = (bool)encoderPhases[0];
DriverStatusOO.m_EncoderPhase2 = (bool)encoderPhases[1];
DriverStatusOO.m_EncoderPhase3 = (bool)encoderPhases[2];
DriverStatusOO.m_EncoderPhase4 = (bool)encoderPhases[3];
DriverStatusOO.m_EncoderPhase5 = (bool)encoderPhases[4];
DriverStatusOO.m_EncoderPhase6 = (bool)encoderPhases[5];
}
void CCStewartWinAppDlg::OnSysSet()
{
SysSetOO.DoModal();
}
void CCStewartWinAppDlg::OnTimer(UINT nIDEvent)
{
// TODO: Add your message handler code here and/or call default
double Position[AXIS_NUMBER];
double Velocity[AXIS_NUMBER];
switch(nIDEvent)
{
case 102: //与SetTimer的第一个参数一致即可
meiObjectsExtern->ObtainPositonVelocity( Position , Velocity);
m_ActLeg1.Format("%f", Position[0] );
m_ActLeg2.Format("%f", Position[1] );
m_ActLeg3.Format("%f", Position[2] );
m_ActLeg4.Format("%f", Position[3] );
m_ActLeg5.Format("%f", Position[4] );
m_ActLeg6.Format("%f", Position[5] );
DisplyMotionStatus();
UpdateData(FALSE);
//PostQuitMessage(0);
break;
default:
//CTishiDlg::KillTimer(nIDEvent);
//PostQuitMessage(0);
break;
}
CDialog::OnTimer(nIDEvent);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -