1

//---------------------------------------------------------------

     2

//

     3

//  m a i n . c

     4

//

     5

//  $LastChangedDate$

     6

//  $Author$

     7

//

     8

//---------------------------------------------------------------

     9

    10

#include <stdio.h>

    11

#include <string.h> // memset()

    12

#include <unistd.h> // usleep()

    13

#include <signal.h>

    14

    15

#include "ec_globals.h"

    16

#include "ec_master.h"

    17

    18

//---------------------------------------------------------------

    19

    20

void signal_handler(int);

    21

void write_data(unsigned char *);

    22

    23

int continue_running;

    24

unsigned short int word;

    25

    26

//---------------------------------------------------------------

    27

    28

int main(int argc, char **argv)

    29

{

    30

  EtherCAT_master_t master;

    31

  EtherCAT_command_t *cmd, *cmd2;

    32

  unsigned char data[256];

    33

  unsigned int i, number;

    34

  struct sigaction sa;

    35

    36

  sa.sa_handler = signal_handler;

    37

  sigaction(SIGINT, &sa, NULL);

    38

    39

  printf("CatEther-Testprogramm.\n");

    40

    41

  EtherCAT_master_init(&master, "eth1");

    42

    43

  if (EtherCAT_check_slaves(&master, NULL, 0) != 0)

    44

  {

    45

    fprintf(stderr, "ERROR while searching for slaves!\n");

    46

    return -1;

    47

  }

    48

    49

  if (master.slave_count == 0)

    50

  {

    51

    fprintf(stderr, "ERROR: No slaves found!\n");

    52

    return -1;

    53

  }

    54

    55

  for (i = 0; i < master.slave_count; i++)

    56

  {

    57

    printf("Slave found: Type %02X, Revision %02X, Build %04X\n",

    58

           master.slaves[i].type, master.slaves[i].revision, master.slaves[i].build);

    59

  }

    60

    61

  printf("Writing Station addresses.\n");

    62

    63

  for (i = 0; i < master.slave_count; i++)

    64

  {

    65

    data[0] = i & 0x00FF;

    66

    data[1] = (i & 0xFF00) >> 8;

    67

    68

    cmd = EtherCAT_position_write(&master, 0 - i, 0x0010, 2, data);

    69

    70

    EtherCAT_send_receive(&master);

    71

    72

    if (cmd->working_counter != 1)

    73

    {

    74

      fprintf(stderr, "ERROR: Slave did'n repond!\n");

    75

      return -1;

    76

    }

    77

    78

    EtherCAT_remove_command(&master, cmd);

    79

  }

    80

    81

  //----------

    82

    83

  for (i = 0; i < master.slave_count; i++)

    84

  {

    85

    printf("\nKlemme %i:\n", i);

    86

    87

    EtherCAT_read_slave_information(&master, i, 0x0008, &number);

    88

    printf("Vendor ID: 0x%04X (%i)\n", number, number);

    89

    90

    EtherCAT_read_slave_information(&master, i, 0x000A, &number);

    91

    printf("Product Code: 0x%04X (%i)\n", number, number);

    92

    93

    EtherCAT_read_slave_information(&master, i, 0x000E, &number);

    94

    printf("Revision Number: 0x%04X (%i)\n", number, number);

    95

  }

    96

    97

  //----------

    98

    99

  printf("\nResetting FMMU's.\n");

   100

   101

  memset(data, 0x00, 256);

   102

  cmd = EtherCAT_broadcast_write(&master, 0x0600, 256, data);

   103

  EtherCAT_send_receive(&master);

   104

   105

  if (cmd->working_counter != master.slave_count)

   106

  {

   107

    fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n",

   108

            cmd->working_counter, master.slave_count);

   109

    return -1;

   110

  }

   111

   112

  EtherCAT_remove_command(&master, cmd);

   113

   114

  //----------

   115

   116

  printf("Resetting Sync Manager channels.\n");

   117

   118

  memset(data, 0x00, 256);

   119

  cmd = EtherCAT_broadcast_write(&master, 0x0800, 256, data);

   120

  EtherCAT_send_receive(&master);

   121

   122

  if (cmd->working_counter != master.slave_count)

   123

  {

   124

    fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n",

   125

            cmd->working_counter, master.slave_count);

   126

    return -1;

   127

  }

   128

   129

  EtherCAT_remove_command(&master, cmd);

   130

   131

  //----------

   132

   133

  printf("Setting INIT state for devices.\n");

   134

   135

  if (EtherCAT_broadcast_state_change(&master, EC_STATE_INIT) != 0)

   136

  {

   137

    fprintf(stderr, "ERROR: Could not set INIT state!\n");

   138

    return -1;

   139

  }

   140

   141

  //----------

   142

   143

  printf("Setting PREOP state for bus coupler.\n");

   144

   145

  if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_PREOP) != 0)

   146

  {

   147

    fprintf(stderr, "ERROR: Could not set state!\n");

   148

    return -1;

   149

  }

   150

   151

  //----------

   152

   153

  printf("Setting Sync managers 0 and 1 of device 1.\n");

   154

   155

  data[0] = 0x00;

   156

  data[1] = 0x18;

   157

  data[2] = 0xF6;

   158

  data[3] = 0x00;

   159

  data[4] = 0x26;

   160

  data[5] = 0x00;

   161

  data[6] = 0x01;

   162

  data[7] = 0x00;

   163

  cmd = EtherCAT_write(&master, 0x0001, 0x0800, 8, data);

   164

   165

  data[0] = 0xF6;

   166

  data[1] = 0x18;

   167

  data[2] = 0xF6;

   168

  data[3] = 0x00;

   169

  data[4] = 0x22;

   170

  data[5] = 0x00;

   171

  data[6] = 0x01;

   172

  data[7] = 0x00;

   173

  cmd2 = EtherCAT_write(&master, 0x0001, 0x0808, 8, data);

   174

   175

  EtherCAT_send_receive(&master);

   176

   177

  if (cmd->working_counter != 1 || cmd2->working_counter != 1)

   178

  {

   179

    fprintf(stderr, "ERROR: Not all slaves responded!\n");

   180

   181

    return -1;

   182

  }

   183

   184

  EtherCAT_remove_command(&master, cmd);

   185

  EtherCAT_remove_command(&master, cmd2);

   186

   187

   188

  //----------

   189

   190

  printf("Setting PREOP state for device 1.\n");

   191

   192

  if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_PREOP))

   193

  {

   194

    fprintf(stderr, "ERROR: Could not set state!\n");

   195

    return -1;

   196

  }

   197

   198

  //----------

   199

   200

  printf("Setting PREOP state for device 4.\n");

   201

   202

  if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_PREOP))

   203

  {

   204

    fprintf(stderr, "ERROR: Could not set state!\n");

   205

    return -1;

   206

  }

   207

   208

  //----------

   209

   210

#if 1

   211

  printf("Setting FMMU 0 of device 1.\n");

   212

   213

  data[0] = 0x00; // Logical start address [4]

   214

  data[1] = 0x00;

   215

  data[2] = 0x00;

   216

  data[3] = 0x00;

   217

  data[4] = 0x04; // Length [2]

   218

  data[5] = 0x00;

   219

  data[6] = 0x00; // Start bit

   220

  data[7] = 0x07; // End bit

   221

  data[8] = 0x00; // Physical start address [2]

   222

  data[9] = 0x10;

   223

  data[10] = 0x00; // Physical start bit

   224

  data[11] = 0x02; // Read/write enable

   225

  data[12] = 0x01; // channel enable [2]

   226

  data[13] = 0x00;

   227

  data[14] = 0x00; // Reserved [2]

   228

  data[15] = 0x00;

   229

  cmd = EtherCAT_write(&master, 0x0001, 0x0600, 16, data);

   230

  EtherCAT_send_receive(&master);

   231

   232

  if (cmd->working_counter != 1)

   233

  {

   234

    fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",

   235

            cmd->working_counter);

   236

    return -1;

   237

  }

   238

   239

  EtherCAT_remove_command(&master, cmd);

   240

#endif

   241

   242

  //----------

   243

   244

#if 1

   245

  printf("Setting FMMU 0 of device 4.\n");

   246

   247

  data[0] = 0x04; // Logical start address [4]

   248

  data[1] = 0x00;

   249

  data[2] = 0x00;

   250

  data[3] = 0x00;

   251

  data[4] = 0x01; // Length [2]

   252

  data[5] = 0x00;

   253

  data[6] = 0x00; // Start bit

   254

  data[7] = 0x07; // End bit

   255

  data[8] = 0x00; // Physical start address [2]

   256

  data[9] = 0x0F;

   257

  data[10] = 0x00; // Physical start bit

   258

  data[11] = 0x02; // Read/write enable

   259

  data[12] = 0x01; // channel enable [2]

   260

  data[13] = 0x00;

   261

  data[14] = 0x00; // Reserved [2]

   262

  data[15] = 0x00;

   263

  cmd = EtherCAT_write(&master, 0x0004, 0x0600, 16, data);

   264

  EtherCAT_send_receive(&master);

   265

   266

  if (cmd->working_counter != 1)

   267

  {

   268

    fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",

   269

            cmd->working_counter);

   270

    return -1;

   271

  }

   272

   273

  EtherCAT_remove_command(&master, cmd);

   274

#endif

   275

   276

  //----------

   277

   278

  printf("Setting Sync manager 2 of device 1.\n");

   279

   280

  data[0] = 0x00;

   281

  data[1] = 0x10;

   282

  data[2] = 0x04;

   283

  data[3] = 0x00;

   284

  data[4] = 0x24;

   285

  data[5] = 0x00;

   286

  data[6] = 0x01;

   287

  data[7] = 0x00;

   288

  cmd = EtherCAT_write(&master, 0x0001, 0x0810, 8, data);

   289

  EtherCAT_send_receive(&master);

   290

   291

  if (cmd->working_counter != 1)

   292

  {

   293

    fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", cmd->working_counter);

   294

    return -1;

   295

  }

   296

   297

  EtherCAT_remove_command(&master, cmd);

   298

   299

  //----------

   300

   301

  printf("Setting Sync manager 0 for device 4.\n");

   302

   303

  data[0] = 0x00;

   304

  data[1] = 0x0F;

   305

  data[2] = 0x01;

   306

  data[3] = 0x00;

   307

  data[4] = 0x46; // 46

   308

  data[5] = 0x00;

   309

  data[6] = 0x01;

   310

  data[7] = 0x00;

   311

  cmd = EtherCAT_write(&master, 0x0004, 0x0800, 8, data);

   312

   313

  EtherCAT_send_receive(&master);

   314

   315

  if (cmd->working_counter != 1)

   316

  {

   317

    fprintf(stderr, "ERROR: Not all slaves responded!\n");

   318

   319

    return -1;

   320

  }

   321

   322

  EtherCAT_remove_command(&master, cmd);

   323

   324

  //----------

   325

   326

  printf("Setting SAVEOP state for bus coupler.\n");

   327

   328

  if (EtherCAT_state_change(&master, 0x0000, EC_STATE_SAVEOP) != 0)

   329

  {

   330

    fprintf(stderr, "ERROR: Could not set state!\n");

   331

    return -1;

   332

  }

   333

   334

  //----------

   335

   336

  printf("Setting SAVEOP state for device 1.\n");

   337

   338

  if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_SAVEOP) != 0)

   339

  {

   340

    fprintf(stderr, "ERROR: Could not set state!\n");

   341

    return -1;

   342

  }

   343

   344

  //----------

   345

   346

  printf("Setting SAVEOP state for device 4.\n");

   347

   348

  if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_SAVEOP) != 0)

   349

  {

   350

    fprintf(stderr, "ERROR: Could not set state!\n");

   351

    return -1;

   352

  }

   353

   354

  //----------

   355

   356

  printf("Setting OP state for bus coupler.\n");

   357

   358

  if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_OP) != 0)

   359

  {

   360

    fprintf(stderr, "ERROR: Could not set state!\n");

   361

    return -1;

   362

  }

   363

   364

  //----------

   365

   366

  printf("Setting OP state for device 1.\n");

   367

   368

  if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_OP) != 0)

   369

  {

   370

    fprintf(stderr, "ERROR: Could not set state!\n");

   371

    return -1;

   372

  }

   373

   374

  //----------

   375

   376

  printf("Setting OP state for device 4.\n");

   377

   378

  if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_OP) != 0)

   379

  {

   380

    fprintf(stderr, "ERROR: Could not set state!\n");

   381

    return -1;

   382

  }

   383

   384

  //----------

   385

   386

  word = 0;

   387

   388

  printf("Starting thread...\n");

   389

   390

  if (EtherCAT_start(&master, 5, write_data, NULL, 10000) != 0)

   391

  {

   392

    return -1;

   393

  }

   394

   395

  continue_running = 1;

   396

   397

  while (continue_running)

   398

  {

   399

    usleep(200000);

   400

   401

    word += 1000;

   402

    word = word & 0x7FFF;

   403

  }

   404

   405

  //----------

   406

   407

  printf("Stopping master thread...\n");

   408

  EtherCAT_stop(&master);

   409

   410

  EtherCAT_master_clear(&master);

   411

   412

  printf("Finished.\n");

   413

   414

  return 0;

   415

}

   416

   417

//---------------------------------------------------------------

   418

   419

void write_data(unsigned char *data)

   420

{

   421

  data[0] = word & 0xFF;

   422

  data[1] = (word & 0xFF00) >> 8;

   423

  data[2] = word & 0xFF;

   424

  data[3] = (word & 0xFF00) >> 8;

   425

   426

  data[4] = 0x01;

   427

}

   428

   429

//---------------------------------------------------------------

   430

   431

void signal_handler(int signum)

   432

{

   433

  if (signum == SIGINT || signum == SIGTERM)

   434

  {

   435

    continue_running = 0;

   436

  }

   437

}

   438

   439

//---------------------------------------------------------------