CanFestival: examples/TestMasterMicroMod/TestMasterMicroMod.c Source File

00001 /*
-00002 This file is part of CanFestival, a library implementing CanOpen Stack. 
-00003 
-00004 Copyright (C): Edouard TISSERANT and Francis DUPIN
-00005 
-00006 See COPYING file for copyrights details.
-00007 
-00008 This library is free software; you can redistribute it and/or
-00009 modify it under the terms of the GNU Lesser General Public
-00010 License as published by the Free Software Foundation; either
-00011 version 2.1 of the License, or (at your option) any later version.
-00012 
-00013 This library is distributed in the hope that it will be useful,
-00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
-00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-00016 Lesser General Public License for more details.
-00017 
-00018 You should have received a copy of the GNU Lesser General Public
-00019 License along with this library; if not, write to the Free Software
-00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
-00021 */
-00022 
-00023 #if defined(WIN32) && !defined(__CYGWIN__)
-00024 #include <windows.h>
-00025 #include "getopt.h"
-00026 void pause(void)
-00027 {
-00028         system("PAUSE");
-00029 }
-00030 #else
-00031 #include <unistd.h>
-00032 #include <stdio.h>
-00033 #include <string.h>
-00034 #include <stdlib.h>
-00035 #include <signal.h>
-00036 #endif
-00037 
-00038 #include "canfestival.h"
-00039 #include "TestMasterMicroMod.h"
-00040 #include "TestMaster.h"
-00041 UNS8 slavenodeid;
-00042 
-00043 
-00044 /*****************************************************************************/
-00045 void TestMaster_heartbeatError(UNS8 heartbeatID)
-00046 {
-00047         eprintf("TestMaster_heartbeatError %d\n", heartbeatID);
-00048 }
-00049 
-00050 /********************************************************
-00051  * ConfigureSlaveNode is responsible to
-00052  *  - setup master RPDO 1 to receive TPDO 1 from id 0x40
-00053  *  - setup master TPDO 1 to send RPDO 1 to id 0x40
-00054  ********************************************************/
-00055 void TestMaster_initialisation()
-00056 {
-00057         UNS32 PDO1_COBID = 0x0180 + slavenodeid; 
-00058         UNS32 PDO2_COBID = 0x0200 + slavenodeid;
-00059         UNS8 size = sizeof(UNS32); 
-00060 
-00061         eprintf("TestMaster_initialisation\n");
-00062 
-00063         /*****************************************
-00064          * Define RPDOs to match slave ID=0x40 TPDOs*
-00065          *****************************************/
-00066         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
-00067                         0x1400, /*UNS16 index*/
-00068                         0x01, /*UNS8 subind*/ 
-00069                         &PDO1_COBID, /*void * pSourceData,*/ 
-00070                         &size, /* UNS8 * pExpectedSize*/
-00071                         RW);  /* UNS8 checkAccess */
-00072                         
-00073 
-00074         /*****************************************
-00075          * Define TPDOs to match slave ID=0x40 RPDOs*
-00076          *****************************************/
-00077         writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
-00078                         0x1800, /*UNS16 index*/
-00079                         0x01, /*UNS8 subind*/ 
-00080                         &PDO2_COBID, /*void * pSourceData,*/ 
-00081                         &size, /* UNS8 * pExpectedSize*/
-00082                         RW);  /* UNS8 checkAccess */
-00083 }
-00084 
-00085 static init_step = 0;
-00086 
-00087 /*Froward declaration*/
-00088 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
-00089 
-00090 
-00091 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
-00092 {
-00093         UNS32 abortCode;
-00094 
-00095         if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
-00096                 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);
-00097 
-00098         /* Finalise last SDO transfer with this node */
-00099         closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);
-00100 
-00101         ConfigureSlaveNode(d, nodeId);
-00102 }
-00103 
-00104 /********************************************************
-00105  * ConfigureSlaveNode is responsible to
-00106  *  - setup slave TPDO 1 transmit time
-00107  *  - setup slave TPDO 2 transmit time
-00108  *  - setup slave Heartbeat Producer time
-00109  *  - switch to operational mode
-00110  *  - send NMT to slave
-00111  ********************************************************
-00112  * This an example of :
-00113  * Network Dictionary Access (SDO) with Callback 
-00114  * Slave node state change request (NMT) 
-00115  ********************************************************
-00116  * This is called first by TestMaster_preOperational
-00117  * then it called again each time a SDO exchange is
-00118  * finished.
-00119  ********************************************************/
-00120 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
-00121 {
-00122         UNS8 res;
-00123         eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
-00124         switch(++init_step){
-00125                 case 1: 
-00126                 {       /*disable Slave's TPDO 1 */
-00127                         UNS32 TPDO_COBId = 0x80000180 + nodeId;
-00128                         
-00129                         eprintf("Master : disable slave %2.2x TPDO 1 \n", nodeId);
-00130                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00132                                         nodeId, /*UNS8 nodeId*/
-00133                                         0x1800, /*UNS16 index*/
-00134                                         0x01, /*UNS8 subindex*/
-00135                                         4, /*UNS8 count*/
-00136                                         0, /*UNS8 dataType*/
-00137                                         &TPDO_COBId,/*void *data*/
-00138                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00139                 }                       
-00140                 break;
-00141 
-00142                 case 2: 
-00143                 {       /*setup Slave's TPDO 1 to be transmitted on SYNC*/
-00144                         UNS8 Transmission_Type = 0x01;
-00145                         
-00146                         eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
-00147                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00149                                         nodeId, /*UNS8 nodeId*/
-00150                                         0x1800, /*UNS16 index*/
-00151                                         0x02, /*UNS8 subindex*/
-00152                                         1, /*UNS8 count*/
-00153                                         0, /*UNS8 dataType*/
-00154                                         &Transmission_Type,/*void *data*/
-00155                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00156                 }                       
-00157                 break;
-00158 
-00159                 case 3: 
-00160                 {       /*re-enable Slave's TPDO 1 */
-00161                         UNS32 TPDO_COBId = 0x00000180 + nodeId;
-00162                         
-00163                         eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId);
-00164                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00166                                         nodeId, /*UNS8 nodeId*/
-00167                                         0x1800, /*UNS16 index*/
-00168                                         0x01, /*UNS8 subindex*/
-00169                                         4, /*UNS8 count*/
-00170                                         0, /*UNS8 dataType*/
-00171                                         &TPDO_COBId,/*void *data*/
-00172                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00173                 }                       
-00174                 break;
-00175                                         
-00176                 case 4: 
-00177                 {       /*disable Slave's TPDO 2 */
-00178                         UNS32 TPDO_COBId = 0x80000200 + nodeId;
-00179                         
-00180                         eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId);
-00181                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00183                                         nodeId, /*UNS8 nodeId*/
-00184                                         0x1400, /*UNS16 index*/
-00185                                         0x01, /*UNS8 subindex*/
-00186                                         4, /*UNS8 count*/
-00187                                         0, /*UNS8 dataType*/
-00188                                         &TPDO_COBId,/*void *data*/
-00189                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00190                 }                       
-00191                 break;
-00192 
-00193                                         
-00194                 case 5:
-00195                 {       
-00196                         UNS8 Transmission_Type = 0x01;
-00197                         
-00198                         eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId);
-00199                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00201                                         nodeId, /*UNS8 nodeId*/
-00202                                         0x1400, /*UNS16 index*/
-00203                                         0x02, /*UNS8 subindex*/
-00204                                         1, /*UNS8 count*/
-00205                                         0, /*UNS8 dataType*/
-00206                                         &Transmission_Type,/*void *data*/
-00207                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00208                 }       
-00209                 break;
-00210 
-00211                 case 6: 
-00212                 {       /*re-enable Slave's TPDO 1 */
-00213                         UNS32 TPDO_COBId = 0x00000200 + nodeId;
-00214                         
-00215                         eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId);
-00216                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00218                                         nodeId, /*UNS8 nodeId*/
-00219                                         0x1400, /*UNS16 index*/
-00220                                         0x01, /*UNS8 subindex*/
-00221                                         4, /*UNS8 count*/
-00222                                         0, /*UNS8 dataType*/
-00223                                         &TPDO_COBId,/*void *data*/
-00224                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00225                 }                       
-00226                 break;
-00227                 
-00228                 case 7: 
-00229                 {
-00230                         UNS16 Heartbeat_Producer_Time = 0x03E8; 
-00231                         eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId);
-00232                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00234                                         nodeId, /*UNS8 nodeId*/
-00235                                         0x1017, /*UNS16 index*/
-00236                                         0x00, /*UNS8 subindex*/
-00237                                         2, /*UNS8 count*/
-00238                                         0, /*UNS8 dataType*/
-00239                                         &Heartbeat_Producer_Time,/*void *data*/
-00240                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00241                 }                       
-00242                 break;
-00243 
-00244                 case 8: 
-00245                 {       /*disable Slave's TPDO 2 */
-00246                         UNS32 TPDO_COBId = 0x80000280 + nodeId;
-00247                         
-00248                         eprintf("Master : disable slave %2.2x TPDO 2 \n", nodeId);
-00249                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00251                                         nodeId, /*UNS8 nodeId*/
-00252                                         0x1801, /*UNS16 index*/
-00253                                         0x01, /*UNS8 subindex*/
-00254                                         4, /*UNS8 count*/
-00255                                         0, /*UNS8 dataType*/
-00256                                         &TPDO_COBId,/*void *data*/
-00257                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00258                 }                       
-00259                 break;
-00260 
-00261                 case 9: 
-00262                 {       /*disable Slave's TPDO 3 */
-00263                         UNS32 TPDO_COBId = 0x80000380 + nodeId;
-00264                         
-00265                         eprintf("Master : disable slave %2.2x TPDO 3 \n", nodeId);
-00266                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00268                                         nodeId, /*UNS8 nodeId*/
-00269                                         0x1802, /*UNS16 index*/
-00270                                         0x01, /*UNS8 subindex*/
-00271                                         4, /*UNS8 count*/
-00272                                         0, /*UNS8 dataType*/
-00273                                         &TPDO_COBId,/*void *data*/
-00274                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00275                 }
-00276                 break;                  
-00277 
-00278                 case 10: 
-00279                 {       /*disable Slave's TPDO 2 */
-00280                         UNS32 TPDO_COBId = 0x80000480 + nodeId;
-00281                         
-00282                         eprintf("Master : disable slave %2.2x TPDO 4 \n", nodeId);
-00283                         res = writeNetworkDictCallBack (d, /*CO_Data* d*/
-00285                                         nodeId, /*UNS8 nodeId*/
-00286                                         0x1803, /*UNS16 index*/
-00287                                         0x01, /*UNS8 subindex*/
-00288                                         4, /*UNS8 count*/
-00289                                         0, /*UNS8 dataType*/
-00290                                         &TPDO_COBId,/*void *data*/
-00291                                         CheckSDOAndContinue); /*SDOCallback_t Callback*/
-00292                 }                       
-00293                 break;                  
-00294                 
-00295                 case 11:
-00296                         /* Put the master in operational mode */
-00297                         setState(d, Operational);
-00298                           
-00299                         /* Ask slave node to go in operational mode */
-00300                         masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
-00301         }
-00302                         
-00303 }
-00304 
-00305 void TestMaster_preOperational()
-00306 {
-00307 
-00308         eprintf("TestMaster_preOperational\n");
-00309         ConfigureSlaveNode(&TestMaster_Data, slavenodeid);
-00310         
-00311 }
-00312 
-00313 void TestMaster_operational()
-00314 {
-00315         eprintf("TestMaster_operational\n");
-00316 }
-00317 
-00318 void TestMaster_stopped()
-00319 {
-00320         eprintf("TestMaster_stopped\n");
-00321 }
-00322 
-00323 void TestMaster_post_sync()
-00324 {
-00325         DO++;
-00326         eprintf("MicroMod Digital Out: %2.2x\n",DO);
-00327         eprintf("MicroMod Digital In (by bit): DI1: %2.2x DI2: %2.2x DI3: %2.2x DI4: %2.2x DI5: %2.2x DI6: %2.2x DI7: %2.2x DI8: %2.2x\n",DI1,DI2,DI3,DI4,DI5,DI6,DI7,DI8);
-00328 }
-00329 
-00330 void TestMaster_post_TPDO()
-00331 {
-00332 //      eprintf("TestMaster_post_TPDO\n");      
-00333 }
-00334 
-00335 //s_BOARD SlaveBoard = {"0", "500K"};
-00336 s_BOARD MasterBoard = {"32", "125K"};
-00337 
-00338 #if !defined(WIN32) || defined(__CYGWIN__)
-00339 void catch_signal(int sig)
-00340 {
-00341   signal(SIGTERM, catch_signal);
-00342   signal(SIGINT, catch_signal);
-00343   
-00344   eprintf("Got Signal %d\n",sig);
-00345 }
-00346 #endif
-00347 
-00348 void help()
-00349 {
-00350   printf("**************************************************************\n");
-00351   printf("*  TestMasterMicroMod                                        *\n");
-00352   printf("*                                                            *\n");
-00353   printf("*  A simple example for PC.                                  *\n");
-00354   printf("*  A CanOpen master that control a MicroMod module:          *\n");
-00355   printf("*  - setup module TPDO 1 transmit type                       *\n");
-00356   printf("*  - setup module RPDO 1 transmit type                       *\n");
-00357   printf("*  - setup module hearbeatbeat period                        *\n");
-00358   printf("*  - disable others TPDOs                                    *\n");
-00359   printf("*  - set state to operational                                *\n");
-00360   printf("*  - send periodic SYNC                                      *\n");
-00361   printf("*  - send periodic RPDO 1 to Micromod (digital output)       *\n");
-00362   printf("*  - listen Micromod's TPDO 1 (digital input)                *\n");
-00363   printf("*  - Mapping RPDO 1 bit per bit (digital input)              *\n");
-00364   printf("*                                                            *\n");
-00365   printf("*   Usage:                                                   *\n");
-00366   printf("*   ./TestMasterMicroMod  [OPTIONS]                          *\n");
-00367   printf("*                                                            *\n");
-00368   printf("*   OPTIONS:                                                 *\n");
-00369   printf("*     -l : Can library [\"libcanfestival_can_virtual.so\"]     *\n");
-00370   printf("*                                                            *\n");
-00371   printf("*    Slave:                                                  *\n");
-00372   printf("*     -i : Slave Node id format [0x01 , 0x7F]                *\n");
-00373   printf("*                                                            *\n");
-00374   printf("*    Master:                                                 *\n");
-00375   printf("*     -m : bus name [\"1\"]                                    *\n");
-00376   printf("*     -M : 1M,500K,250K,125K,100K,50K,20K,10K                *\n");
-00377   printf("*                                                            *\n");
-00378   printf("**************************************************************\n");
-00379 }
-00380 
-00381 /***************************  INIT  *****************************************/
-00382 void InitNodes(CO_Data* d, UNS32 id)
-00383 {
-00384         /****************************** INITIALISATION MASTER *******************************/
-00385         if(MasterBoard.baudrate){
-00386                 /* Defining the node Id */
-00387                 setNodeId(&TestMaster_Data, 0x01);
-00388 
-00389                 /* init */
-00390                 setState(&TestMaster_Data, Initialisation);
-00391         }
-00392 }
-00393 
-00394 /****************************************************************************/
-00395 /***************************  MAIN  *****************************************/
-00396 /****************************************************************************/
-00397 int main(int argc,char **argv)
-00398 {
-00399 
-00400   int c;
-00401   extern char *optarg;
-00402   char* LibraryPath="libcanfestival_can_virtual.so";
-00403   char *snodeid;
-00404   while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF)
-00405   {
-00406     switch(c)
-00407     {
-00408       case 'm' :
-00409         if (optarg[0] == 0)
-00410         {
-00411           help();
-00412           exit(1);
-00413         }
-00414         MasterBoard.busname = optarg;
-00415         break;
-00416       case 'M' :
-00417         if (optarg[0] == 0)
-00418         {
-00419           help();
-00420           exit(1);
-00421         }
-00422         MasterBoard.baudrate = optarg;
-00423         break;
-00424       case 'l' :
-00425         if (optarg[0] == 0)
-00426         {
-00427           help();
-00428           exit(1);
-00429         }
-00430         LibraryPath = optarg;
-00431         break;
-00432       case 'i' :
-00433         if (optarg[0] == 0)
-00434         {
-00435           help();
-00436           exit(1);
-00437         }
-00438         snodeid = optarg;
-00439                 sscanf(snodeid,"%x",&slavenodeid);
-00440         break;
-00441       default:
-00442         help();
-00443         exit(1);
-00444     }
-00445   }
-00446 
-00447 #if !defined(WIN32) || defined(__CYGWIN__)
-00448   /* install signal handler for manual break */
-00449         signal(SIGTERM, catch_signal);
-00450         signal(SIGINT, catch_signal);
-00451 #endif
-00452 
-00453 #ifndef NOT_USE_DYNAMIC_LOADING
-00454         LoadCanDriver(LibraryPath);
-00455 #endif          
-00456 
-00457         TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
-00458         TestMaster_Data.initialisation = TestMaster_initialisation;
-00459         TestMaster_Data.preOperational = TestMaster_preOperational;
-00460         TestMaster_Data.operational = TestMaster_operational;
-00461         TestMaster_Data.stopped = TestMaster_stopped;
-00462         TestMaster_Data.post_sync = TestMaster_post_sync;
-00463         TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
-00464         
-00465         if(!canOpen(&MasterBoard,&TestMaster_Data)){
-00466                 eprintf("Cannot open Master Board\n");
-00467                 goto fail_master;
-00468         }
-00469         
-00470         // Start timer thread
-00471         StartTimerLoop(&InitNodes);
-00472 
-00473         // wait Ctrl-C
-00474         pause();
-00475         eprintf("Finishing.\n");
-00476         
-00477         // Reset the slave node for next use (will stop emitting heartbeat)
-00478         masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);
-00479         
-00480         // Stop master
-00481         setState(&TestMaster_Data, Stopped);
-00482         
-00483         // Stop timer thread
-00484         StopTimerLoop();
-00485         
-00486 fail_master:
-00487         if(MasterBoard.baudrate) canClose(&TestMaster_Data);    
-00488 
-00489   return 0;
-00490 }
-00491 
-00492 
-