Пример #1
0
void ecatcheck( void *ptr )
{
    int slave;

    while(1)
    {
        if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
        {
            if (needlf)
            {
               needlf = FALSE;
               printf("\n");
            }
            /* one ore more slaves are not responding */
            ec_group[currentgroup].docheckstate = FALSE;
            ec_readstate();
            for (slave = 1; slave <= ec_slavecount; slave++)
            {
               if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
               {
                  ec_group[currentgroup].docheckstate = TRUE;
                  if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
                  {
                     printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
                     ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
                     ec_writestate(slave);
                  }
                  else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
                  {
                     printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
                     ec_slave[slave].state = EC_STATE_OPERATIONAL;
                     ec_writestate(slave);                              
                  }
                  else if(ec_slave[slave].state > 0)
                  {
                     if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
                     {
                        ec_slave[slave].islost = FALSE;
                        printf("MESSAGE : slave %d reconfigured\n",slave);                           
                     }
                  } 
                  else if(!ec_slave[slave].islost)
                  {
                     /* re-check state */
                     ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
                     if (!ec_slave[slave].state)
                     {
                        ec_slave[slave].islost = TRUE;
                        printf("ERROR : slave %d lost\n",slave);                           
                     }
                  }
               }
               if (ec_slave[slave].islost)
               {
                  if(!ec_slave[slave].state)
                  {
                     if (ec_recover_slave(slave, EC_TIMEOUTMON))
                     {
                        ec_slave[slave].islost = FALSE;
                        printf("MESSAGE : slave %d recovered\n",slave);                           
                     }
                  }
                  else
                  {
                     ec_slave[slave].islost = FALSE;
                     printf("MESSAGE : slave %d found\n",slave);                           
                  }
               }
            }
            if(!ec_group[currentgroup].docheckstate)
               printf("OK : all slaves resumed OPERATIONAL.\n");
        }
        usleep(10000);
    }   
}   
Пример #2
0
void simpletest(char *ifname)
{
    int i, j, oloop, iloop, wkc_count, chk;
    needlf = FALSE;
    inOP = FALSE;

   printf("Starting simple test\n");
   
   /* initialise SOEM, bind socket to ifname */
   if (ec_init(ifname))
   {   
      printf("ec_init on %s succeeded.\n",ifname);
      /* find and auto-config slaves */


       if ( ec_config_init(FALSE) > 0 )
      {
         printf("%d slaves found and configured.\n",ec_slavecount);

         ec_config_map(&IOmap);

         ec_configdc();

         printf("Slaves mapped, state to SAFE_OP.\n");
         /* wait for all slaves to reach SAFE_OP state */
         ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 4);

         oloop = ec_slave[0].Obytes;
         if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
         if (oloop > 8) oloop = 8;
         iloop = ec_slave[0].Ibytes;
         if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
         if (iloop > 8) iloop = 8;

         printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);

         printf("Request operational state for all slaves\n");
         expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
         printf("Calculated workcounter %d\n", expectedWKC);
         ec_slave[0].state = EC_STATE_OPERATIONAL;
         /* send one valid process data to make outputs in slaves happy*/
         ec_send_processdata();
         ec_receive_processdata(EC_TIMEOUTRET);
         /* request OP state for all slaves */
         ec_writestate(0);
         chk = 40;
         /* wait for all slaves to reach OP state */
         do
         {
            ec_send_processdata();
            ec_receive_processdata(EC_TIMEOUTRET);
            ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
         }
         while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
         if (ec_slave[0].state == EC_STATE_OPERATIONAL )
         {
            printf("Operational state reached for all slaves.\n");
            wkc_count = 0;
            inOP = TRUE;
                /* cyclic loop */
            for(i = 1; i <= 10000; i++)
            {
               ec_send_processdata();
               wkc = ec_receive_processdata(EC_TIMEOUTRET);

                    if(wkc >= expectedWKC)
                    {
                        printf("Processdata cycle %4d, WKC %d , O:", i, wkc);

                        for(j = 0 ; j < oloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].outputs + j));
                        }

                        printf(" I:");                  
                        for(j = 0 ; j < iloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].inputs + j));
                        }   
                        printf(" T:%lld\r",ec_DCtime);
                        needlf = TRUE;
                    }
                    usleep(5000);
                    
                }
                inOP = FALSE;
            }
            else
            {
                printf("Not all slaves reached operational state.\n");
                ec_readstate();
                for(i = 1; i<=ec_slavecount ; i++)
                {
                    if(ec_slave[i].state != EC_STATE_OPERATIONAL)
                    {
                        printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                            i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
                    }
                }
            }           
            printf("\nRequest init state for all slaves\n");
            ec_slave[0].state = EC_STATE_INIT;
            /* request INIT state for all slaves */
            ec_writestate(0);
        }
        else
        {
            printf("No slaves found!\n");
        }
        printf("End simple test, close socket\n");
        /* stop SOEM, close socket */
        ec_close();
    }
    else
    {
        printf("No socket connection on %s\nExcecute as root\n",ifname);
    }   
}   
Пример #3
0
void redtest(char *ifname, char *ifname2)
{
   int cnt, i, j, oloop, iloop;

   printf("Starting Redundant test\n");

   /* initialise SOEM, bind socket to ifname */
//   if (ec_init_redundant(ifname, ifname2))
   if (ec_init(ifname))
   {
      printf("ec_init on %s succeeded.\n",ifname);
      /* find and auto-config slaves */
      if ( ec_config(FALSE, &IOmap) > 0 )
      {
         printf("%d slaves found and configured.\n",ec_slavecount);
         /* wait for all slaves to reach SAFE_OP state */
         ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE);

         /* configure DC options for every DC capable slave found in the list */
         ec_configdc();

         /* read indevidual slave state and store in ec_slave[] */
         ec_readstate();
         for(cnt = 1; cnt <= ec_slavecount ; cnt++)
         {
            printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
                  cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
                  ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
            printf("         Out:%8.8x,%4d In:%8.8x,%4d\n",
                  (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes);
            /* check for EL2004 or EL2008 */
            if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052)))
            {
               digout = ec_slave[cnt].outputs;
            }
         }
         expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
         printf("Calculated workcounter %d\n", expectedWKC);

         printf("Request operational state for all slaves\n");
         ec_slave[0].state = EC_STATE_OPERATIONAL;
         /* request OP state for all slaves */
         ec_writestate(0);
         /* activate cyclic process data */
         dorun = 1;
         /* wait for all slaves to reach OP state */
         ec_statecheck(0, EC_STATE_OPERATIONAL,  EC_TIMEOUTSTATE);
         oloop = ec_slave[0].Obytes;
         if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
         if (oloop > 8) oloop = 8;
         iloop = ec_slave[0].Ibytes;
         if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
         if (iloop > 8) iloop = 8;
         if (ec_slave[0].state == EC_STATE_OPERATIONAL )
         {
            printf("Operational state reached for all slaves.\n");
            inOP = TRUE;
            /* acyclic loop 5000 x 20ms = 10s */
            for(i = 1; i <= 5000; i++)
            {
               printf("Processdata cycle %5d , Wck %3d, DCtime %12lld, dt %12lld, O:",
                  dorun, wkc , ec_DCtime, gl_delta);
               for(j = 0 ; j < oloop; j++)
               {
                  printf(" %2.2x", *(ec_slave[0].outputs + j));
               }
               printf(" I:");
               for(j = 0 ; j < iloop; j++)
               {
                  printf(" %2.2x", *(ec_slave[0].inputs + j));
               }
               printf("\r");
               fflush(stdout);
               osal_usleep(20000);
            }
            dorun = 0;
            inOP = FALSE;
         }
         else
         {
            printf("Not all slaves reached operational state.\n");
             ec_readstate();
             for(i = 1; i<=ec_slavecount ; i++)
             {
                 if(ec_slave[i].state != EC_STATE_OPERATIONAL)
                 {
                     printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                         i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
                 }
             }
         }
         printf("Request safe operational state for all slaves\n");
         ec_slave[0].state = EC_STATE_SAFE_OP;
         /* request SAFE_OP state for all slaves */
         ec_writestate(0);
      }
      else
      {
         printf("No slaves found!\n");
      }
      printf("End redundant test, close socket\n");
      /* stop SOEM, close socket */
      ec_close();
   }
   else
   {
      printf("No socket connection on %s\nExcecute as root\n",ifname);
   }
}
Пример #4
0
void simpletest(char *ifname)
{
    int i, j, oloop, iloop, wkc_count, chk, slc;
    UINT mmResult;

    needlf = FALSE;
    inOP = FALSE;

   printf("Starting simple test\n");
   
   /* initialise SOEM, bind socket to ifname */
   if (ec_init(ifname))
   {   
      printf("ec_init on %s succeeded.\n",ifname);
      /* find and auto-config slaves */


       if ( ec_config_init(FALSE) > 0 )
       {
         printf("%d slaves found and configured.\n",ec_slavecount);

         if((ec_slavecount > 1))
         {
             for(slc = 1; slc <= ec_slavecount; slc++)
             {
                 // beckhoff EL7031, using ec_slave[].name is not very reliable
                 if((ec_slave[slc].eep_man == 0x00000002) && (ec_slave[slc].eep_id == 0x1b773052))
                 {
                     printf("Found %s at position %d\n", ec_slave[slc].name, slc);
                     // link slave specific setup to preop->safeop hook
                     ec_slave[slc].PO2SOconfig = &EL7031setup;
                 }
                 // Copley Controls EAP, using ec_slave[].name is not very reliable
                 if((ec_slave[slc].eep_man == 0x000000ab) && (ec_slave[slc].eep_id == 0x00000380))
                 {
                     printf("Found %s at position %d\n", ec_slave[slc].name, slc);
                     // link slave specific setup to preop->safeop hook
                     ec_slave[slc].PO2SOconfig = &AEPsetup;
                 }
             }
         }


         ec_config_map(&IOmap);

         ec_configdc();

         printf("Slaves mapped, state to SAFE_OP.\n");
         /* wait for all slaves to reach SAFE_OP state */
         ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 4);

         oloop = ec_slave[0].Obytes;
         if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
         if (oloop > 8) oloop = 8;
         iloop = ec_slave[0].Ibytes;
         if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
         if (iloop > 8) iloop = 8;

         printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);

         printf("Request operational state for all slaves\n");
         expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
         printf("Calculated workcounter %d\n", expectedWKC);
         ec_slave[0].state = EC_STATE_OPERATIONAL;
         /* send one valid process data to make outputs in slaves happy*/
         ec_send_processdata();
         ec_receive_processdata(EC_TIMEOUTRET);

         /* start RT thread as periodic MM timer */
         mmResult = timeSetEvent(1, 0, RTthread, 0, TIME_PERIODIC);

         /* request OP state for all slaves */
         ec_writestate(0);
         chk = 40;
         /* wait for all slaves to reach OP state */
         do
         {
            ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
         }
         while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
         if (ec_slave[0].state == EC_STATE_OPERATIONAL )
         {
            printf("Operational state reached for all slaves.\n");
            wkc_count = 0;
            inOP = TRUE;

            
            /* cyclic loop, reads data from RT thread */
            for(i = 1; i <= 500; i++)
            {
                    if(wkc >= expectedWKC)
                    {
                        printf("Processdata cycle %4d, WKC %d , O:", rtcnt, wkc);

                        for(j = 0 ; j < oloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].outputs + j));
                        }

                        printf(" I:");                  
                        for(j = 0 ; j < iloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].inputs + j));
                        }   
                        printf(" T:%lld\r",ec_DCtime);
                        needlf = TRUE;
                    }
                    osal_usleep(50000);
                    
            }
            inOP = FALSE;
         }
         else
         {
                printf("Not all slaves reached operational state.\n");
                ec_readstate();
                for(i = 1; i<=ec_slavecount ; i++)
                {
                    if(ec_slave[i].state != EC_STATE_OPERATIONAL)
                    {
                        printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                            i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
                    }
                }
         }           

         /* stop RT thread */
         timeKillEvent(mmResult);

         printf("\nRequest init state for all slaves\n");
         ec_slave[0].state = EC_STATE_INIT;
         /* request INIT state for all slaves */
         ec_writestate(0);
        }
        else
        {
            printf("No slaves found!\n");
        }
        printf("End simple test, close socket\n");
        /* stop SOEM, close socket */
        ec_close();
    }
    else
    {
        printf("No socket connection on %s\nExcecute as root\n",ifname);
    }   
}   
Пример #5
0
void eboxtest(char *ifname)
{
	int cnt, i;
	
	printf("Starting E/BOX test\n");
	
	/* initialise SOEM, bind socket to ifname */
	if (ec_init(ifname))
	{	
		printf("ec_init on %s succeeded.\n",ifname);
		/* find and auto-config slaves */
		if ( ec_config_init(FALSE) > 0 )
		{
			printf("%d slaves found and configured.\n",ec_slavecount);

			// check if first slave is an E/BOX
			if (( ec_slavecount >= 1 ) &&
				 (strcmp(ec_slave[1].name,"E/BOX") == 0))
			{
				// reprogram PDO mapping to set slave in stream mode
				// this can only be done in pre-OP state
				os=sizeof(ob2); ob2 = 0x1601;
				ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);	
				os=sizeof(ob2); ob2 = 0x1a01;
				ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);	
			}

			ec_config_map(&IOmap);

			ec_configdc();

			/* wait for all slaves to reach SAFE_OP state */
			ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE);
			
			/* configure DC options for every DC capable slave found in the list */
			printf("DC capable : %d\n",ec_configdc());
			
			/* check configuration */
			if (( ec_slavecount >= 1 ) &&
				 (strcmp(ec_slave[1].name,"E/BOX") == 0)
			   )
			{
				printf("E/BOX found.\n");

			/* connect struct pointers to slave I/O pointers */
			in_EBOX = (in_EBOX_streamt*) ec_slave[1].inputs;
			out_EBOX = (out_EBOX_streamt*) ec_slave[1].outputs;

			/* read indevidual slave state and store in ec_slave[] */
			ec_readstate();
			for(cnt = 1; cnt <= ec_slavecount ; cnt++)
			{
				printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
					   cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
					   ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
			}
			printf("Request operational state for all slaves\n");

			/* send one processdata cycle to init SM in slaves */	
			ec_send_processdata();
			ec_receive_processdata(EC_TIMEOUTRET);
				
			ec_slave[0].state = EC_STATE_OPERATIONAL;
			/* request OP state for all slaves */
			ec_writestate(0);
			/* wait for all slaves to reach OP state */
			ec_statecheck(0, EC_STATE_OPERATIONAL,  EC_TIMEOUTSTATE);
			if (ec_slave[0].state == EC_STATE_OPERATIONAL )
			{
				printf("Operational state reached for all slaves.\n");
				ain[0] = 0;
				ain[1] = 0;
				ainc = 0;
				dorun = 1;
				usleep(100000); // wait for linux to sync on DC
				ec_dcsync0(1, TRUE, SYNC0TIME, 0); // SYNC0 on slave 1
				/* acyclic loop 20ms */
				for(i = 1; i <= 200; i++)
				{
					/* read DC difference register for slave 2 */
//					ec_FPRD(ec_slave[1].configadr, ECT_REG_DCSYSDIFF, sizeof(DCdiff), &DCdiff, EC_TIMEOUTRET);
//					if(DCdiff<0) { DCdiff = - (int32)((uint32)DCdiff & 0x7ffffff); }
					printf("PD cycle %5d DCtime %12lld Cnt:%3d Data: %6d %6d %6d %6d %6d %6d %6d %6d \n",
						   cyclecount, ec_DCtime, in_EBOX->counter, in_EBOX->stream[0], in_EBOX->stream[1],
					       in_EBOX->stream[2], in_EBOX->stream[3], in_EBOX->stream[4], in_EBOX->stream[5],
					       in_EBOX->stream[98], in_EBOX->stream[99]);
					usleep(20000);
				}
				dorun = 0;
//				printf("\nCnt %d : Ain0 = %f  Ain2 = %f\n", ainc, ain[0] / ainc, ain[1] / ainc);
			}
			else
			{
				printf("Not all slaves reached operational state.\n");
			}			
			}
			else
			{
				printf("E/BOX not found in slave configuration.\n");
			}	
			ec_dcsync0(1, FALSE, 8000, 0); // SYNC0 off
			printf("Request safe operational state for all slaves\n");
			ec_slave[0].state = EC_STATE_SAFE_OP;
			/* request SAFE_OP state for all slaves */
			ec_writestate(0);
			/* wait for all slaves to reach state */
			ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE);
			ec_slave[0].state = EC_STATE_PRE_OP;
			/* request SAFE_OP state for all slaves */
			ec_writestate(0);
			/* wait for all slaves to reach state */
			ec_statecheck(0, EC_STATE_PRE_OP,  EC_TIMEOUTSTATE);
			if (( ec_slavecount >= 1 ) &&
				 (strcmp(ec_slave[1].name,"E/BOX") == 0))
			{
				// restore PDO to standard mode
				// this can only be done is pre-op state
				os=sizeof(ob2); ob2 = 0x1600;
				ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);	
				os=sizeof(ob2); ob2 = 0x1a00;
				ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);	
			}
			printf("Streampos %d\n", streampos);
			output_cvs("stream.txt", streampos);
		}
		else
		{
			printf("No slaves found!\n");
		}
		printf("End E/BOX, close socket\n");
		/* stop SOEM, close socket */
		ec_close();
	}
	else
	{
		printf("No socket connection on %s\nExcecute as root\n",ifname);
	}	
}	
Пример #6
0
void slaveinfo(char *ifname)
{
	int cnt, i, j, nSM;
	uint16 ssigen, dtype;

	printf("Starting slaveinfo\n");

	/* initialise SOEM, bind socket to ifname */
	if (ec_init(ifname))
	{
		printf("ec_init on %s succeeded.\n",ifname);
		/* find and auto-config slaves */
		if ( ec_config(FALSE, &IOmap) > 0 )
		{
			printf("%d slaves found and configured.\n",ec_slavecount);
			printf("Calculated workcounter %d\n",ec_group[0].expectedWKC);
			/* wait for all slaves to reach SAFE_OP state */
			ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE * 3);
			if (ec_slave[0].state != EC_STATE_SAFE_OP )
			{
				printf("Not all slaves reached safe operational state.\n");
				ec_readstate();
				for(i = 1; i<=ec_slavecount ; i++)
				{
					if(ec_slave[i].state != EC_STATE_SAFE_OP)
					{
						printf("Slave %d State=%2x StatusCode=%4x : %s\n",
							i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
					}
				}
			}

			ec_configdc();

			ec_readstate();
			for( cnt = 1 ; cnt <= ec_slavecount ; cnt++)
			{
				printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n",
					   cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
					   ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
				if (ec_slave[cnt].hasdc) printf(" DCParentport:%d\n", ec_slave[cnt].parentport);
				printf(" Activeports:%d.%d.%d.%d\n", (ec_slave[cnt].activeports & 0x01) > 0 ,
				    								 (ec_slave[cnt].activeports & 0x02) > 0 ,
				    								 (ec_slave[cnt].activeports & 0x04) > 0 ,
				    								 (ec_slave[cnt].activeports & 0x08) > 0 );
				printf(" Configured address: %4.4x\n", ec_slave[cnt].configadr);
				printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ec_slave[cnt].eep_man, (int)ec_slave[cnt].eep_id, (int)ec_slave[cnt].eep_rev);
				for(nSM = 0 ; nSM < EC_MAXSM ; nSM++)
				{
					if(ec_slave[cnt].SM[nSM].StartAddr > 0)
						printf(" SM%1d A:%4.4x L:%4d F:%8.8x Type:%d\n",nSM, ec_slave[cnt].SM[nSM].StartAddr, ec_slave[cnt].SM[nSM].SMlength,
						    	(int)ec_slave[cnt].SM[nSM].SMflags, ec_slave[cnt].SMtype[nSM]);
				}
				for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++)
				{
					printf(" FMMU%1d Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j,
						     (int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit,
						     ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit,
						     ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive);
				}
				printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",
				         ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func);
				printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_rl, ec_slave[cnt].mbx_proto);
				ssigen = ec_siifind(cnt, ECT_SII_GENERAL);
				/* SII general section */
				if (ssigen)
                {
					ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07);
					ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08);
					ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09);
					ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a);
					if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0)
					{
						ec_slave[cnt].blockLRW = 1;
						ec_slave[0].blockLRW++;
					}
					ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e);
					ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8;
					ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent;
                }
				printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n",
					     ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails);
				printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n",
					     ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW);
				if ((ec_slave[cnt].mbx_proto & 0x04) && printSDO)
				{
					ODlist.Entries = 0;
					memset(&ODlist, 0, sizeof(ODlist));
					if( ec_readODlist(cnt, &ODlist))
					{
						printf(" CoE Object Description found, %d entries.\n",ODlist.Entries);
						for( i = 0 ; i < ODlist.Entries ; i++)
						{
							ec_readODdescription(i, &ODlist);
							while(EcatError)
							{
								printf("%s", ec_elist2string());
							}
							printf(" Index: %4.4x Datatype: %4.4x Objectcode: %2.2x Name: %s\n",
								   ODlist.Index[i], ODlist.DataType[i], ODlist.ObjectCode[i], ODlist.Name[i]);
							memset(&OElist, 0, sizeof(OElist));
							ec_readOE(i, &ODlist, &OElist);
							while(EcatError)
							{
								printf("%s", ec_elist2string());
							}
							for( j = 0 ; j < ODlist.MaxSub[i]+1 ; j++)
							{
								if ((OElist.DataType[j] > 0) && (OElist.BitLength[j] > 0))
								{
									printf("  Sub: %2.2x Datatype: %4.4x Bitlength: %4.4x Obj.access: %4.4x Name: %s\n",
									   j, OElist.DataType[j], OElist.BitLength[j], OElist.ObjAccess[j], OElist.Name[j]);
									if ((OElist.ObjAccess[j] & 0x0007))
									{
										dtype = OElist.DataType[j];
										printf("          Value :%s\n", SDO2string(cnt, ODlist.Index[i], j, OElist.DataType[j]));
									}
								}
							}
						}
					}
					else
					{
						while(EcatError)
						{
							printf("%s", ec_elist2string());
						}
					}
				}
			}
		}
		else
		{
			printf("No slaves found!\n");
		}
		printf("End slaveinfo, close socket\n");
		/* stop SOEM, close socket */
		ec_close();
	}
	else
	{
		printf("No socket connection on %s\nExcecute as root\n",ifname);
	}
}
Пример #7
0
///establishes the ethercat connection
void EthercatMaster::initializeEthercat() {
  // Bouml preserved body begin 000410F1

    /* initialise SOEM, bind socket to ifname */
    if (ec_init(ethernetDevice.c_str())) {
      LOG(info) << "Initializing EtherCAT on " << ethernetDevice;
      /* find and auto-config slaves */
      if (ec_config(TRUE, &IOmap_) > 0) {

        LOG(trace) << ec_slavecount << " slaves found and configured.";

        /* wait for all slaves to reach Pre OP state */
        /*ec_statecheck(0, EC_STATE_PRE_OP,  EC_TIMEOUTSTATE);
        if (ec_slave[0].state != EC_STATE_PRE_OP ){
        printf("Not all slaves reached pre operational state.\n");
        ec_readstate();
        //If not all slaves operational find out which one
          for(int i = 1; i<=ec_slavecount ; i++){
            if(ec_slave[i].state != EC_STATE_PRE_OP){
              printf("Slave %d State=%2x StatusCode=%4x : %s\n",
              i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
            }
          }
        }
         */

        /* distributed clock is not working
        //Configure distributed clock
        if(!ec_configdc()){
          LOG(warning) << "no distributed clock is available";
        }else{

          uint32 CyclTime = 4000000;
          uint32 CyclShift = 0;
          for (int i = 1; i <= ec_slavecount; i++) {
            ec_dcsync0(i, true, CyclTime, CyclShift);
          }

        }
         */

        /* wait for all slaves to reach SAFE_OP state */
        ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
        if (ec_slave[0].state != EC_STATE_SAFE_OP) {
          LOG(warning) << "Not all slaves reached safe operational state.";
          ec_readstate();
          //If not all slaves operational find out which one
          for (int i = 1; i <= ec_slavecount; i++) {
            if (ec_slave[i].state != EC_STATE_SAFE_OP) {
              LOG(info) << "Slave " << i << " State=" << ec_slave[i].state << " StatusCode=" << ec_slave[i].ALstatuscode << " : " << ec_ALstatuscode2string(ec_slave[i].ALstatuscode);

            }
          }
        }


        //Read the state of all slaves
        //ec_readstate();

        LOG(trace) << "Request operational state for all slaves";

        ec_slave[0].state = EC_STATE_OPERATIONAL;
        // request OP state for all slaves
        /* send one valid process data to make outputs in slaves happy*/
        ec_send_processdata();
        ec_receive_processdata(EC_TIMEOUTRET);
        /* request OP state for all slaves */
        ec_writestate(0);
        // wait for all slaves to reach OP state

        ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
        if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
          LOG(trace) << "Operational state reached for all slaves.";
        } else {
          throw std::runtime_error("Not all slaves reached operational state.");

        }

      } else {
        throw std::runtime_error("No slaves found!");
      }

    } else {
      throw std::runtime_error("No socket connection on " + ethernetDevice + "\nExcecute as root");
    }



    std::string baseJointControllerName = "TMCM-174";
    std::string manipulatorJointControllerName = "TMCM-174";
    std::string actualSlaveName;
    nrOfSlaves = 0;
    YouBotSlaveMsg emptySlaveMsg;
    quantity<si::current> maxContinuousCurrentBase = 3.54 * ampere;
    quantity<si::time> thermalTimeConstantWindingBase = 16.6 * second;
    quantity<si::time> thermalTimeConstantMotorBase = 212 * second;
    quantity<si::current> maxContinuousCurrentJoint13 = 2.36 * ampere;
    quantity<si::time> thermalTimeConstantWindingJoint13 = 16.6 * second;
    quantity<si::time> thermalTimeConstantMotorJoint13 = 212 * second;
    quantity<si::current> maxContinuousCurrentJoint4 = 1.07 * ampere;
    quantity<si::time> thermalTimeConstantWindingJoint4 = 13.2 * second;
    quantity<si::time> thermalTimeConstantMotorJoint4 = 186 * second;
    quantity<si::current> maxContinuousCurrentJoint5 = 0.49 * ampere;
    quantity<si::time> thermalTimeConstantWindingJoint5 = 8.1 * second;
    quantity<si::time> thermalTimeConstantMotorJoint5 = 108 * second;
    int manipulatorNo = 0;


    configfile->readInto(baseJointControllerName, "BaseJointControllerName");
    configfile->readInto(manipulatorJointControllerName, "ManipulatorJointControllerName");

    //reserve memory for all slave with a input/output buffer
    for (int cnt = 1; cnt <= ec_slavecount; cnt++) {
      //     printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
      //             cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
      //             ec_slave[cnt].state, (int) ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);

      ethercatSlaveInfo.push_back(ec_slave[cnt]);

      actualSlaveName = ec_slave[cnt].name;
      if ((actualSlaveName == baseJointControllerName || actualSlaveName == manipulatorJointControllerName) && ec_slave[cnt].Obits > 0 && ec_slave[cnt].Ibits > 0) {
        nrOfSlaves++;
        //   joints.push_back(YouBotJoint(nrOfSlaves));

        firstBufferVector.push_back(emptySlaveMsg);
        secondBufferVector.push_back(emptySlaveMsg);
        ethercatOutputBufferVector.push_back((SlaveMessageOutput*) (ec_slave[cnt].outputs));
        ethercatInputBufferVector.push_back((SlaveMessageInput*) (ec_slave[cnt].inputs));
        YouBotSlaveMailboxMsg emptyMailboxSlaveMsg(cnt);
        firstMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
        secondMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
        newOutputDataFlagOne.push_back(false);
        newOutputDataFlagTwo.push_back(false);
        newMailboxDataFlagOne.push_back(false);
        newMailboxDataFlagTwo.push_back(false);
        newMailboxInputDataFlagOne.push_back(false);
        newMailboxInputDataFlagTwo.push_back(false);
        pendingMailboxMsgsReply.push_back(false);
        int i = 0;
        bool b = false;
        upperLimit.push_back(i);
        lowerLimit.push_back(i);
        limitActive.push_back(b);
        jointLimitReached.push_back(b);
        inverseMovementDirection.push_back(b);
        if (actualSlaveName == baseJointControllerName) {
          motorProtections.push_back(MotorProtection(maxContinuousCurrentBase,
                  thermalTimeConstantWindingBase,
                  thermalTimeConstantMotorBase));
        }
        if (actualSlaveName == manipulatorJointControllerName) {
          manipulatorNo++;
          if (manipulatorNo >= 1 && manipulatorNo <= 3) {
            motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint13,
                    thermalTimeConstantWindingJoint13,
                    thermalTimeConstantMotorJoint13));
          }
          if (manipulatorNo == 4) {
            motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint4,
                    thermalTimeConstantWindingJoint4,
                    thermalTimeConstantMotorJoint4));
          }
          if (manipulatorNo == 5) {
            motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint5,
                    thermalTimeConstantWindingJoint5,
                    thermalTimeConstantMotorJoint5));
            manipulatorNo = 0;
          }

        }
      }

    }
    if (nrOfSlaves != motorProtections.size()) {
      throw std::runtime_error("Insufficient motor protections loaded");
    }

    if (nrOfSlaves > 0) {
      LOG(info) << nrOfSlaves << " EtherCAT slaves found" ;
    } else {
      throw std::runtime_error("No EtherCAT slave could be found");
      return;
    }

    stopThread = false;
    threads.create_thread(boost::bind(&EthercatMaster::updateSensorActorValues, this));

    SLEEP_MILLISEC(10); //needed to start up thread and EtherCAT communication

    return;
  // Bouml preserved body end 000410F1
}