Program Listing for File torque_control_executable.cpp

Return to documentation for file (torque_control_executable.cpp)

#include <inttypes.h>
#include <stdio.h>
#include <string.h>

#include "ethercat.h"

#define EC_TIMEOUTMON 500

char IOmap[4096];
OSAL_THREAD_HANDLE thread1;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;

/* define pointer structure */
#pragma pack(1)
typedef struct {
  uint16_t Statusword;
  int8_t OpModeDisplay;
  int32_t PositionValue;
  int32_t VelocityValue;
  int16_t TorqueValue;
  uint16_t AnalogInput1;
  uint16_t AnalogInput2;
  uint16_t AnalogInput3;
  uint16_t AnalogInput4;
  uint32_t TuningStatus;
  uint32_t DigitalInputs;
  uint32_t UserMISO;
  uint32_t Timestamp;
  int32_t PositionDemandInternalValue;
  int32_t VelocityDemandValue;
  int16_t TorqueDemand;
} in_somanet_50t;

typedef struct {
  uint16_t Controlword;
  int8_t OpMode;
  int16_t TargetTorque;
  int32_t TargetPosition;
  int32_t TargetVelocity;
  int16_t TorqueOffset;
  int32_t TuningCommand;
  int32_t PhysicalOutputs;
  int32_t BitMask;
  int32_t UserMOSI;
  int32_t VelocityOffset;
} out_somanet_50t;
#pragma pack()

void simpletest(const char *ifname) {
  int i, j, 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);

      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 = 200;
      // 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");
        inOP = true;

        j = 0;
        // create and connect struture pointers to I/O
        in_somanet_50t *in_somanet_1;
        in_somanet_1 = (in_somanet_50t *)ec_slave[0].inputs;
        out_somanet_50t *out_somanet_1;
        out_somanet_1 = (out_somanet_50t *)ec_slave[0].outputs;

        // cyclic loop
        for (i = 1; i <= 10000; i++) {
          ec_send_processdata();
          wkc = ec_receive_processdata(EC_TIMEOUTRET);

          if (wkc >= expectedWKC) {
            j++;
            // Profile torque mode
            if (j == 1)
              out_somanet_1->OpMode = 4;

            // Fault reset: Fault -> Switch on disabled, if the drive is in fault
            // state
            if ((in_somanet_1->Statusword & 0b0000000001001111) ==
                0b0000000000001000)
              out_somanet_1->Controlword = 0b10000000;

            // Shutdown: Switch on disabled -> Ready to switch on
            else if ((in_somanet_1->Statusword & 0b0000000001001111) ==
                     0b0000000001000000)
              out_somanet_1->Controlword = 0b00000110;

            // Switch on: Ready to switch on -> Switched on
            else if ((in_somanet_1->Statusword & 0b0000000001101111) ==
                     0b0000000000100001)
              out_somanet_1->Controlword = 0b00000111;

            // Enable operation: Switched on -> Operation enabled
            else if ((in_somanet_1->Statusword & 0b0000000001101111) ==
                     0b0000000000100011)
              out_somanet_1->Controlword = 0b00001111;

            // Sending torque command
            else if ((in_somanet_1->Statusword & 0b0000000001101111) ==
                     0b0000000000100111)
              out_somanet_1->TargetTorque = 0;

            // printf("Processdata cycle %4d , WKC %d ,", i, wkc);
            // printf(" Statusword: %X ,", in_somanet_1->Statusword);
            // printf(" Op Mode Display: %d ,", in_somanet_1->OpModeDisplay);
            printf(" ActualPos: %" PRId32 " ,", in_somanet_1->PositionValue);
            // printf(" ActualVel: %" PRId32 " ,", in_somanet_1->VelocityValue);
            // printf(" DemandVel: %" PRId32 " ,",
            // in_somanet_1->VelocityDemandValue);
            // printf(" ActualTorque: %" PRId32 " ,",
            // in_somanet_1->TorqueValue);
            printf(" DemandTorque: %" PRId32 " ,", in_somanet_1->TorqueDemand);

            printf(" T:%" PRId64 "\r", ec_DCtime);
            needlf = true;
          }
          osal_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);
  }
}

OSAL_THREAD_FUNC ecatcheck(void *ptr) {
  int slave;
  (void)ptr; // unused

  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 > EC_STATE_NONE) {
            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_STATE_NONE) {
              ec_slave[slave].islost = true;
              printf("ERROR : slave %d lost\n", slave);
            }
          }
        }
        if (ec_slave[slave].islost) {
          if (ec_slave[slave].state == EC_STATE_NONE) {
            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");
    }
    osal_usleep(10000);
  }
}

int main(int /*argc*/, char * /*argv*/[]) {
  printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");

  // create thread to handle slave error handling in OP
  osal_thread_create(&thread1, 128000, (void *)&ecatcheck, (void *)&ctime);
  // start cyclic part
  const char interface_name[] = "eno0";
  simpletest(interface_name);

  printf("End program\n");
  return (0);
}