[etherlab-users] Ds402 or SoE

[Florian Pose]

On Tue, Jun 16, 2009 at 04:53:39PM -0800, Henry Bausley wrote:
> Are there any examples of communicating to a drive using either the
> CAN Ds402 or SoE out there? 

Yes, we had a few Lenze 9400 Servo drives with DS402 profile in an
application. Here are some code snippets:

const static ec_pdo_entry_reg_t domain_out_regs[] = {
    {SpindleDrivePos, Lenze_E94AYCET,  0x6040, 0, &off_SpindleControlWord},
    {SpindleDrivePos, Lenze_E94AYCET,  0x60FF, 0, &off_SpindleTargetVelocity},
    {SpindleDrivePos, Lenze_E94AYCET,  0x60FE, 1, &off_SpindleDigOut},
    {}
};

const static ec_pdo_entry_reg_t domain_in_regs[] = {
    {SpindleDrivePos, Lenze_E94AYCET,  0x6041, 0, &off_SpindleStatusWord},
    {SpindleDrivePos, Lenze_E94AYCET,  0x606C, 0, &off_SpindleCurrentVelocity},
    {SpindleDrivePos, Lenze_E94AYCET,  0x6064, 0, &off_SpindleCurrentPosition},
    {SpindleDrivePos, Lenze_E94AYCET,  0x6078, 0, &off_SpindleCurrentCurrent},
    {SpindleDrivePos, Lenze_E94AYCET,  0x60FD, 0, &off_SpindleDigIn},
    {}
};

static ec_pdo_entry_info_t lenze_output_pdo_entries[] = {
    {0x60FF, 0, 32}, // DS402 Target Velocity
    {0x6040, 0, 16}, // DS402 Controlword
    {0x60FE, 1,  8}, // Digital Outputs
};

static ec_pdo_entry_info_t lenze_input_pdo_entries[] = {
    {0x606C, 0, 32}, // DS402 Velocity Actual Value
    {0x6064, 0, 32}, // DS402 Position Actual Value
    {0x6078, 0, 32}, // DS402 Current Actual Value
    {0x6041, 0, 16}, // DS402 Statusword
    {0x60FD, 0,  8}, // Digital Inputs
};

static ec_pdo_info_t lenze_pdo_1600[] = {
    {0x1600, 3, lenze_output_pdo_entries}
};

static ec_pdo_info_t lenze_pdo_1a00[] = {
    {0x1A00, 5, lenze_input_pdo_entries},
};

static ec_sync_info_t lenze_syncs[] = {
    {2, EC_DIR_OUTPUT, 1, lenze_pdo_1600},
    {3, EC_DIR_INPUT,  1, lenze_pdo_1a00},
};


/** DS402 profile handler for velocity mode.
 *
 * This is Simulink auto-generated code, so a little bit cryptic, sorry! ;-)
 */
void update_DS402_Velocity(
        struct DS402_Velocity_signal *s, /**< Signals. */
        const struct DS402_Velocity_param *p, /**< Parameters. */
        uint8_T getReady,
        uint8_T run,
        uint8_T quickStop,
        uint32_T targetVelocity
        )
{
  uint8_T ReadytoSwitchOn = (s->StatusWord & 0x0001) != 0;
  uint8_T SwitchedOn = (s->StatusWord & 0x0002) != 0;
  uint8_T VoltageEnabled = (s->StatusWord & 0x0010) != 0;
  uint8_T SwitchOnDisabled = (s->StatusWord & 0x0040) != 0;

  uint8_T LogicalOperator2 = ReadytoSwitchOn || SwitchOnDisabled;
  uint8_T LogicalOperator4 = getReady && VoltageEnabled && LogicalOperator2;
  uint8_T LogicalOperator3 = ReadytoSwitchOn && LogicalOperator4;
  uint8_T LogicalOperator6 = LogicalOperator3 && SwitchedOn && run;
  uint8_T LogicalOperator1 = !(getReady || run);
  uint8_T LogicalOperator = LogicalOperator1 && p->DriveReset;

  uint16_T shift_m = LogicalOperator3;
  uint16_T shift_g = (uint16_T)(LogicalOperator4 << 1U);
  uint16_T shift = (uint16_T)(!quickStop << 2U);
  uint16_T shift_j = (uint16_T)(LogicalOperator6 << 3U);
  uint16_T shift_l = (uint16_T)(LogicalOperator << 7U);

  s->ControlWord = (shift_m) | (shift_g) | (shift) | (shift_j) | (shift_l);
  s->TargetVelocity = targetVelocity;

  s->Fault = (s->StatusWord & 0x0008) != 0;
  s->Ready = (s->StatusWord & 0x0004) != 0;
  s->Warning = (s->StatusWord & 0x0080) != 0;
}

-- 
Best regards,
Florian Pose

http://etherlab.org