xhouse/EF3-Interface
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增导出XYZ工作台接口，新增导出灯光调节接口，完善debug信息输出。

Browse Source
This commit is contained in:
TAO Cheng
2014-04-25 17:23:08 +08:00
parent f1188c017c
commit d1cf313511
19 changed files with 1252 additions and 432 deletions
Download Patch File Download Diff File Expand all files Collapse all files
PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/SO7_Proto.cpp
+484 -80
View File
@@ -10,6 +10,9 @@
#define MAX_IN_BUFF_SIZE 1024
#define M_PI 3.14159265358979323846
const INT HOME_TIMEOUT=300000;
const INT MOVETO_TIMEOUT=60000;
const INT MIN_SLEEP_TIME=20;
const DOUBLE ROTARY_MMtoScale_RESOLUTION=0.0005;
//***** Static Data *****
static char *outBuff = NULL;
@@ -26,9 +29,8 @@ HANDLE CSO7_Proto::g_hEP02_Serial_Mutex;
//================================================================
struct_so7_machine CSO7_Proto::g_machine;
usb_dev_handle *CSO7_Proto::g_dev=NULL;
CLogger *CSO7_Proto::g_pLogger;
CLogger *CSO7_Proto::g_pLogger=NULL;
HANDLE CSO7_Proto::g_hHomedEvent = NULL;
CLogger* g_pLog=NULL;
//===========================================================================
@@ -562,8 +564,7 @@ CSO7_Proto::CSO7_Proto()
so7_motion_reset_controller_parameter();
so7_config_para_set_default();
m_bHomingActive = false;
g_pLogger = new CLogger(_T("\\UtilityDebug.Log"));
g_pLogger->Send(_T("Construct Cso7_Proto.\r\n"));
g_pLogger = new CLogger(_T("\\MachineInterface.Log"));
};
//******************************************************************************
@@ -574,9 +575,10 @@ CSO7_Proto::~CSO7_Proto()
free(ep_buff[i]._buffer);
};
if (g_pLogger)
g_pLogger->Send(_T("Destruct Cso7_Proto.\r\n"));
delete g_pLogger;
g_pLogger = NULL;
{
delete g_pLogger;
g_pLogger = NULL;
}
}
#pragma warning(disable:4996)
@@ -2568,7 +2570,34 @@ SSI_STATUS_MOTION CSO7_Proto::Load_So7_Config()
g_machine.s_machine_config.motion.m_DebugOutputEnable=atoi(cTemp);
}
}
//==================Motion=============================
else if(!_stricmp(token, "X_SCALE_RESOLUTION"))
{
token = strtok( NULL, seps);
if (token)
{
strcpy(cTemp,token);
g_machine.s_machine_config.x_axis._scale_resolution=atof(cTemp);
}
}
else if(!_stricmp(token, "Y_SCALE_RESOLUTION"))
{
token = strtok( NULL, seps);
if (token)
{
strcpy(cTemp,token);
g_machine.s_machine_config.y_axis._scale_resolution=atof(cTemp);
}
}
else if(!_stricmp(token, "Z_SCALE_RESOLUTION"))
{
token = strtok( NULL, seps);
if (token)
{
strcpy(cTemp,token);
g_machine.s_machine_config.z_axis._scale_resolution=atof(cTemp);
}
}
//=================Rotary========================
else if (!_stricmp(token,"ROTARY_AXIS_NUMBER"))
{
@@ -2846,6 +2875,10 @@ int CSO7_Proto::Get_SeqNumber(usb_dev_handle *udev)
//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto::Init_SO7Usb()
{
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("Enter Initialize Usb.\n"));
}
//Set initial state of the machine
g_machine.s_status._machine_running = false;
@@ -2862,7 +2895,6 @@ SSI_STATUS_MOTION CSO7_Proto::Init_SO7Usb()
if (!g_dev)
{
MessageBox(NULL, _T("Unable to open device"), _T("Message"), MB_OK|MB_ICONERROR);
g_pLogger->SendAndFlushPerMode(_T("Unable to open device %s \r\n"), usb_strerror());
g_machine.IsOffline=TRUE;
rStatus= SSI_STATUS_MOTION_DATALINK_ERROR;
}
@@ -2880,7 +2912,6 @@ SSI_STATUS_MOTION CSO7_Proto::Init_SO7Usb()
rStatus=SSI_STATUS_MOTION_DATALINK_ERROR;
}
g_pLogger->SendAndFlushPerMode(_T("Init:Open device succeed .\r\n"));
// ********************************************************************
// This event is used to kick the Serial Usb Command process. This threading model
// is important because the underlying library is not thread-safe.
@@ -2972,8 +3003,10 @@ SSI_STATUS_MOTION CSO7_Proto::Exit_SO7Usb()
g_hEP02_Thread_State = THREAD_EXIT;
ReleaseMutex(g_hEP02_Serial_Mutex);
CloseHandle(g_hEP02_Serial_Mutex);
g_pLogger->SendAndFlushPerMode(_T("Exit: Exit_SO7Usb \r\n"));
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("Exit Exit_SO7Usb.\n"));
}
return Status;
}
@@ -3009,9 +3042,12 @@ SSI_STATUS_MOTION CSO7_Proto::_do_single_threaded_usb_comm(int iEP_Base)
//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto::_start_machine()
{
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("_start_machine.\n"));
}
g_hEP8x_Thread_State = THREAD_RUNNING_STATE2;
g_machine.s_status._machine_running = true;
g_pLogger->SendAndFlushPerMode(_T("_start_machine\n"));
SetEvent(g_hHomedEvent);
//so7_motion_probe_on_off_(false);
@@ -3023,6 +3059,10 @@ SSI_STATUS_MOTION CSO7_Proto::_start_machine()
//===============================================================================
SSI_STATUS_MOTION CSO7_Proto::_shutdown_machine()
{
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("_shutdown_machine.\n"));
}
g_machine.s_status._machine_running = false;
return SSI_STATUS_MOTION_NORMAL;
};
@@ -3216,25 +3256,74 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_laser_on_off(bool _bOnOff)
//**********************************************************************//
//**********************************************************************//
SSI_STATUS_MOTION CSO7_Proto::so7_motion_init_firmware_para()
{
_send_cmd_SO7_CMD_GET_RESET_FLAG();
if(g_machine.IsSupportReadInterrputMsg)
{
_send_cmd_SO7_CMD_SET_GET_INTERRUPT_MSG_METHOD(g_machine.s_machine_config.motion.GetInterruptMsgMethod);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_READ_FIRMWARE_VERSION_INFO();
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
CStringA csAStr("");
csAStr.Format("FirmwareVersion:%s.\n",g_machine.FirmwareInfo);
CString csStr(csAStr);
g_pLogger->SendAndFlushWithTime(csStr);
}
if (g_machine.FirmwareVer==FirmwareVer_6_X)
{
_send_cmd_SO7_CMD_WRITE_DATA_TO_FPGA(E_AXIS_X,E_WRITE_ACCURA_ERR,static_cast<char>(g_machine.s_machine_config.motion.m_AccuraErrPulseX));
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_WRITE_DATA_TO_FPGA(E_AXIS_Y,E_WRITE_ACCURA_ERR,static_cast<char>(g_machine.s_machine_config.motion.m_AccuraErrPulseY));
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_WRITE_DATA_TO_FPGA(E_AXIS_Z,E_WRITE_ACCURA_ERR,static_cast<char>(g_machine.s_machine_config.motion.m_AccuraErrPulseZ));
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_WRITE_DATA_TO_FPGA(E_AXIS_X,E_WRITE_EQUIDISTANCE,static_cast<char>(g_machine.s_machine_config.motion.m_EQUIDIS_X));
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_WRITE_DATA_TO_FPGA(E_AXIS_Y,E_WRITE_EQUIDISTANCE,static_cast<char>(g_machine.s_machine_config.motion.m_EQUIDIS_Y));
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_WRITE_DATA_TO_FPGA(E_AXIS_Z,E_WRITE_EQUIDISTANCE,static_cast<char>(g_machine.s_machine_config.motion.m_EQUIDIS_Z));
Sleep(MIN_SLEEP_TIME);
}
}
else
{
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("FirmwareVersion:UNKNOWN.\n"));
}
}
return SSI_STATUS_MOTION_NORMAL;
}
//==================================================================
bool CSO7_Proto::so7_motion_is_homed()
{
if (g_machine.IsOffline)
{
g_machine.Sys_Reset_Flag =1;
SetEvent(g_hHomedEvent);
}
//if (g_machine.IsOffline)
//{
// g_machine.Sys_Reset_Flag =1;
// SetEvent(g_hHomedEvent);
//}
_send_cmd_SO7_CMD_GET_RESET_FLAG();
if (g_machine.Sys_Reset_Flag == 1)
{
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("so7_motion_is_homed:true.\n"));
}
SetEvent(g_hHomedEvent);
return true;
}
else
{
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("so7_motion_is_homed:false.\n"));
}
return false;
}
};
//========================================================================
// Move the stage left/right until the index location is non-zero
//========================================================================
@@ -3260,7 +3349,6 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_Dcc_Home()
// Home
so7_motion_reset_worktable_lower_left(CT_HOME_XYZ);
g_pLogger->SendAndFlushPerMode(_T("so7_motion_reset_worktable_lower_left.\n"));
TRACE0(" - waiting for X,Y,Zm to stop moving\n");
//========================================
// Wait until X-Y-Zm stopped moving
@@ -3271,14 +3359,66 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_Dcc_Home()
_send_cmd_SO7_CMD_GET_RESET_FLAG();
iRetry++;
}
g_pLogger->SendAndFlushPerMode(_T("[%d]waiting for X,Y,Zm to stop moving\n"),iRetry);
g_pLogger->SendAndFlushPerMode(_T("Home succeed.\n"));
//_get_xyz_index(g_machine.s_machine_config.x_axis._neg_working_limit,g_machine.s_machine_config.y_axis._neg_working_limit,g_machine.s_machine_config.z_axis._neg_working_limit);
_send_cmd_SO7_CMD_SET_VER_NUMBER();
m_bHomingActive = false;
SetEvent(g_hHomedEvent);
return SSI_STATUS_MOTION_NORMAL;
}
//========================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_motion_Dcc_HomeXYZ(char cHomeMachineMode)
{
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("Enter so7_motion_Dcc_HomeXYZ(%d).\n"),cHomeMachineMode);
}
if (g_machine.IsOffline)
{
SetEvent(g_hHomedEvent);
return SSI_STATUS_MOTION_NORMAL;
}
//²éѯÊÇ·ñ¸´Î»
_send_cmd_SO7_CMD_GET_RESET_FLAG();
g_machine.cVerNumber=3;
if (g_machine.Sys_Reset_Flag == 1)
{
_send_cmd_SO7_CMD_SET_VER_NUMBER();
SetEvent(g_hHomedEvent);
return SSI_STATUS_MOTION_NORMAL;
}
m_bHomingActive = true; // Tell the world we need to home the stage
// Home
so7_motion_reset_worktable_lower_left(cHomeMachineMode);
TRACE0("Waiting for X,Y,Zm to stop moving\n");
//========================================
// Wait until X-Y-Zm stopped moving
//========================================
INT lSleep = 20;
INT lMaxLoopCnt = HOME_TIMEOUT/lSleep;
INT lLoopCnt = 0;
Sleep(lSleep);
do
{
Sleep(lSleep);
_send_cmd_SO7_CMD_GET_RESET_FLAG();
lLoopCnt++;
} while (g_machine.Sys_Reset_Flag != 1 && lLoopCnt < lMaxLoopCnt);
//_get_xyz_index(g_machine.s_machine_config.x_axis._neg_working_limit,g_machine.s_machine_config.y_axis._neg_working_limit,g_machine.s_machine_config.z_axis._neg_working_limit);
m_bHomingActive = false;
if (g_machine.Sys_Reset_Flag == 1)
{
SetEvent(g_hHomedEvent);
_send_cmd_SO7_CMD_SET_VER_NUMBER();
return SSI_STATUS_MOTION_NORMAL;
}
else
{
return SSI_STATUS_MOTION_TIMEOUT;
}
}
//==================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_Motion_R_IsHomed(bool &bHomed)
@@ -3286,6 +3426,7 @@ SSI_STATUS_MOTION CSO7_Proto::so7_Motion_R_IsHomed(bool &bHomed)
_send_cmd_SO7_CMD_GET_RESET_FLAG();
if (g_machine.Sys_Reset_Flag == 1)
{
SetEvent(g_hHomedEvent);
bHomed=true;
}
else
@@ -3299,6 +3440,10 @@ SSI_STATUS_MOTION CSO7_Proto::so7_Motion_R_IsHomed(bool &bHomed)
SSI_STATUS_MOTION CSO7_Proto::so7_motion_Dcc_Home_R()
{
//²éѯÊÇ·ñ¸´Î»
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("Enter so7_motion_Dcc_Home_R.\n"));
}
_send_cmd_SO7_CMD_GET_RESET_FLAG();
if (g_machine.Sys_Reset_Flag == 1)
{
@@ -3326,17 +3471,15 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_Dcc_Home_R()
g_machine.z._pos_fixed._long_=lMoveToDis;
}
_send_cmd_SO7_CMD_MOVE_TO_POS_XYZ(CT_MOVETOXYZ);
g_pLogger->SendAndFlushPerMode(_T("so7_motion_reset_worktable_lower_left.\n"));
//========================================
const long lSleep = 20;
const long lMaxLoopCnt = 2000/lSleep;
long lLoopCnt = 0;
INT lSleep = 20;
INT lMaxLoopCnt = MOVETO_TIMEOUT/lSleep;
INT lLoopCnt = 0;
Sleep(lSleep);
BOOL IsFinished(FALSE);
bool IsFinished(FALSE);
do
{
so7_motion_is_finished(EMSG_STOPXYZ_1_MOVETOXYZ,IsFinished);
so7_Motion_R_IsMotionFInished(IsFinished);
Sleep(lSleep);
++lLoopCnt;
} while (!IsFinished && lLoopCnt < lMaxLoopCnt);
@@ -3346,7 +3489,6 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_Dcc_Home_R()
_send_cmd_SO7_CMD_SET_RESET_FLAG();
g_machine.cVerNumber = 3;
_send_cmd_SO7_CMD_SET_VER_NUMBER();
g_pLogger->SendAndFlushPerMode(_T("Home succeed.\n"));
SetEvent(g_hHomedEvent);
return SSI_STATUS_MOTION_NORMAL;
}
@@ -3445,6 +3587,11 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_set_position_R(double dRad,bool bWait)
{
dRMovetoDis=dRMovetoDis;
}
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("so7_motion_set_position_R:[From]%.4f;[To]%.4f;[Dis]%.4F.\n")
,dRStart,dRad,dRMovetoDis);
}
lMoveToDis=static_cast<long>(dRMovetoDis*(static_cast<double>(g_machine.s_machine_config.motion.m_RotaryCirclePulse))/(2.0*M_PI));
g_machine.x._pos_fixed._long_=0;
g_machine.y._pos_fixed._long_=0;
@@ -3464,23 +3611,23 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_set_position_R(double dRad,bool bWait)
_send_cmd_SO7_CMD_MOVE_TO_POS_XYZ(CT_MOVETOXYZ);
g_machine.MotionType=EMSG_STOPXYZ_1_MOVETOXYZ;
BOOL IsFinished(TRUE);
bool IsFinished(true);
if (bWait)
{
const long lSleep = 20;
const long lMaxLoopCnt = 2000/lSleep; // use max homing time of 20 seconds
long lLoopCnt = 0;
INT lSleep = 20;
INT lMaxLoopCnt = MOVETO_TIMEOUT/lSleep; // use max homing time of 20 seconds
INT lLoopCnt = 0;
Sleep(lSleep);
do
{
so7_motion_is_finished(EMSG_STOPXYZ_1_MOVETOXYZ,IsFinished);
so7_Motion_R_IsMotionFInished(IsFinished);
Sleep(lSleep);
++lLoopCnt;
} while (!IsFinished && lLoopCnt < lMaxLoopCnt);
}
g_machine.MotionType=-1;
if (IsFinished)
{
g_machine.MotionType=-1;
return SSI_STATUS_MOTION_NORMAL;
}
else
@@ -3502,10 +3649,19 @@ SSI_STATUS_MOTION CSO7_Proto::so7_Motion_R_IsMotionFInished(bool &bFinished)
if (IsFinished)
{
bFinished=true;
IsMotionFinishedManual(TRUE);
}
else
{
bFinished=false;
IsFinished=IsMotionFinishedManual();
if (IsFinished)
{
bFinished=true;
}
else
{
bFinished=false;
}
}
}
return SSI_STATUS_MOTION_NORMAL;
@@ -3515,14 +3671,6 @@ SSI_STATUS_MOTION CSO7_Proto::so7_Motion_R_IsMotionFInished(bool &bFinished)
SSI_STATUS_MOTION CSO7_Proto::so7_motion_move_R(char _SpeedGear)
{
//4-FASTER,1-SLOWER
if (_SpeedGear>4)
{
_SpeedGear=4;
}
if (_SpeedGear<1)
{
_SpeedGear=1;
}
if (g_machine.s_machine_config.motion.m_RotaryAxisNO==MACHINE_AXIS_X)
{
_send_cmd_SO7_CMD_MOVE_X(_SpeedGear);
@@ -3581,9 +3729,9 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_get_position_xyz(double & dX, double &
dY = ScaleToMM(lY, g_machine.s_machine_config.y_axis._scale_resolution);
dZ = ScaleToMM(lZ, g_machine.s_machine_config.z_axis._scale_resolution);
dX -= g_machine.s_machine_config.x_axis._neg_working_limit;
dY -= g_machine.s_machine_config.y_axis._neg_working_limit;
dZ -= g_machine.s_machine_config.z_axis._neg_working_limit;
//dX -= g_machine.s_machine_config.x_axis._neg_working_limit;
//dY -= g_machine.s_machine_config.y_axis._neg_working_limit;
//dZ -= g_machine.s_machine_config.z_axis._neg_working_limit;
return SSI_STATUS_MOTION_NORMAL;
};
@@ -3618,7 +3766,11 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_set_position_xyz(double dX, double dY,
Y.to = MMtoScale(dY, g_machine.s_machine_config.y_axis._scale_resolution);
Z.from = MMtoScale(dZStart, g_machine.s_machine_config.z_axis._scale_resolution);
Z.to = MMtoScale(dZ, g_machine.s_machine_config.z_axis._scale_resolution);
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("so7_motion_set_position_xyz:[From]X:%.4f,Y:%.4f,Z:%.4f;[To]X:%.4f,Y:%.4f,Z:%.4f.\n")
,dXStart,dYStart,dZStart,dX,dY,dZ);
}
// move the position to make the -X, -Y, -Z position 0,0,0
//X.to += g_machine.s_machine_config.x_axis._neg_working_limit;
//Y.to += g_machine.s_machine_config.y_axis._neg_working_limit;
@@ -3634,32 +3786,114 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_set_position_xyz(double dX, double dY,
// _calculate_straightline_motion(g_machine.s_machine_config._dXYZSpeed);
_send_cmd_SO7_CMD_MOVE_TO_POS_XYZ(CT_MOVETOXYZ);
g_machine.MotionType=EMSG_STOPXYZ_1_MOVETOXYZ;
#pragma message("Test settle wait comparing the status bit to the scale monitor")
TRACE1("Presettle Time: %lf\n", TimeInSecs());
if (bWait)
{
const long lSleep = 20;
const long lMaxLoopCnt = 5000/lSleep; // use max homing time of 20 seconds
long lLoopCnt = 0;
INT lSleep = 20;
INT lMaxLoopCnt = MOVETO_TIMEOUT/lSleep; // use max homing time of 20 seconds
INT lLoopCnt = 0;
Sleep(lSleep);
BOOL IsFinished(FALSE);
bool IsFinished(FALSE);
do
{
so7_motion_is_finished(EMSG_STOPXYZ_1_MOVETOXYZ,IsFinished);
so7_Motion_XYZ_IsMotionFinished(IsFinished);
Sleep(lSleep);
++lLoopCnt;
} while (!IsFinished && lLoopCnt < lMaxLoopCnt);
TRACE1("Presettle Time: %lf\n", TimeInSecs());
//WaitForSettleXYZZM();
TRACE1("Postsettle Time: %lf\n", TimeInSecs());
if (IsFinished)
{
g_machine.MotionType=-1;
return SSI_STATUS_MOTION_NORMAL;
}
else
{
return SSI_STATUS_MOTION_TIMEOUT;
}
}
return SSI_STATUS_MOTION_NORMAL;
};
//==================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_Motion_XYZ_IsMotionFinished(bool &bFinished)
{
if (g_machine.MotionType<EMSG_STOPXYZ_1_MOVETOXYZ)
{
bFinished=true;
}
else
{
BOOL IsFinished(FALSE);
so7_motion_is_finished(g_machine.MotionType,IsFinished);
if (IsFinished)
{
g_machine.MotionType=-1;
bFinished=true;
IsMotionFinishedManual(TRUE);
}
else
{
IsFinished=IsMotionFinishedManual();
if (IsFinished)
{
g_machine.MotionType=-1;
bFinished=true;
}
else
{
bFinished=false;
}
}
}
return SSI_STATUS_MOTION_NORMAL;
}
//==================================
BOOL CSO7_Proto::IsMotionFinishedManual(BOOL _BResetCnts)
{
if (_BResetCnts)
{
g_machine.MotionFinished=FALSE;
g_machine.MotionFinishedCnts=0;
}
else
{
if (g_machine.s_machine_config.motion.m_CNC_Deadlock_Solution>=1)
{
double dPosX(0.0),dPosY(0.0),dPosZ(0.0);
so7_motion_get_position_xyz(dPosX,dPosY,dPosZ);
if ((fabs(dPosX-g_machine.x._ReCorded_Pos)<=g_machine.s_machine_config.x_axis._motor_precision)
&& (fabs(dPosY-g_machine.y._ReCorded_Pos)<=g_machine.s_machine_config.y_axis._motor_precision)
&& (fabs(dPosZ-g_machine.z._ReCorded_Pos)<=g_machine.s_machine_config.z_axis._motor_precision))
{
g_machine.MotionFinishedCnts++;
if (g_machine.MotionFinishedCnts>g_machine.s_machine_config.motion.m_CNC_Deadlock_JudgeMaxCnts)
{
g_machine.MotionFinished=TRUE;
}
else
{
g_machine.MotionFinished=FALSE;
}
}
else
{
g_machine.MotionFinishedCnts=0;
}
g_machine.x._ReCorded_Pos=dPosX;
g_machine.y._ReCorded_Pos=dPosY;
g_machine.z._ReCorded_Pos=dPosZ;
}
else
{
g_machine.MotionFinished=FALSE;
g_machine.MotionFinishedCnts=0;
}
}
return g_machine.MotionFinished;
};
//==================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_motion_is_finished(char MotionType,BOOL& IsFinished)
{
WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done
@@ -3693,9 +3927,97 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_is_finished(char MotionType,BOOL& IsFin
// Full Speed -> dPercentSpeed = 100%
// Slow Speed -> dPercentSpeed = 20%
//========================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_motion_set_speed_xyz(double dPercentSpeed)
SSI_STATUS_MOTION CSO7_Proto::so7_motion_set_speed_xyz(EMACHINE_AXIS cAxis,char cSpeedGear,char Acce,char cHoldSpeed,char cStartSpeed,char cRefreshCycle,double dBufferDis)
{
g_machine.s_machine_config._dXYZSpeed = dPercentSpeed;
char SetSpeedGear(0);
EMACHINE_AXIS SetAXIS(MACHINE_AXIS_NONE);
SetSpeedGear=static_cast<char>(abs(cSpeedGear));
if (SetSpeedGear>4)
{
SetSpeedGear=4;
}
else if (SetSpeedGear<1)
{
SetSpeedGear=1;
}
SetSpeedGear=4-SetSpeedGear;
if (cAxis==MACHINE_AXIS_R)
{
if (g_machine.s_machine_config.motion.m_RotaryAxisNO==MACHINE_AXIS_X)
{
SetAXIS=MACHINE_AXIS_X;
}
else if (g_machine.s_machine_config.motion.m_RotaryAxisNO==MACHINE_AXIS_Y)
{
SetAXIS=MACHINE_AXIS_Y;
}
else if (g_machine.s_machine_config.motion.m_RotaryAxisNO==MACHINE_AXIS_Z)
{
SetAXIS=MACHINE_AXIS_Z;
}
}
switch(cAxis)
{
case MACHINE_AXIS_X:
{
g_machine.s_machine_config.x_axis._speed_base[SetSpeedGear]=Acce;
g_machine.s_machine_config.x_axis._speed_max[SetSpeedGear]=cHoldSpeed;
g_machine.s_machine_config.x_axis._speed_start[SetSpeedGear]=cStartSpeed;
g_machine.s_machine_config.x_axis._speed_fresh[SetSpeedGear]=cRefreshCycle;
g_machine.s_machine_config.x_axis._speed_slow_dis[SetSpeedGear]=dBufferDis;
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(0,SetSpeedGear);
Sleep(MIN_SLEEP_TIME);
break;
}
case MACHINE_AXIS_Y:
{
g_machine.s_machine_config.y_axis._speed_base[SetSpeedGear]=Acce;
g_machine.s_machine_config.y_axis._speed_max[SetSpeedGear]=cHoldSpeed;
g_machine.s_machine_config.y_axis._speed_start[SetSpeedGear]=cStartSpeed;
g_machine.s_machine_config.y_axis._speed_fresh[SetSpeedGear]=cRefreshCycle;
g_machine.s_machine_config.y_axis._speed_slow_dis[SetSpeedGear]=dBufferDis;
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(1,SetSpeedGear);
Sleep(MIN_SLEEP_TIME);
break;
}
case MACHINE_AXIS_Z:
{
g_machine.s_machine_config.z_axis._speed_base[SetSpeedGear]=Acce;
g_machine.s_machine_config.z_axis._speed_max[SetSpeedGear]=cHoldSpeed;
g_machine.s_machine_config.z_axis._speed_start[SetSpeedGear]=cStartSpeed;
g_machine.s_machine_config.z_axis._speed_fresh[SetSpeedGear]=cRefreshCycle;
g_machine.s_machine_config.z_axis._speed_slow_dis[SetSpeedGear]=dBufferDis;
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(2,SetSpeedGear);
Sleep(MIN_SLEEP_TIME);
break;
}
case MACHINE_AXIS_ZOOM:
{
if (SetSpeedGear==4)
{
SetSpeedGear=0;
}
else if (SetSpeedGear==3)
{
SetSpeedGear=0;
}
else if (SetSpeedGear==2)
{
SetSpeedGear=1;
}
else if (SetSpeedGear==1)
{
SetSpeedGear=2;
}
g_machine.s_machine_config.zm_axis._speed._char_[0]=cStartSpeed;
g_machine.s_machine_config.zm_axis._speed._char_[1]=cHoldSpeed;
_send_cmd_SO7_CMD_SET_ZOOM_SPEED(SetSpeedGear);
Sleep(MIN_SLEEP_TIME);
break;
}
default:break;
}
return SSI_STATUS_MOTION_NORMAL;
}
@@ -3717,7 +4039,78 @@ SSI_STATUS_MOTION CSO7_Proto::so7_motion_get_3D_max_speed(double &dMaxSpeed)
return SSI_STATUS_MOTION_NORMAL;
}
//========================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_motion_set_all_speed_para()
{
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(0,0);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(0,1);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(0,2);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(0,3);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(0,4);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(1,0);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(1,1);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(1,2);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(1,3);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(1,4);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(2,0);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(2,1);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(2,2);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(2,3);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PARAMETER(2,4);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PRECISION(0);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PRECISION(1);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_SPEED_PRECISION(2);
Sleep(MIN_SLEEP_TIME);
_send_cmd_SO7_CMD_SET_MOTOR_SPEED_WHEELBASE_PARAMETER();
Sleep(MIN_SLEEP_TIME);
return SSI_STATUS_MOTION_NORMAL;
}
//========================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_motion_get_all_speed_para()
{
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 0, 0);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 0, 1);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 0, 2);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 0, 3);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 0, 4);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 1, 0);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 1, 1);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 1, 2);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 1, 3);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 1, 4);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 2, 0);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 2, 1);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 2, 2);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 2, 3);
_send_cmd_SO7_CMD_READ_SPEED_PARAMETER( 2, 4);
_send_cmd_SO7_CMD_READ_SPEED_PRECISION(0);
_send_cmd_SO7_CMD_READ_SPEED_PRECISION(1);
_send_cmd_SO7_CMD_READ_SPEED_PRECISION(2);
_send_cmd_SO7_CMD_READ_MOTOR_SPEED_WHEELBASE_PARAMETER();
return SSI_STATUS_MOTION_NORMAL;
}
//========================================================================
SSI_STATUS_MOTION CSO7_Proto::_calculate_straightline_motion(double dSpeedMM)
{
@@ -3821,8 +4214,8 @@ SSI_STATUS_MOTION CSO7_Proto::so7_light_set_light_off()
g_machine.s_lights_value._spare_light1=1;
g_machine.s_lights_value.segment[0]=0;
g_machine.s_lights_value.segment[1]=0;
_send_cmd_SO7_CMD_SET_ALL_LIGHT_VALUE();
Sleep(3*MIN_SLEEP_TIME);
return SSI_STATUS_MOTION_NORMAL;
};
@@ -3833,36 +4226,41 @@ SSI_STATUS_MOTION CSO7_Proto::so7_light_set_light()
WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done
_send_cmd_SO7_CMD_SET_ALL_LIGHT_VALUE();
Sleep(3*MIN_SLEEP_TIME);
return SSI_STATUS_MOTION_NORMAL;
};
//==================================================================
SSI_STATUS_MOTION CSO7_Proto::so7_light_set_lamp_state(double dBottomPercent, double dTopPercent)
SSI_STATUS_MOTION CSO7_Proto::so7_light_set_lamp_state(double dTopLightPercent,double dBottomLightPercent,double dCoaxialLightPercent,double dReservedLightPercent,double dRingLightPercent,char cOuterRingLightSwitch,char cInnerRingLightSwitch)
{
WaitForSingleObject(g_hHomedEvent, INFINITE); // machine start and homing is done
if(!g_pLog)
g_pLog=new CLogger(_T("\\Lamp.Log"));
g_machine.s_lights_value._top_light = (static_cast<char>(dTopPercent* (MAXLIGHTVALUE)/100.0 ))+1;
g_machine.s_lights_value._bottom_light = (static_cast<char>(dBottomPercent*(MAXLIGHTVALUE)/100.0))+1;
g_pLog->SendAndFlushPerMode(_T("dBottomPercent: %f dTopPercent: %f\n"),dBottomPercent,dTopPercent);
g_pLog->SendAndFlushPerMode(_T("so7_light_set_lamp_state bottom: %d top: %d\n"), g_machine.s_lights_value._bottom_light,g_machine.s_lights_value._top_light);
TRACE2("so7_light_set_lamp_state bottom: %d top: %d\n",
g_machine.s_lights_value._bottom_light,g_machine.s_lights_value._top_light);
delete g_pLog;
g_pLog=NULL;
WaitForSingleObject(g_hHomedEvent, INFINITE);
g_machine.s_lights_value._bottom_light = (static_cast<char>(dTopLightPercent* (MAXLIGHTVALUE-MINLIGHTVALUE)/100.0 ))+MINLIGHTVALUE;
g_machine.s_lights_value._top_light = (static_cast<char>(dBottomLightPercent*(MAXLIGHTVALUE-MINLIGHTVALUE)/100.0))+MINLIGHTVALUE;
g_machine.s_lights_value._ring_light = (static_cast<char>(dRingLightPercent*(MAXLIGHTVALUE-MINLIGHTVALUE)/100.0))+MINLIGHTVALUE;
g_machine.s_lights_value._coaxial_light = (static_cast<char>(dCoaxialLightPercent*(MAXLIGHTVALUE-MINLIGHTVALUE)/100.0))+MINLIGHTVALUE;
g_machine.s_lights_value._spare_light1 = (static_cast<char>(dReservedLightPercent*(MAXLIGHTVALUE-MINLIGHTVALUE)/100.0))+MINLIGHTVALUE;
g_machine.s_lights_value.segment[0] = cOuterRingLightSwitch;
g_machine.s_lights_value.segment[1] = cInnerRingLightSwitch;
_send_cmd_SO7_CMD_SET_ALL_LIGHT_VALUE();
Sleep(3*MIN_SLEEP_TIME);
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("so7_light_set_lamp_state:TOP:%d,BOT:%d,COAXIAL:%d,RESERVED:%d,RING:%d,ORING:%d,IRING:%d.\n")
,(BYTE)g_machine.s_lights_value._bottom_light,(BYTE)g_machine.s_lights_value._top_light,(BYTE)g_machine.s_lights_value._coaxial_light
,(BYTE)g_machine.s_lights_value._spare_light1,(BYTE)g_machine.s_lights_value._ring_light,(BYTE)g_machine.s_lights_value.segment[0]
,(BYTE)g_machine.s_lights_value.segment[1]);
}
return SSI_STATUS_MOTION_NORMAL;
};
//**********************************************************************//
//**********************************************************************//
//********************************************************************************//
//*******************************************************************************//
//==================================================================================
//==============================================================
//==================================================================================
SSI_STATUS_MOTION CSO7_Proto::_send_cmd_SO7_CMD_MOVE_X(char SpeedGear)
{
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
@@ -4539,6 +4937,12 @@ SSI_STATUS_MOTION CSO7_Proto::_send_cmd_SO7_CMD_COMMON_COMMAND_DATA(char Cmd,cha
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
if (g_machine.s_machine_config.motion.m_DebugOutputEnable>=1)
{
g_pLogger->SendAndFlushWithTime(_T("SEND_SYS_COMMAND:Cmd:%d,BOT:%d,SubCmd:%d,Type:%d,Data:%d.\n")
,Cmd,SubCmd,Type,Data);
}
*(ep_buff[EP_02_CMD_IDX]._buffer) = Cmd;
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = SubCmd;
*(ep_buff[EP_02_CMD_IDX]._buffer+2) = Type;