📄 root_3drvs.c
字号:
#include "Functions.h"
void
main( int argc, char *argv[] )
{
BOOL bret ;
DWORD ret,PROCESSID;
DWORD dwWaitMesg;
int i,j;
///////////
char chName[61],chUnit[10];
ULONG ulAttribute,ulMin,ulMax,ulData,ulFail;
USHORT *uspData,usActPhase;
ULONG *plCommandValue[3],*plActualValue[3],ulLoop,ulRet;
HANDLE hSyncData[3],hMDTChannel[3];
T_CYC_DATA *pSCycleMDTData[3],*pSCycleATData[3];
//////////
strcpy( SCS_Task[ROOT].Name, "ROOT");
SCS_Task[ROOT].TaskID = GetCurrentThreadId();
SCS_Task[ROOT].TaskHandle = GetCurrentThread();
SCS_Task[ROOT].Status = STARTED;
bret = RtSetThreadPriority( SCS_Task[ROOT].TaskHandle,
RT_PRIORITY_MAX);
Init_SHM();
RtPrintf("SERCANS II Started1\n");
// PROCESSID = GetCurrentProcessId();
// TerminateProcess(PROCESSID,0);
if(SercansInit())
return;
create_parameter_handler_tasks();
start_parameter_handler_tasks();
get_nc_serc_cyc();
//read parameter test
ChangePhase(0);
do
{
GetPhase( &usActPhase);
if(usActPhase == 0) break;
Sleep(10);
}while(1);
CRead_Y_Minimum(4,&ulMin);//SERCOS周期
//RtPrintf("chUnit:%s\n",chUnit);
RtPrintf("ulMin:%d\n",ulMin);
CRead_Y_Attribute(4,&ulAttribute);
RtPrintf("Read_Y_Attribute ulAttribute=0x%08x\n",ulAttribute);
CRead_Y_Parameter(4,&ulData);
RtPrintf("Read_Y_Parameter ulData=%d\n",ulData);
ulFail = CRead_Y_Parameter(4,&ulData);
if (ulFail == 0)
RtPrintf("Read_Y_Parameter Y4 ulData=0x%08x\n",ulData);
else
RtPrintf("Read_Y_Parameter ulFail=0x%08x\n",ulFail);
ulFail = CRead_Y_Parameter(5,&ulData);//NC周期
if (ulFail == 0)
RtPrintf("Read_Y_Parameter Y5 ulData=0x%08x\n",ulData);
else
RtPrintf("Read_Y_Parameter ulFail=0x%08x\n",ulFail);
ret = CRead_Y_Parameter(2,&ulData);//可用的驱动器
RtPrintf("Read_Y_Parameter ulFail=0x%08x\n",ret);
if (ret == 0){
RtPrintf("Read_Y_Parameter Parameter Y2 SVCH_FINISHED *pulData=0x%08x\n",ulData);
uspData=(USHORT*)ulData;
RtPrintf("Read_Y_Parameter 2 uspData[0]=0x%08x\n",uspData[0]);
RtPrintf("Read_Y_Parameter 2 uspData[1]=0x%08x\n",uspData[1]);
RtPrintf("Read_Y_Parameter 2 uspData[2]=%d\n",uspData[2]);
RtPrintf("Read_Y_Parameter 2 uspData[3]=%d\n",uspData[3]);
RtPrintf("Read_Y_Parameter 2 uspData[4]=%d\n",uspData[4]);
RtPrintf("Read_Y_Parameter 2 uspData[5]=%d\n",uspData[5]);
}
else
RtPrintf("Read_Y_Parameter FAILED ulFail=0x%08x\n",ret);
uspData[0]=6;
uspData[2]=1;//设置操作的驱动器地址
uspData[3]=2;
uspData[4]=3;
ulFail=CWrite_Y_Parameter(2,ulData);
RtPrintf("Write_Y_Parameter ulFail=0x%08x\n",ulFail);
get_exist_drives();
for (i=1;i<4;i++){
ulFail = CRead_A_Parameter(2,i,&ulData);//配置命令通道
if (ulFail == 0){
RtPrintf("Read_A_Parameter Parameter A2 SVCH_FINISHED *pulData=0x%08x\n",ulData);
uspData=(USHORT*)ulData;
RtPrintf("Read_A_Parameter 2 uspData[0]=0x%08x\n",uspData[0]);//命令通道中总字节数
RtPrintf("Read_A_Parameter 2 uspData[2]=%d\n",uspData[2]);//前两个字节所存储的参数
RtPrintf("Read_A_Parameter 2 uspData[3]=%d\n",uspData[3]);
}
else
RtPrintf("Read_A2_Parameter FAILED ulFail=0x%08x\n",ret);
uspData[2]=47;//36速度命令
CWrite_A_Parameter(2,i,ulData);
RtPrintf("Write_A_Parameter ulFail=0x%08x\n",ulFail);//0为成功
ulFail = CRead_A_Parameter(3,i,&ulData);//配置实际值通道
if (ulFail == 0){
RtPrintf("Read_A_Parameter Parameter A3 SVCH_FINISHED *pulData=0x%08x\n",ulData);
uspData=(USHORT*)ulData;
RtPrintf("Read_A_Parameter 3 uspData[0]=0x%08x\n",uspData[0]);//反馈通道中总字节数
RtPrintf("Read_A_Parameter 3 uspData[2]=%d\n",uspData[2]);//前两个字节所存储的参数
RtPrintf("Read_A_Parameter 3 uspData[3]=%d\n",uspData[3]);
}
else
RtPrintf("Read_A2_Parameter FAILED ulFail=0x%08x\n",ret);
uspData[2]=51;//40速度反馈
CWrite_A_Parameter(3,i,ulData);
RtPrintf("Write_A_Parameter ulFail=0x%08x\n",ulFail);//0为成功
}
ChangePhase(2);
do
{
GetPhase( &usActPhase);
if(usActPhase == 2) break;
Sleep(10);
}while(1);
for(i=1;i<4;i++){
//change position data scaling type to rotary.
//模态轴
CWrite_S_Parameter(0,76,i,0xC2);
//change opration mode to position(0xb)
CWrite_S_Parameter(0,32,i,0xb);
}
ChangePhase(4);
do
{
GetPhase( &usActPhase);
if(usActPhase == 4) break;
Sleep(10);
}while(1);
// RtPrintf("main (long*)&ipc_handles.CYCLE_DATA_MDT[1]=%d\n",
// *((long*)&ipc_handles.CYCLE_DATA_AT[0].usData[0]));
for(i=0;i<3;i++){
plCommandValue[i] = (long*)&ipc_handles.CYCLE_DATA_MDT[i].usData[0];
plActualValue[i] = (long*)&ipc_handles.CYCLE_DATA_AT[i].usData[0];
* plCommandValue[i] = * plActualValue[i];
// RtPrintf("plActualValue0=%d\n", *plActualValue);
while( (ipc_handles.CYCLE_DATA_AT[i].usControl & 0xC000) != 0x8000 )
Sleep(10);
// RtPrintf("plActualValue=%d\n", *plActualValue);
ipc_handles.CYCLE_DATA_MDT[i].usControl = 0xE000;
while( (ipc_handles.CYCLE_DATA_AT[i].usControl & 0xC000) != 0xC000)
Sleep(10);
}
for(i=0;i<225000;i++)//4ms一周期，可运行15min
{
// synch_cycle_data(ipc_handles.REAL_CNL);
dwWaitMesg=RtWaitForSingleObject(ipc_handles.REAL_CNL,10);
if (dwWaitMesg==WAIT_TIMEOUT)
break;
for(j=0;j<3;j++){
if (*plCommandValue[j]>3600000)
*plCommandValue[j]=0;
*plCommandValue[0] +=10000;
*plCommandValue[1] +=20000;
*plCommandValue[2] +=30000;
// RtPrintf("plCommandValue=%d\n", *plCommandValue);
}
// trigger_lifecounter(ipc_handles.SOFT_SYNC);
RtReleaseSemaphore( ipc_handles.SOFT_SYNC, 1, NULL);
if (dwWaitMesg==WAIT_TIMEOUT)
RtPrintf("time out\n");
}
for (i=0;i<3;i++){
ipc_handles.CYCLE_DATA_MDT[i].usControl = 0x0000;
}
//////////////////////////////////////////////////////////
// by sharemem //
//////////////////////////////////////////////////////////
// Init the life-counter function
/* ulRet=init_lifecounter (&hLifeCounter);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
// Init the access to cyclic-data
ulRet=init_synch (&hSyncData);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
// open the cyclic command value channel
ulRet=open_mdt_channel (&pSCycleMDTData, &hMDTChannel);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
// open the cyclic actual value channel
ulRet=open_at_channel (&pSCycleATData, &hATChannel);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
// just for an easy access to the first drive parameter S-0-0036
plCommandValue = (long*)&pSCycleMDTData[0].usData[0];
// just for an easy access to the first drive parameter S-0-0051
plActualValue = (long*)&pSCycleATData[0].usData[0];
// set the command value to the actual value
RtPrintf("plCommandValue =%d\n",*plCommandValue);
// wait until drive control section ready to operate (drive status word)
while( (pSCycleATData[0].usControl & 0xC000) != 0x8000 )
Sleep(10);
RtPrintf("plCommandValue =%d\n",*plActualValue);
*plCommandValue = *plActualValue;
// set drive start / drive enable / drive on
// at control word for drive 1
pSCycleMDTData[0].usControl = 0xE000;
// RtPrintf("*plCommandValue=%d\n",*plCommandValue);
// wait until drive is ready (drive status word)
while( (pSCycleATData[0].usControl & 0xC000) != 0xC000 )
Sleep(10);
// Activate live counter
ulRet=CWrite_Y_Parameter(9, // Identnumber Y-0-0009
0);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
// set thread priority to max
RtSetThreadPriority( GetCurrentThread(), RT_PRIORITY_MAX);
// gives a command position from 0 to 360 degree
for( ulLoop=0; ulLoop< 3600 ; ulLoop++ )
{
// syncronisation to cyclic access
ulRet=synch_cycle_data (hSyncData);
// set new value
*plCommandValue += 1000;
// Increment the lifecounter
ulRet=trigger_lifecounter (hLifeCounter);
}
// Deactivate live counter
ulRet=CWrite_Y_Parameter (9, // Identnumber Y-0-0009
0);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
// set drive off
// at cntrol word for drive 1
pSCycleMDTData[0].usControl = 0x0000;
// close the cyclic command value channel
ulRet=close_mdt_channel (hMDTChannel);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
// close the cyclic actual value channel
ulRet=close_at_channel (hATChannel);
if( ulRet != 0 )
{
// see documentation for explanation
return(-1);
}
//read parameter test
RtWaitForSingleObject(ipc_handles.TERMINATE_FLAG,INFINITE);
*/
RtReleaseInterruptVector(hInt);
TerminateThread(SCS_Task[ACPH].TaskHandle,0);
TerminateThread(SCS_Task[SYST].TaskHandle,0);
TerminateThread(SCS_Task[ROOT].TaskHandle,0);
// SCS_Task[ROOT].Status = SUSPENDED;
// ret = SuspendThread( GetCurrentThread() );
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -