int SetPosHandler(int argc, char *argv[])
{
const int correctArgc = 5;
if (argc == correctArgc) {
uint8_t i;
for (i = 0; i < 4; i++) {
int pos = atoi(argv[i + 1]);
SetCnt(g_motorAddress[i],pos);
}
return 0;
}else if (argc < correctArgc) {
return CMDLINE_TOO_FEW_ARGS;
}else{
return CMDLINE_TOO_MANY_ARGS;
}
}
void Channel::Reset()
{
SetCnt(0);
SrcAddr = 0;
TimerReload = 0;
LoopPos = 0;
Length = 0;
Timer = 0;
Pos = 0;
FIFOReadPos = 0;
FIFOWritePos = 0;
FIFOReadOffset = 0;
FIFOLevel = 0;
}
void Init()
{
T *pT = (T*)malloc(sizeof(T)) ;
unsigned int *pAddr[PRO_MAX];//Ò»¶ÑµØÖ·
for (int i = 0; i < PRO_MAX; ++i)
{
pAddr[i] = (unsigned int*)malloc(sizeof(unsigned int));
}
*pAddr[0] = (unsigned int)pT;
for (int i = 1; i < PRO_MAX; ++i)
{
*pAddr[i] = (unsigned int)pAddr[i-1];
}
SetCnt(GetTickCount() % PRO_LIMIT1 + PRO_LIMIT2);
m_Base = (unsigned int)pAddr[GetCnt() - 1];
m_Inited = 1;
}
void CMtAXL::HomeSearchProc(void)
{
if(!m_fsmHome.IsRun())
return;
if(MC_ERROR == m_fsmHome.GetStatus())
{
printf("\n Axis[%d] Fail home search!!", m_config.nAxisNo);
CancelHomeSearch();
return;
}
else
{
BOOL bError = (!m_state.bServoOn || m_state.bAlarm);
bError |= (m_state.bLimitCw && m_state.bLimitCCw);
if(TRUE == bError)
{
m_fsmHome.SetStatus(MC_ERROR);
return;
}
}
switch(m_fsmHome.GetStatus())
{
case C_START_HOME:
if(m_config.bHomeUseOrg)
m_fsmHome.SetStatus(C_GO_ORG_SENSOR);
else if(FALSE == m_state.bLimitCCw)
m_fsmHome.SetStatus(C_GO_LIMIT_SENSOR);
else
m_fsmHome.SetStatus(C_STOP_LIMIT_SENSOR);
break;
case C_GO_LIMIT_SENSOR:
if(m_fsmHome.Once())
{
SearchMove(FALSE, m_config.nFastHomeSpeed, NegEndLimit, SIGNAL_UP_EDGE, TRUE);
}
else
{
m_fsmHome.SetStatus(C_STOP_LIMIT_SENSOR);
}
break;
case C_STOP_LIMIT_SENSOR:
if(m_state.bDriving)
{
m_fsmHome.RstDelay();
break;
}
if(FALSE == m_fsmHome.DelaymS(500))
break;
AxmSignalSetLimit(m_config.nAxisNo, EMERGENCY_STOP, m_config.nPosLogic, !m_config.nNegLogic);
CaptureMove(TRUE, m_config.nSlowHomeSpeed, NegEndLimit, SIGNAL_DOWN_EDGE, TRUE);
m_fsmHome.SetStatus(C_ESCAPE_LIMIT_SENSOR);
break;
case C_ESCAPE_LIMIT_SENSOR:
if(m_state.bDriving)
{
m_fsmHome.RstDelay();
break;
}
if(FALSE == m_fsmHome.DelaymS(500))
break;
AxmSignalSetLimit(m_config.nAxisNo, EMERGENCY_STOP, m_config.nPosLogic, m_config.nNegLogic);
m_fsmHome.SetStatus(C_GO_Z_PHASE);
break;
case C_GO_ORG_SENSOR:
if(m_state.bDriving)
{
m_fsmHome.RstDelay();
break;
}
if(FALSE == m_fsmHome.DelaymS(500))
break;
if(m_state.bOrg)
{
m_fsmHome.SetStatus(C_STOP_ORG_SENSOR);
}
else if(m_state.bLimitCCw)
{
m_fsmHome.SetStatus(C_RE_GO_ORG_SENSOR);
}
else
{
SearchMove(FALSE, m_config.nFastHomeSpeed, HomeSensor, m_config.nOrgEdge, TRUE);
m_fsmHome.SetStatus(C_STOP_ORG_SENSOR);
}
break;
case C_RE_GO_ORG_SENSOR:
SearchMove(TRUE, m_config.nFastHomeSpeed, HomeSensor, m_config.nOrgEdge, TRUE);
m_fsmHome.SetStatus(C_GO_ORG_SENSOR);
break;
case C_STOP_ORG_SENSOR:
if(m_state.bDriving)
{
m_fsmHome.RstDelay();
break;
}
if(FALSE == m_fsmHome.DelaymS(500))
break;
if(!m_state.bOrg && m_state.bLimitCCw)
{
m_fsmHome.SetStatus(C_RE_GO_ORG_SENSOR);
break;
}
if(m_config.bHomeUseZ)
SearchMove(TRUE, m_config.nSlowHomeSpeed, HomeSensor, !m_config.nOrgEdge, TRUE);
else
CaptureMove(TRUE, m_config.nSlowHomeSpeed, HomeSensor, !m_config.nOrgEdge, TRUE);
m_fsmHome.SetStatus(C_ESCAPE_ORG_SENSOR);
break;
case C_ESCAPE_ORG_SENSOR:
if(m_state.bDriving)
{
m_fsmHome.RstDelay();
break;
}
if(FALSE == m_fsmHome.DelaymS(500))
break;
m_fsmHome.SetStatus(C_GO_Z_PHASE);
break;
case C_GO_Z_PHASE:
if(m_config.bHomeUseZ)
{
CaptureMove(TRUE, m_config.nSlowHomeSpeed, EncodZPhase, SIGNAL_UP_EDGE, TRUE);
m_fsmHome.SetStatus(C_STOP_Z_PHASE);
}
else
{
m_fsmHome.SetStatus(C_SET_CNT_ZERO);
}
break;
case C_STOP_Z_PHASE:
if(m_state.bDriving)
{
m_fsmHome.RstDelay();
break;
}
if(FALSE == m_fsmHome.DelaymS(500))
break;
m_fsmHome.SetStatus(C_SET_CNT_ZERO);
break;
case C_SET_CNT_ZERO:
{
if(m_state.bDriving)
{
m_fsmHome.RstDelay();
break;
}
if(FALSE == m_fsmHome.DelaymS(500))
break;
// int nCaptureCnt = GetCaptureCnt();
SetCnt(0);
m_fsmHome.SetStatus(C_END_HOME);
}
break;
case C_END_HOME:
{
m_bHome = TRUE;
m_fsmHome.SetStatus(MC_STOP);
Update();
int Initspeed = m_table.nSpeed[m_config.nDefaultIndex];
int maxSpeed = (int)(m_config.nMaxSpeed * 0.2);
if(maxSpeed < Initspeed)
Initspeed = maxSpeed;
Move(m_config.nDefaultIndex, Initspeed);
}
break;
}
}//------------------------------------------------------------------
void CMtAXL::ServoOn(void)
{
m_state.bServoOn = TRUE;
SetCnt(m_state.nRealCnt);
AxmSignalServoOn(m_config.nAxisNo, TRUE);
}//------------------------------------------------------------------
