LI Bin
752 lines
25 KiB
C++
752 lines
25 KiB
C++
|
|
|
|
#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;
|
|
} |