xhouse/EF3-Interface
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
92299ad02a2f6b4f70d2e7a60f931c5c496894eb
EF3-Interface/PcDmis/Base/Interfac/Msi/Hsi/SevenOcean/SO7_Proto_Aux.cpp
T
LI Bin a4113d9247 Add rotary control panel.
2014-03-18 15:31:31 +08:00

752 lines
25 KiB
C++
Raw Blame History

#include "stdafx.h"
#include "SO7_Proto_Aux.h"
#include "math.h"
#define MY_CONFIG 1
#define MAX_DEVPATH_LENGTH 256
#define ENDPOINT_TIMEOUT 500
#define MAX_IN_BUFF_SIZE 1024
//***** Static Data *****
static char *outBuff = NULL;
struct_so7_r_ep_buff CSO7_Proto_Aux::ep_buff[lEPSIZE];
//================================================================
int CSO7_Proto_Aux::g_hEP8x_Thread_State=THREAD_PAUSED;
HANDLE CSO7_Proto_Aux::g_hEP8x_Thread_Id=NULL;
//================================================================
int CSO7_Proto_Aux::g_hEP02_Thread_State=THREAD_PAUSED;
HANDLE CSO7_Proto_Aux::g_hEP02_Thread_Id=NULL;
HANDLE CSO7_Proto_Aux::g_hEP02_Serial_Mutex;
//================================================================
struct_so7_r_machine CSO7_Proto_Aux::g_machine;
usb_dev_handle *CSO7_Proto_Aux::g_dev=NULL;
CLogger *CSO7_Proto_Aux::g_pLogger1;
HANDLE CSO7_Proto_Aux::g_hHomedEvent = NULL;
CLogger* g_pLog1=NULL;
//******************************************************************************
CSO7_Proto_Aux::CSO7_Proto_Aux()
{
ep_buff[EP_01_CMD_IDX]._ep = EP_S07_01;
ep_buff[EP_81_DATA_IDX]._ep = EP_S07_81;
ep_buff[EP_02_CMD_IDX]._ep = EP_S07_02;
ep_buff[EP_82_DATA_IDX]._ep = EP_S07_82;
for (int i=0;i<lEPSIZE;i++)
{
ep_buff[i]._size = 0;
ep_buff[i]._save_send_cmd = 99;
ep_buff[i]._async_context = NULL;
ep_buff[i]._buffer = (char *)malloc(MAX_BUFF_SIZE);
ep_buff[i]._hProtoPending = false;
ep_buff[i]._event = NULL;
};
for (int i=0;i<20;i++)
{
for (int j=0;j<2;j++)
{
g_machine.GetInterruptMsg[i][j]=0;
}
}
g_machine.IsSupportReadInterrputMsg=FALSE;
g_machine.IsOffline=TRUE;
g_machine.FPGAData=0;
g_machine.FirmwareVer=FirmwareVer_3_X_R;
g_machine.r._Move_Speed_Gear =2;
g_machine.r._MoveTo_Speed_Gear =0;
g_machine.r._pos_fixed._long_ =0;
g_machine.r._ZSignal_pos._long_=0;
g_machine.r._d_cur_pos_ = 0;
g_machine.ADC_Number=0;
g_machine.ADC_Value =0;
g_machine.Sys_Reset_Flag=0;
g_machine.cVerNumber=3;
g_machine.dRotaryCirclDis=100;
g_machine.SEQ_NUMBER=0;
g_machine.s_machine_r_config.r_axis._scale_resolution=0.5;
g_machine.s_machine_r_config.r_axis._neg_working_limit=0;
g_machine.s_machine_r_config.r_axis._MoveToSpeed[0]=0.0;
g_machine.s_machine_r_config.r_axis._MoveToSpeed[1]=0.0;
g_machine.s_machine_r_config.r_axis._MotionSpeedScale=1.0;
g_machine.s_machine_r_config.motion._EnCloseLoop=FALSE;
g_machine.s_machine_r_config.motion._RetryTimes=5;
g_machine.s_machine_r_config.motion._ShiftPositionR=0.0;
g_machine.s_machine_r_config.motion.GetInterruptMsgMethod=E_GET_INTERRUPT_MSG_INTERRUPT;//E_GET_INTERRUPT_MSG_INQUIRY;
g_machine.s_machine_r_config.motion.m_WriteDataSleepTime=0;
g_machine.s_machine_r_config.motion.m_AccuraErrPulseR=1;
g_machine.s_machine_r_config.motion.m_EQUIDIS_R=0;
g_machine.s_machine_r_config.motion.m_SDK3000_CntThreadSleepVal=550000;
g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[0]=70;
g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[1]=70;
g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[2]=70;
g_machine.s_machine_r_config.motion.m_SV4000E_DenoisePara[3]=70;
g_machine.s_machine_r_config.motion.m_MachineType=MACHINE_SO7_R_CONTROLLER;
g_machine.s_machine_r_config.motion.m_VideoCardType=0;
g_machine.s_rstatus._bIsZMMotionFinished=0;
g_machine.r._scale_pos._long_ = 0;
g_machine.r._d_cur_pos_ = 0;
g_machine.r._dSet_Zero_Pos = 0.0;
g_machine.r._lSet_Zero_Pos = 0;
g_machine.s_machine_r_config._dRSpeed[0] = 50;
g_machine.s_machine_r_config._dRSpeed[1] = 50;
g_machine.InterruptFlag[0]=0;
g_machine.InterruptFlag[1]=0;
g_machine.InPortStatus=0;
m_bHomingActive = false;
g_pLogger1 = new CLogger(_T("\\UtilityDebug.Log"));
g_pLogger1->Send(_T("Construct Cso7_Proto.\r\n"));
};
//******************************************************************************
CSO7_Proto_Aux::~CSO7_Proto_Aux()
{
for (int i=0;i<lEPSIZE;i++)
{
free(ep_buff[i]._buffer);
};
if (g_pLogger1)
g_pLogger1->Send(_T("Destruct Cso7_Proto.\r\n"));
delete g_pLogger1;
g_pLogger1 = NULL;
}
#pragma warning(disable:4996)
//******************************************************************************
// Send is direct and async.
// The receive thread will receive data and interpret it.
//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto_Aux::Init_SO7Usb()
{
//Set initial state of the machine
g_machine.s_rstatus._machine_running = false;
SSI_STATUS_MOTION rStatus=SSI_STATUS_MOTION_NORMAL;
g_machine.IsOffline=FALSE;
int usb_status = NULL;
usb_init(); // initialize the library
usb_status = usb_find_busses(); // find all busses
usb_status = usb_find_devices(); // find all connected devices
g_dev = _open_usb_dev(USB_R_SEQ_NUMBER);
if (!g_dev)
{
MessageBox(NULL, _T("Unable to open device"), _T("Message"), MB_OK|MB_ICONERROR);
g_pLogger1->SendAndFlushPerMode(_T("Unable to open device %s \r\n"), usb_strerror());
g_machine.IsOffline=TRUE;
rStatus= SSI_STATUS_MOTION_DATALINK_ERROR;
}
else if (usb_set_configuration(g_dev, MY_CONFIG) < 0)
{
MessageBox(NULL, _T("Unable to SET CONFIGURATION"), _T("Message"), MB_OK|MB_ICONERROR);
g_machine.IsOffline=TRUE;
rStatus=SSI_STATUS_MOTION_DATALINK_ERROR;
}
else if (usb_claim_interface(g_dev, 0) < 0)
{
usb_close(g_dev);
MessageBox(NULL, _T("Unable to CLAIM DEVICE"), _T("Message"), MB_OK|MB_ICONERROR);
g_machine.IsOffline=TRUE;
rStatus=SSI_STATUS_MOTION_DATALINK_ERROR;
}
g_pLogger1->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.
//
ep_buff[EP_02_CMD_IDX]._event = CreateEvent(NULL, // default security attributes
FALSE, // manual reset event object
NULL, // signaled
NULL); // unamed object
g_hEP02_Thread_Id = CreateThread( (LPSECURITY_ATTRIBUTES) NULL,
0,
(LPTHREAD_START_ROUTINE) g_EP02_Thread,
(LPVOID) this,
0,
NULL);
g_hEP02_Thread_State = THREAD_RUNNING_STATE1;
// ********************************************************************
// Prepare and start EP_S07_81 Thread - Use async commit.
//
ep_buff[EP_82_DATA_IDX]._event = CreateEvent(NULL, // default security attributes
FALSE, // manual reset event object
NULL, // signaled
NULL); // unamed object
g_hEP8x_Thread_State = THREAD_PAUSED;
g_hEP8x_Thread_Id = CreateThread( (LPSECURITY_ATTRIBUTES) NULL,
0,
(LPTHREAD_START_ROUTINE) g_EP8x_Thread,
(LPVOID) this,
0,
NULL);
g_hEP02_Serial_Mutex = CreateMutex(NULL, // default security attributes
FALSE, // initial owner
NULL); // name
// *********************************************************************
g_hHomedEvent = CreateEvent(NULL, // default security attributes
TRUE, // manual reset event object
FALSE, // initial state is signaled
NULL); // unamed object
return rStatus;
}
//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto_Aux::Exit_SO7Usb()
{
SSI_STATUS_MOTION Status=SSI_STATUS_MOTION_NORMAL;
g_hEP8x_Thread_State = THREAD_EXIT;
g_hEP02_Thread_State = THREAD_EXIT;
SetEvent(ep_buff[EP_82_DATA_IDX]._event);
if (g_hEP8x_Thread_Id)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(g_hEP8x_Thread_Id,&dwCode);
Sleep(1);
}
}
SetEvent(ep_buff[EP_02_CMD_IDX]._event);
if (g_hEP02_Thread_Id)
{
DWORD dwCode = STILL_ACTIVE;
while (dwCode == STILL_ACTIVE)
{
GetExitCodeThread(g_hEP02_Thread_Id,&dwCode);
Sleep(1);
}
}
if (g_dev)
{
usb_release_interface(g_dev,0);
usb_close(g_dev);
g_dev = NULL;
}
SetEvent(ep_buff[EP_82_DATA_IDX]._event);
CloseHandle(ep_buff[EP_82_DATA_IDX]._event);
SetEvent(ep_buff[EP_02_CMD_IDX]._event);
CloseHandle(ep_buff[EP_02_CMD_IDX]._event);
g_hEP02_Thread_State = THREAD_EXIT;
ReleaseMutex(g_hEP02_Serial_Mutex);
CloseHandle(g_hEP02_Serial_Mutex);
g_pLogger1->SendAndFlushPerMode(_T("Exit: Exit_SO7Usb \r\n"));
return Status;
}
//******************************************************************************
usb_dev_handle* CSO7_Proto_Aux::_open_usb_dev(unsigned short sSeqNumber)
{
struct usb_bus *bus = NULL;
struct usb_device *dev = NULL;
usb_dev_handle *udev = NULL;
for (bus = usb_get_busses(); bus; bus = bus->next)
{
for (dev = bus->devices; dev; dev = dev->next)
{
if (dev->descriptor.idVendor == SEVENOCEAN_VID && dev->descriptor.idProduct == SEVENOCEAN_PID)
{
udev = usb_open(dev);
usb_claim_interface(udev, 0);
if(Get_R_SeqNumber(udev) != sSeqNumber)
{
usb_close(udev);
}
else
{
return udev;
}
}
}
}
return NULL;
}
//******************************************************************************
int CSO7_Proto_Aux::Get_R_SeqNumber(usb_dev_handle *udev)
{
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
*(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_SEQ_NUMBER;
ep_buff[EP_02_CMD_IDX]._size = 0x02;
ep_buff[EP_82_DATA_IDX]._size = 0x01;
int _ret;
_ret = usb_bulk_write(udev, ep_buff[2]._ep, (char *)ep_buff[2]._buffer,(int)ep_buff[2]._size, 50);
if (_ret < 0)
{
return -1;
}
_ret = usb_bulk_read(udev, ep_buff[3]._ep, (char *)ep_buff[3]._buffer,(int)ep_buff[3]._size, 5000);
if (_ret < 0)
{
return -1;
}
g_machine.SEQ_NUMBER = -1;
g_machine.SEQ_NUMBER = *(ep_buff[EP_82_DATA_IDX]._buffer);
return g_machine.SEQ_NUMBER;
}
//===========================================================================
// Worker Thread to serialize EP_S07_02 commands.
//===========================================================================
unsigned __stdcall CSO7_Proto_Aux::g_EP02_Thread(LPVOID pThis)
{
CSO7_Proto_Aux* _This = (CSO7_Proto_Aux*)pThis;
for (;;)
{
if (g_hEP02_Thread_State == THREAD_EXIT)
ExitThread(0);
WaitForSingleObject(ep_buff[EP_02_CMD_IDX]._event,INFINITE);
switch (g_hEP02_Thread_State)
{
case THREAD_EXIT:
{
ExitThread(0);
}
case THREAD_PAUSED:
break;
case THREAD_RUNNING_STATE1:
{
_This->_send_usb_cmd(EP_02_CMD_IDX);
break;
}
case THREAD_RUNNING_STATE2:
{
_This->_write_usb_data_only(EP_02_CMD_IDX);
break;
}
default:
ExitThread(0);
}
};
g_hEP02_Thread_State = THREAD_EXIT;
ExitThread(0);
};
//******************************************************************************
// This is the worker thread to process USBD_TRANSFER_DIRECTION_IN
//******************************************************************************
unsigned __stdcall CSO7_Proto_Aux::g_EP8x_Thread(LPVOID pThis)
{
CSO7_Proto_Aux* _This = (CSO7_Proto_Aux*)pThis;
for (;;)
{
if (g_hEP8x_Thread_State == THREAD_EXIT)
ExitThread(0);
WaitForSingleObject(ep_buff[EP_82_DATA_IDX]._event, INFINITE);
switch (g_hEP8x_Thread_State)
{
case THREAD_EXIT:
{
ExitThread(0);
}
case THREAD_PAUSED:
break;
case THREAD_RUNNING_STATE1:
{
_This->_read_data_8x(EP_81_DATA_IDX);
break;
}
case THREAD_RUNNING_STATE2:
{
_This->_read_data_8x(EP_82_DATA_IDX);
break;
}
default:
ExitThread(0);
}
}
}
//===============================================================================
// _send_usb_cmd(iEP) sends data to the corresponding iEP channel.
// iEP = EP_S07_01 / EP_S07_02 EP_S07_01 = 0x01; EP_S07_02 = 0x02;
//===============================================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_usb_cmd(int iEP_Base)
{
if (g_machine.IsOffline)
{
SetEvent(ep_buff[iEP_Base+1]._event);
ep_buff[iEP_Base]._hProtoPending = false;
return SSI_STATUS_MOTION_NORMAL;
}
int _ret;
ep_buff[iEP_Base]._save_send_cmd = ep_buff[iEP_Base]._buffer[0];
ep_buff[iEP_Base]._save_send_cmd0 = ep_buff[iEP_Base]._buffer[1];
ep_buff[iEP_Base]._save_send_cmd1 = ep_buff[iEP_Base]._buffer[2];
_ret = usb_bulk_write(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int) ep_buff[iEP_Base]._size, 50);
if (_ret < 0)
{
TRACE("Write Timeout \n");
ASSERT(0);
return SSI_STATUS_MOTION_TIMEOUT;
}
SetEvent(ep_buff[iEP_Base+1]._event);
ep_buff[iEP_Base]._hProtoPending = false;
return SSI_STATUS_MOTION_NORMAL;
}
//===============================================================================
// _write_usb_data_only(iEP) sends data to the corresponding iEP channel.No need to
//process the reap async
// iEP = EP_S07_01 / EP_S07_02 EP_S07_01 = 0x01; EP_S07_02 = 0x02;
//===============================================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_write_usb_data_only(int iEP_Base)
{
if (g_machine.IsOffline)
{
ep_buff[iEP_Base]._hProtoPending = false;
ep_buff[iEP_Base+1]._hProtoPending = false;
return SSI_STATUS_MOTION_NORMAL;
}
int _ret;
ep_buff[iEP_Base]._save_send_cmd = ep_buff[iEP_Base]._buffer[0];
ep_buff[iEP_Base]._save_send_cmd0 = ep_buff[iEP_Base]._buffer[1];
ep_buff[iEP_Base]._save_send_cmd1 = ep_buff[iEP_Base]._buffer[2];
//TRACE3("_send_usb_cmd() iEP : %X - ep_buff[iEP]._save_send_cmd : %X ._buffer[0] : %X\r\n", iEP_Base, ep_buff[iEP_Base]._save_send_cmd, ep_buff[iEP_Base]._buffer[0]);
_ret = usb_bulk_write(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int) ep_buff[iEP_Base]._size, 50);
if (_ret < 0)
{
TRACE("Write Timeout \n");
ASSERT(0);
return SSI_STATUS_MOTION_TIMEOUT;
}
ep_buff[iEP_Base]._hProtoPending = false;
ep_buff[iEP_Base+1]._hProtoPending = false;
return SSI_STATUS_MOTION_NORMAL;
}
//===============================================================================
// iEP_Base : EP_81_DATA_IDX/EP_82_DATA_IDX
//===============================================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_read_data_8x(int iEP_Base)
{
if (g_machine.IsOffline)
{
ep_buff[EP_82_DATA_IDX]._hProtoPending = false;
ep_buff[EP_01_CMD_IDX]._hProtoPending = false;
ep_buff[EP_81_DATA_IDX]._hProtoPending = false;
return SSI_STATUS_MOTION_NORMAL;
}
if (iEP_Base == EP_82_DATA_IDX)
{
int _ret;
_ret = usb_bulk_read(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int) ep_buff[iEP_Base]._size, 5000);
if (_ret > 0)
{
_process_rcv_transfer_data(iEP_Base);
ep_buff[EP_82_DATA_IDX]._hProtoPending = false;
}
else
{
TRACE1("Read Timeout %xx\n", *((char *)ep_buff[iEP_Base]._buffer));
ASSERT(0);
return SSI_STATUS_MOTION_TIMEOUT;
}
return SSI_STATUS_MOTION_NORMAL;
}
else
{
int _ret;
_ret = usb_interrupt_read(g_dev, ep_buff[iEP_Base]._ep, (char *)ep_buff[iEP_Base]._buffer,(int) ep_buff[iEP_Base]._size, 20);
if (_ret > 0)
{
_process_rcv_transfer_data(iEP_Base);
}
else
{
g_machine.InterruptFlag[0] = 0;
TRACE1("There is no data interrupt read from controller. %xx\n", *((char *)ep_buff[iEP_Base]._buffer));
}
ep_buff[EP_01_CMD_IDX]._hProtoPending = false;
ep_buff[EP_81_DATA_IDX]._hProtoPending = false;
return SSI_STATUS_MOTION_NORMAL;
}
}
//******************************************************************************
// Parse the data received based on the index (which EP are we processing).
// The CCmdObj should only have one CMD_STATUS_PROCESSING.
// Update CCmdObj _ep_01_status and _ep_81_status.
// Update _status to CMD_STATUS_COMPLETE when both _ep_01_status and _ep_81_status
// are CMD_STATUS_COMPLETE.
//
//******************************************************************************
void CSO7_Proto_Aux::_process_rcv_transfer_data(int iEP)
{
switch (iEP)
{
case EP_01_CMD_IDX :
TRACE0("_process_rcv_transfer_data() : Update EP_01_CMD_IDX.\r\n");
break;
case EP_82_DATA_IDX : //
if(ep_buff[EP_02_CMD_IDX]._save_send_cmd == CT_DATA)
{
switch (ep_buff[EP_02_CMD_IDX]._save_send_cmd0)
{
case CT_READ_AXISXYZ:
_process_SO7_CMD_READ_AXIS_R();
break;
case CT_READ_SEC_REALY:
_process_SO7_CMD_READ_ZSIGNAL_POS_R();
break;
default:
TRACE1("_process_rcv_transfer_data() : Unknown ep_buff[EP_02_CMD_IDX]._save_send_cmd : %X \r\n", ep_buff[EP_02_CMD_IDX]._save_send_cmd);
TRACE1("_process_rcv_transfer_data() : Unknown ep_buff[EP_81_DATA_IDX]._buffer[0] : %X \r\n", ep_buff[EP_81_DATA_IDX]._buffer[0]);
break;
};
}
break;
case EP_02_CMD_IDX :
TRACE0("_process_rcv_transfer_data() : Update EP_02_CMD_IDX.\r\n");
break;
case EP_81_DATA_IDX :
TRACE0("_process_rcv_transfer_data() : Update EP_81_DATA_IDX.\r\n");
break;
default:
break;
};
};
//******************************************************************************
// Kick the g_hEP01_Thread_Event to get the g_EP01_Thread going.
// iEP = EP_S07_01 or EP_S07_02 = 0x01 or 0x02
//******************************************************************************
SSI_STATUS_MOTION CSO7_Proto_Aux::_do_single_threaded_usb_comm(int iEP_Base)
{
while ((ep_buff[iEP_Base]._hProtoPending == TRUE) || (ep_buff[iEP_Base+1]._hProtoPending == TRUE))
{
ASSERT(0);
Sleep(3);
}
ep_buff[iEP_Base]._hProtoPending = ep_buff[iEP_Base+1]._hProtoPending = TRUE;
if (iEP_Base == EP_01_CMD_IDX)
SetEvent(ep_buff[EP_82_DATA_IDX]._event);
else
SetEvent(ep_buff[iEP_Base]._event);
while ((ep_buff[iEP_Base]._hProtoPending == TRUE) || (ep_buff[iEP_Base+1]._hProtoPending == TRUE))
{
Sleep(3);
}
return SSI_STATUS_MOTION_NORMAL;
}
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_process_SO7_CMD_READ_AXIS_R()
{
g_machine.r._scale_pos._long_=0;
g_machine.r._scale_pos._char_[2] = *(ep_buff[EP_82_DATA_IDX]._buffer);
g_machine.r._scale_pos._char_[1] = *(ep_buff[EP_82_DATA_IDX]._buffer+1);
g_machine.r._scale_pos._char_[0] = *(ep_buff[EP_82_DATA_IDX]._buffer+2);
g_machine.r._scale_pos._char_[3] = 0;
if (g_machine.r._scale_pos._long_ > 8388608)
g_machine.r._scale_pos._long_ = g_machine.r._scale_pos._long_ - 16777216;
return SSI_STATUS_MOTION_NORMAL;
};
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_process_SO7_CMD_READ_ZSIGNAL_POS_R()
{
g_machine.r._ZSignal_pos._char_[2] = *(ep_buff[EP_82_DATA_IDX]._buffer);
g_machine.r._ZSignal_pos._char_[1] = *(ep_buff[EP_82_DATA_IDX]._buffer+1);
g_machine.r._ZSignal_pos._char_[0] = *(ep_buff[EP_82_DATA_IDX]._buffer+2);
g_machine.r._ZSignal_pos._char_[3] = 0;
if (g_machine.r._ZSignal_pos._long_ > 8388608)
g_machine.r._ZSignal_pos._long_=g_machine.r._ZSignal_pos._long_-16777216;
return SSI_STATUS_MOTION_NORMAL;
}
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_RESET_R(int iType)
{
iType++;
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
*(ep_buff[EP_02_CMD_IDX]._buffer) = CT_SCALE;
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_SET_RESET_FLAG;
*(ep_buff[EP_02_CMD_IDX]._buffer+2) = 0x01;
ep_buff[EP_02_CMD_IDX]._size = 0x03;
ep_buff[EP_82_DATA_IDX]._size = 0x02;
g_hEP02_Thread_State=THREAD_RUNNING_STATE2;
_do_single_threaded_usb_comm(EP_02_CMD_IDX);
ReleaseMutex(g_hEP02_Serial_Mutex);
return SSI_STATUS_MOTION_NORMAL;
};
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_MOVE_R(int iType, int iMoveAngle)
{
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
g_machine.r._pos_fixed._long_ = (ONE_R_PULSE * iMoveAngle / 360);
*(ep_buff[EP_02_CMD_IDX]._buffer) = CT_MOTOR;
if(iType)
{
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_MOVETOY;
}
else
{
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_MOVETOX;
}
*(ep_buff[EP_02_CMD_IDX]._buffer+2)=(((g_machine.r._pos_fixed._char_[3])<0x80)?(g_machine.r._pos_fixed._char_[2]):((g_machine.r._pos_fixed._char_[2])|0x80));//����λ
*(ep_buff[EP_02_CMD_IDX]._buffer+3)=(g_machine.r._pos_fixed._char_[1]);
*(ep_buff[EP_02_CMD_IDX]._buffer+4)=(g_machine.r._pos_fixed._char_[0]);
ep_buff[EP_02_CMD_IDX]._size = 0x07;
ep_buff[EP_81_DATA_IDX]._size = 0x07;
g_hEP02_Thread_State = THREAD_RUNNING_STATE2;
_do_single_threaded_usb_comm(EP_02_CMD_IDX);
ReleaseMutex(g_hEP02_Serial_Mutex);
return SSI_STATUS_MOTION_NORMAL;
};
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_READ_AXIS_R()
{
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
*(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_AXISXYZ;
*(ep_buff[EP_02_CMD_IDX]._buffer+2)=0x00;
ep_buff[EP_02_CMD_IDX]._size = 0x03;
ep_buff[EP_82_DATA_IDX]._size = 0x09;
g_hEP02_Thread_State=THREAD_RUNNING_STATE1;
g_hEP8x_Thread_State=THREAD_RUNNING_STATE2;
_do_single_threaded_usb_comm(EP_02_CMD_IDX);
ReleaseMutex(g_hEP02_Serial_Mutex);
return SSI_STATUS_MOTION_NORMAL;
};
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_READ_ZSIGNAL_POS_R(int iType)
{
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
*(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
if(iType)
{
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_SEC_REALY;
}
else
{
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_READ_SEC_REALX;
}
*(ep_buff[EP_02_CMD_IDX]._buffer+2)=0;
ep_buff[EP_02_CMD_IDX]._size = 0x03;
ep_buff[EP_82_DATA_IDX]._size = 0x03;
g_hEP02_Thread_State=THREAD_RUNNING_STATE1;
g_hEP8x_Thread_State=THREAD_RUNNING_STATE2;
_do_single_threaded_usb_comm(EP_02_CMD_IDX);
ReleaseMutex(g_hEP02_Serial_Mutex);
return SSI_STATUS_MOTION_NORMAL;
}
//******************************************************************************
long CSO7_Proto_Aux::GetRCurrentPosition(int iType)
{
long ztos = -1;
_send_cmd_SO7_CMD_READ_AXIS_R();
_send_cmd_SO7_CMD_READ_ZSIGNAL_POS_R(iType);
ztos = abs(g_machine.r._scale_pos._long_ - g_machine.r._ZSignal_pos._long_);
if(ztos > 8388608)
{
ztos = ztos - 9413433;
}
return (ztos * 360 / 4720000);
}
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_STOP_R(int iType)
{
iType++;
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
*(ep_buff[EP_02_CMD_IDX]._buffer) = CT_MOTOR;
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_STOPA;
*(ep_buff[EP_02_CMD_IDX]._buffer+2)=0x00;
TRACE("[STOP_R]\n");
ep_buff[EP_02_CMD_IDX]._size = 0x04;
ep_buff[EP_81_DATA_IDX]._size = 0x04;
g_hEP02_Thread_State=THREAD_RUNNING_STATE2;
_do_single_threaded_usb_comm(EP_02_CMD_IDX);
ReleaseMutex(g_hEP02_Serial_Mutex);
return SSI_STATUS_MOTION_NORMAL;
};
//==============================================================
SSI_STATUS_MOTION CSO7_Proto_Aux::_send_cmd_SO7_CMD_SET_SEQ_NUMBER()
{
WaitForSingleObject(g_hEP02_Serial_Mutex, INFINITE);
memset(ep_buff[EP_02_CMD_IDX]._buffer, 0x00, MAX_BUFF_SIZE);
*(ep_buff[EP_02_CMD_IDX]._buffer) = CT_DATA;
*(ep_buff[EP_02_CMD_IDX]._buffer+1) = CT_WRITE_SEQ_NUMBER;
*(ep_buff[EP_02_CMD_IDX]._buffer+2)=g_machine.SEQ_NUMBER;
ep_buff[EP_02_CMD_IDX]._size = 0x03;
ep_buff[EP_81_DATA_IDX]._size = 0x45;
g_hEP02_Thread_State=THREAD_RUNNING_STATE2;
_do_single_threaded_usb_comm(EP_02_CMD_IDX);
ReleaseMutex(g_hEP02_Serial_Mutex);
return SSI_STATUS_MOTION_NORMAL;
}
Reference in New Issue View Git Blame Copy Permalink