Servo control issue, relay clicking, beginner

[SamuelJamesFisher]

Hello, I'm new to SOEM, trying to control actuator with a Panasonic minas a6 servo driver. My issue is that the relay is clicking on and off, I know there will be something simple wrong in my code which i have been bashing my head on for the last week. Please could you take some time out of your day and have a look at this hideous beast.

void CALLBACK RTthread(UINT uTimerID, UINT uMsg, DWORD_PTR dwUser, DWORD_PTR dw1, DWORD_PTR dw2)
{

    if (inOP)
    {

        enableSlave(1);
        slaveOutputArray[1]->targetPos = 10;
        homing(1);
    }

    IOmap[0]++;

    ec_send_processdata();
    wkc = ec_receive_processdata(EC_TIMEOUTSAFE);
    rtcnt++;

    //runMotor();
    //15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0
}

bool enableSlave(int slave)
{
        //int sizeOfInt = sizeof(uint16_t);
        //uint16_t test;

        printf("wkc %d", wkc);

        uint16_t buffer = slaveInputArray[slave]->StatusWord; //ec_slave[1].inputs[1];

        for (int i = 15; i >= 0; --i) {
            printf("%d", (bool)(buffer & (1U << i)));
        }

        printf("\nstatusword = %#x\n", (buffer & 0x4f));
        printf("statusword = %#x\n", (buffer & 0x6f));

        //0123 4567 89 10 11 12
        //0000 0010 00 1  1  1 000
        //15 14 13 12 11 10 98 7654 3210
        //0  0  0  1  0  1  10 001 1 0111

        if ((buffer & 0x4F) == 0x00)
        {
            printf("off 1 wait\n");
        }
        else if ((buffer & 0x4F) == 0x40)
        {
            printf("switch on disabled\n");
            slaveOutputArray[slave]->controlWord = std::uint16_t(0x06);
        }
        else if ((buffer & 0x6F) == 0x21)
        {
            printf("ready to switch on\n");
            slaveOutputArray[slave]->controlWord = std::uint16_t(0x07);
        }
        else if ((buffer & 0x6F) == 0x23)
        {
            printf("enable op\n");

            //slaveOutputArray[slave]->targetPos = slaveInputArray[slave]->pos + 20;

            slaveOutputArray[slave]->controlWord = std::uint16_t(0x0F);
        }
        else if ((buffer & 0x6F) == 0x27)
        {
            printf("------------WE ARE A GO SIR READY FOR MOVEMENT-------------------\n");

            return true;
        }
        else if ((buffer & 0x6F) == 0x07)
        {
            printf("Quick stop active\n");
            slaveOutputArray[slave]->controlWord = std::uint16_t(0x00);
        }
        else if ((buffer & 0x4F) == 0x0F)
        {
            printf("fault fixing\n");
            slaveOutputArray[slave]->controlWord = std::uint16_t(0x00);
        }
        else if ((buffer & 0x4F) == 0x08)
        {
            printf("fault\n");

            //ec_sync

            slaveOutputArray[slave]->controlWord = std::uint16_t(0x80);//std::uint16_t(0x80); //0x6040

        }
        else
        {
            printf("OTHER????\n");
        }

    return false;

}
int homing(int slave)
{
    uint16_t buffer1 = slaveInputArray[slave]->StatusWord;

    if (needClear && ((buffer1 & 0x6F) != 0x27))
    {
        printf("home1\n");
        homeCount = 0;
        needClear = true;
        return -1;
    }
    else if (slaveInputArray[slave]->ModesOfOp != 6)
    {
        printf("Setting MODE\n");

        slaveOutputArray[slave]->modeOfOp = 6;
    }

    else if (needClear)
    {
        printf("Setting Clear\n");

        if (buffer1 & 0x3400)
        {
            if (++homeCount % 20 < 10)
            {
                printf("Clear 1\n");
                slaveOutputArray[slave]->controlWord = std::uint16_t(0x1F);
            }
            else
            {
                printf("Clear 2\n");
                slaveOutputArray[slave]->controlWord = std::uint16_t(0x0F);
            }
        }
        else
        {
            printf("Clear 3\n");
            slaveOutputArray[slave]->controlWord = std::uint16_t(0x0F);
            needClear = false;
        }

        return 3;
    }
    else if ((buffer1 & 0x3400) == 0x0000)
    {
        printf("start the home\n");
        slaveOutputArray[slave]->controlWord = std::uint16_t(0x1F);
        return 4;
    }
    // home attained //
    else if ((buffer1 & 0x3400) == 0x1400)
    {
        printf("home got\n");
        slaveOutputArray[slave]->controlWord = std::uint16_t(0x0F);
        homeCount = 0;
        needClear = true;
        return 0;
    }
    // home error //
    else if (buffer1 & 0x2000)
    {
        printf("home ERROR\n");
        homeCount = 0;
        needClear = true;
        return -2;
    }
    // homing ... //
    else
    {
        printf("We should be homing right now\n");
        return -3;
    }
    return -10;
}

if (!findCorrectInterface())
    {
        printf("Couldnt find the slaves sir, terminating program\n");
        ec_close();
        std::getchar();
        return 0;
    }

    osal_thread_create(&errorThread, 128000, &ecatcheck, (void*)&ctime);

    /*
    */
    int oloop, iloop;
    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);

    int chk = 200;

    printf("%d slaves found and configured.\n", ec_slavecount);

    printf("Slaves mapped, state to SAFE_OP.\n");

    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);

    slaveInputArray[0] = (slaveInput*)ec_slave[1].inputs;
    slaveInputArray[1] = (slaveInput*)ec_slave[2].inputs;

    slaveOutputArray[0] = (slaveOutput*)ec_slave[1].outputs;
    slaveOutputArray[1] = (slaveOutput*)ec_slave[2].outputs;

    /* send one valid process data to make outputs in slaves happy*/
    ec_send_processdata();
    wkc = ec_receive_processdata(EC_TIMEOUTRET);

    UINT mmResult;
    mmResult = timeSetEvent(1, 0, RTthread, 0, TIME_PERIODIC);

    ec_writestate(0);
    chk = 200;
    /* 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_SAFE_OP)
    {
        ec_slave[0].state = EC_STATE_OPERATIONAL;
        ec_writestate(0);
    }
    chk = 200;
    do
    {
        ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
    } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));

    ec_writestate(0);

    if (ec_slave[0].state == EC_STATE_OPERATIONAL)
    {
        if (ec_slave[0].hasdc)
        {
            printf("SYNC ME-----------------------------");
            //ec_sync(ec_DCtime, cycletime, &toff);
        }

        //slaveOutputArray[0]->modeOfOp = 6;
        //slaveOutputArray[1]->modeOfOp = 6;

        needClear = true;
        homeCount = 0;
        inOP = true;

        printf("Slaves ready for commands sir\n");

typedef struct PACKED
{
    uint16_t        ErrorCode;
    uint16_t        StatusWord;
    int8            ModesOfOp;
    int32           pos;
    uint16_t        touchProbeStatus;
    int32           touchProbePos1;
    int32           followingError;
    uint32          DigitalInputs;
} slaveInput;

typedef struct PACKED
{
    uint16_t        controlWord;
    int8            modeOfOp;
    int32           targetPos;
    uint16_t        touchProbe;
} slaveOutput;

ecinit on \Device\NPF{BB694750-FBA8-4150-A229-0B3599800059} succeeded. 2 slaves found and configured. Calculated workcounter 6

Slave:1 Name:MDDLN55BE Output size: 72bits Input size: 184bits State: 4 Delay: 0[ns] Has DC: 1 DCParentport:0 Activeports:1.1.0.0 Configured address: 1001 Man: 0000066f ID: 60380009 Rev: 00010000 SM0 A:1000 L: 256 F:00010026 Type:1 SM1 A:1200 L: 256 F:00010022 Type:2 SM2 A:1400 L: 9 F:00010064 Type:3 SM3 A:1600 L: 23 F:00010020 Type:4 FMMU0 Ls:00000000 Ll: 9 Lsb:0 Leb:7 Ps:1400 Psb:0 Ty:02 Act:01 FMMU1 Ls:00000012 Ll: 23 Lsb:0 Leb:7 Ps:1600 Psb:0 Ty:01 Act:01 FMMUfunc 0:1 1:2 2:3 3:0 MBX length wr: 256 rd: 256 MBX protocols : 04 CoE details: 0f FoE details: 00 EoE details: 00 SoE details: 00 Ebus current: 0[mA] only LRD/LWR:0 PDO mapping according to CoE : SM2 outputs addr b index: sub bitl data_type name [0x0000.0] 0x6040:0x00 0x10 UNSIGNED16 Controlword [0x0002.0] 0x6060:0x00 0x08 INTEGER8 Modes of operation [0x0003.0] 0x607A:0x00 0x20 INTEGER32 Target position [0x0007.0] 0x60B8:0x00 0x10 UNSIGNED16 Touch probe function SM3 inputs addr b index: sub bitl data_type name [0x0012.0] 0x603F:0x00 0x10 UNSIGNED16 Error code [0x0014.0] 0x6041:0x00 0x10 UNSIGNED16 Statusword [0x0016.0] 0x6061:0x00 0x08 INTEGER8 Modes of operation display [0x0017.0] 0x6064:0x00 0x20 INTEGER32 Position actual value [0x001B.0] 0x60B9:0x00 0x10 UNSIGNED16 Touch probe status [0x001D.0] 0x60BA:0x00 0x20 INTEGER32 Touch probe pos1 pos value [0x0021.0] 0x60F4:0x00 0x20 INTEGER32 Following error actual value [0x0025.0] 0x60FD:0x00 0x20 UNSIGNED32 Digital inputs

Slave:2 Name:MDDLN55BE Output size: 72bits Input size: 184bits State: 4 Delay: 660[ns] Has DC: 1 DCParentport:1 Activeports:1.0.0.0 Configured address: 1002 Man: 0000066f ID: 60380009 Rev: 00010000 SM0 A:1000 L: 256 F:00010026 Type:1 SM1 A:1200 L: 256 F:00010022 Type:2 SM2 A:1400 L: 9 F:00010064 Type:3 SM3 A:1600 L: 23 F:00010020 Type:4 FMMU0 Ls:00000009 Ll: 9 Lsb:0 Leb:7 Ps:1400 Psb:0 Ty:02 Act:01 FMMU1 Ls:00000029 Ll: 23 Lsb:0 Leb:7 Ps:1600 Psb:0 Ty:01 Act:01 FMMUfunc 0:1 1:2 2:3 3:0 MBX length wr: 256 rd: 256 MBX protocols : 04 CoE details: 0f FoE details: 00 EoE details: 00 SoE details: 00 Ebus current: 0[mA] only LRD/LWR:0 PDO mapping according to CoE : SM2 outputs addr b index: sub bitl data_type name [0x0009.0] 0x6040:0x00 0x10 UNSIGNED16 Controlword [0x000B.0] 0x6060:0x00 0x08 INTEGER8 Modes of operation [0x000C.0] 0x607A:0x00 0x20 INTEGER32 Target position [0x0010.0] 0x60B8:0x00 0x10 UNSIGNED16 Touch probe function SM3 inputs addr b index: sub bitl data_type name [0x0029.0] 0x603F:0x00 0x10 UNSIGNED16 Error code [0x002B.0] 0x6041:0x00 0x10 UNSIGNED16 Statusword [0x002D.0] 0x6061:0x00 0x08 INTEGER8 Modes of operation display [0x002E.0] 0x6064:0x00 0x20 INTEGER32 Position actual value [0x0032.0] 0x60B9:0x00 0x10 UNSIGNED16 Touch probe status [0x0034.0] 0x60BA:0x00 0x20 INTEGER32 Touch probe pos1 pos value [0x0038.0] 0x60F4:0x00 0x20 INTEGER32 Following error actual value [0x003C.0] 0x60FD:0x00 0x20 UNSIGNED32 Digital inputs End slaveinfo, close socket End program

[nakarlsson]

One common mistake for this sample code is not removing the cyclicly adding of ++ to IOmap byte 0. For you it means the controlword of drive 1 will alter every RTthread call. If you also experience issues with drive 2, a wireshark would help, NOTE: Windows is not ideal when it comes to realtime, and you might experience "glitches" in communcation on NICs.

void CALLBACK RTthread(UINT uTimerID, UINT uMsg, DWORD_PTR dwUser, DWORD_PTR dw1, DWORD_PTR dw2) .. IOmap[0]++; <--- this will alter your slave 1 control word

[nakarlsson]

@SamuelJamesFisher, Still a problem?

[SamuelJamesFisher]

Sorry for the delay. I have solved it. "IOmap[0]++" Was not helping. Other things I learned to help others out, Don't bother trying to do sync or any Realtime stuff with a windows laptop, Panasonic minas a6 servo driver and probably others need constant communication, READ READ the servo driver ethercat manual THOROUGHLY there could be a small line that you hadn't done which causes the whole thing not to work.