Hi,
I'm having quite a bit of trouble here, trying to make a modified and
working mini.c example for our omnidirectional robot base, see
http://hermann-uwe.de/blog/dancing-omnidirectional-robot-base-controlled-via-linux-shellscripts-based-on-igh-ethercat-master
After various problems with the kernel module building I now have a
sort-of working mini.c module, but I cannot get the four slaves (motors)
of the robot to work in synchronous mode.
See below for what I did so far and output from the relevant commands.
I guess I'm doing something fundamentally stupid somewhere, but I just
cannot figure out what exactly that is. My current guess for the root
cause of the problems are these errors:
EtherCAT WARNING: PDOs configured for SM2, but slave 1 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 1!
EtherCAT WARNING: PDOs configured for SM2, but slave 2 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 2!
EtherCAT WARNING: PDOs configured for SM2, but slave 3 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 3!
EtherCAT WARNING: PDOs configured for SM2, but slave 0 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 0!
I'm not sure how to fix it, though. Maybe some EEPROM contents are
bogus? Using TwinCAT on Windows the synchorous mode operation seems to
work fine from what I can tell.
See also the attached mini.c for the test code I currently use.
Here's some more output:
$ uname -a
Linux greenwood 2.6.24-etchnhalf.1-686 #1 SMP Fri Dec 26 04:10:16 UTC 2008 i686
GNU/Linux
$ lspci | grep Ethernet # e1000 driver
02:01.0 Ethernet controller: Intel Corporation 82540EP Gigabit Ethernet
Controller (Mobile) (rev 03)
$ tar xfvj ethercat-1.4.0.tar.bz2
$ cd ethercat-1.4.0
$ ./configure --enable-e1000
$ make all modules
$ make modules_install install
$ depmod
$ cat /etc/sysconfig/ethercat
MASTER0_DEVICE="00:11:22:33:44:55"
DEVICE_MODULES="e1000"
#MODPROBE_FLAGS="-b"
[Start the robot, wait 10 seconds or so, just in case]
$ /opt/etherlab/etc/init.d/ethercat start
[wait 10 seconds]
$ dmesg
EtherCAT: Master driver 1.4.0 r1611
EtherCAT: 1 master waiting for devices.
ACPI: PCI interrupt for device 0000:02:01.0 disabled
EtherCAT Intel(R) PRO/1000 Network Driver - version 7.3.20-k2-NAPI
Copyright (c) 1999-2006 Intel Corporation.
ACPI: PCI Interrupt 0000:02:01.0[A] -> Link [LNKA] -> GSI 11 (level, low) ->
IRQ 11
ec_e1000: 0000:02:01.0: e1000_probe: (PCI:33MHz:32-bit) 00:11:22:33:44:55
EtherCAT: Accepting device 00:11:22:33:44:55 for master 0.
EtherCAT: Starting EtherCAT-IDLE thread.
ec_e1000: ec0: e1000_probe: Intel(R) PRO/1000 Network Connection
ec_e1000: ec0: e1000_watchdog: NIC Link is Up 100 Mbps Full Duplex, Flow
Control: RX
EtherCAT: Link state changed to UP.
EtherCAT: 4 slave(s) responding.
EtherCAT: Slave states: PREOP.
EtherCAT: Scanning bus.
EtherCAT: Bus scanning completed in 80 ms.
EtherCAT: Starting EoE processing.
EtherCAT ERROR: Timeout while waiting for SDO dictionary list response on slave
0.
EtherCAT ERROR: Timeout while waiting for SDO dictionary list response on slave
1.
EtherCAT ERROR: Timeout while waiting for SDO dictionary list response on slave
2.
EtherCAT ERROR: Timeout while waiting for SDO dictionary list response on slave
3.
$ cd examples/mini
[copy our mini.c over the original one from the tarball]
$ make clean
$ cp ../../Module.symvers . # Needed to work around a kbuild bug.
$ make modules
$ make modules_install
$ depmod
[the activity LEDs on the 4 slaves are blinking frantically, the (CANOpen?)
state is "1", as shown on the 4 boxes via a charater display]
$ /opt/etherlab/bin/ethercat master -v
Master0
Phase: Idle
Slaves: 4
Device0: 00:11:22:33:44:55 (attached)
Tx count: 636468
Rx count: 636467
Device1: None.
$ /opt/etherlab/bin/ethercat slaves -v
=== Slave 0 ===
Alias: 1001
State: PREOP
Flag: +
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
=== Slave 1 ===
Alias: 1002
State: PREOP
Flag: +
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
=== Slave 2 ===
Alias: 1003
State: PREOP
Flag: +
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
=== Slave 3 ===
Alias: 1004
State: PREOP
Flag: +
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
$ /opt/etherlab/bin/ethercat config -v
[no output]
$ /opt/etherlab/bin/ethercat sii_read -v -p 0
SII Area:
08 00 02 04 f0 00 00 00 e9 03 00 00 00 00 d1 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
$ /opt/etherlab/bin/ethercat sii_read -v -p 1
SII Area:
08 00 02 04 f0 00 00 00 ea 03 00 00 00 00 aa 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
$ /opt/etherlab/bin/ethercat sii_read -v -p 2
SII Area:
08 00 02 04 f0 00 00 00 eb 03 00 00 00 00 83 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
$ /opt/etherlab/bin/ethercat sii_read -v -p 3
SII Area:
08 00 02 04 f0 00 00 00 ec 03 00 00 00 00 5c 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
$ /opt/etherlab/bin/ethercat domains -v
[no output]
$ /opt/etherlab/bin/ethercat pdos -v
=== Slave 0 ===
=== Slave 1 ===
=== Slave 2 ===
=== Slave 3 ===
$ /opt/etherlab/bin/ethercat sdos -v
=== Slave 0 ===
=== Slave 1 ===
=== Slave 2 ===
=== Slave 3 ===
$ modprobe ec_mini
[state as per display is still "1" for each slave]
ec_omnidrive: Starting...
EtherCAT: Requesting master 0...
EtherCAT: Successfully requested master 0.
ec_omnidrive: Registering domain...
ec_omnidrive: Configuring PDOs for motor0...
ec_omnidrive: Configuring PDOs for motor1...
ec_omnidrive: Configuring PDOs for motor2...
ec_omnidrive: Configuring PDOs for motor3...
ec_omnidrive: Registering PDO entries...
ec_omnidrive: Activating master...
EtherCAT: Domain0: Logical address 0x00000000, 8 byte, expected working counter
4.
EtherCAT: Datagram domain0-0: Logical offset 0x00000000, 8 byte, type LWR.
EtherCAT: Stopping EoE processing.
EtherCAT: Master thread exited.
EtherCAT: Starting EtherCAT-OP thread.
EtherCAT: Starting EoE processing.
ec_omnidrive: Starting cyclic sample thread.
ec_omnidrive: Started.
ec_omnidrive: Motor0 velocity: 0.
ec_omnidrive: 4 slave(s).
ec_omnidrive: AL states: 0x02.
ec_omnidrive: Link is up.
ec_omnidrive: motor0: State 0x02.
ec_omnidrive: motor0: online.
ec_omnidrive: motor1: State 0x02.
ec_omnidrive: motor1: online.
ec_omnidrive: motor2: State 0x02.
ec_omnidrive: motor2: online.
ec_omnidrive: motor3: State 0x02.
ec_omnidrive: motor3: online.
EtherCAT WARNING: PDOs configured for SM2, but slave 1 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 1!
EtherCAT WARNING: PDOs configured for SM2, but slave 2 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 2!
EtherCAT WARNING: PDOs configured for SM2, but slave 3 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 3!
EtherCAT WARNING: PDOs configured for SM2, but slave 0 does not provide the
sync manager information!
EtherCAT ERROR: Failed to determine PDO sync manager for FMMU on slave 0!
ec_omnidrive: Motor0 velocity: 11.
ec_omnidrive: Motor0 velocity: 12.
ec_omnidrive: Motor0 velocity: 13.
ec_omnidrive: Motor0 velocity: 14.
ec_omnidrive: Motor0 velocity: 15.
ec_omnidrive: Motor0 velocity: 10.
ec_omnidrive: Motor0 velocity: 11.
ec_omnidrive: Motor0 velocity: 12.
ec_omnidrive: Motor0 velocity: 13.
ec_omnidrive: Motor0 velocity: 14.
ec_omnidrive: Motor0 velocity: 15.
ec_omnidrive: Motor0 velocity: 10.
[...]
$ /opt/etherlab/bin/ethercat master -v
Master0
Phase: Operation
Slaves: 4
Device0: 00:11:22:33:44:55 (attached)
Tx count: 1289586
Rx count: 1289585
Device1: None.
$ /opt/etherlab/bin/ethercat slaves -v
=== Slave 0 ===
Alias: 1001
State: PREOP
Flag: E
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
=== Slave 1 ===
Alias: 1002
State: PREOP
Flag: E
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
=== Slave 2 ===
Alias: 1003
State: PREOP
Flag: E
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
=== Slave 3 ===
Alias: 1004
State: PREOP
Flag: E
Identity:
Vendor Id: 0x0000015a
Product code: 0x03010001
Revision number: 0x00010000
Serial number: 0x00000000
Mailboxes:
RX: 0x1800/558, TX: 0x1a2e/558
Supported protocols: EoE, CoE
$ /opt/etherlab/bin/ethercat config -v
Alias: 0
Position: 0
Vendor Id: 0x0000015a
Product code: 0x03010001
Attached slave: 0 (PREOP)
SM2 (Output)
PDO 0x1600
PDO entry 0x6042:00, 16 bit
SDO configuration:
None.
Alias: 0
Position: 1
Vendor Id: 0x0000015a
Product code: 0x03010001
Attached slave: 1 (PREOP)
SM2 (Output)
PDO 0x1600
PDO entry 0x6042:00, 16 bit
SDO configuration:
None.
Alias: 0
Position: 2
Vendor Id: 0x0000015a
Product code: 0x03010001
Attached slave: 2 (PREOP)
SM2 (Output)
PDO 0x1600
PDO entry 0x6042:00, 16 bit
SDO configuration:
None.
Alias: 0
Position: 3
Vendor Id: 0x0000015a
Product code: 0x03010001
Attached slave: 3 (PREOP)
SM2 (Output)
PDO 0x1600
PDO entry 0x6042:00, 16 bit
SDO configuration:
None.
$ /opt/etherlab/bin/ethercat domains -v
Domain0: LogBaseAddr 0x00000000, Size 8, WorkingCounter 0/4
SlaveConfig 0:0, SM2 (Output), LogAddr 0x00000000, Size 2
0x0e 0x00
SlaveConfig 0:1, SM2 (Output), LogAddr 0x00000002, Size 2
0x00 0x00
SlaveConfig 0:2, SM2 (Output), LogAddr 0x00000004, Size 2
0x00 0x00
SlaveConfig 0:3, SM2 (Output), LogAddr 0x00000006, Size 2
0x00 0x00
$ /opt/etherlab/bin/ethercat pdos -v
=== Slave 0 ===
=== Slave 1 ===
=== Slave 2 ===
=== Slave 3 ===
$ /opt/etherlab/bin/ethercat sdos -v
=== Slave 0 ===
=== Slave 1 ===
=== Slave 2 ===
=== Slave 3 ===
$ /opt/etherlab/bin/ethercat xml
<?xml version="1.0" ?>
<EtherCATInfo>
<!-- Slave 0 -->
<Vendor>
<Id>346</Id>
</Vendor>
<Descriptions>
<Devices>
<Device>
<Type ProductCode="#x03010001" RevisionNo="#x00010000"></Type>
</Device>
</Devices>
</Descriptions>
</EtherCATInfo>
<?xml version="1.0" ?>
<EtherCATInfo>
<!-- Slave 1 -->
<Vendor>
<Id>15a</Id>
</Vendor>
<Descriptions>
<Devices>
<Device>
<Type ProductCode="#x03010001" RevisionNo="#x00010000"></Type>
</Device>
</Devices>
</Descriptions>
</EtherCATInfo>
<?xml version="1.0" ?>
<EtherCATInfo>
<!-- Slave 2 -->
<Vendor>
<Id>15a</Id>
</Vendor>
<Descriptions>
<Devices>
<Device>
<Type ProductCode="#x03010001" RevisionNo="#x00010000"></Type>
</Device>
</Devices>
</Descriptions>
</EtherCATInfo>
<?xml version="1.0" ?>
<EtherCATInfo>
<!-- Slave 3 -->
<Vendor>
<Id>15a</Id>
</Vendor>
<Descriptions>
<Devices>
<Device>
<Type ProductCode="#x03010001" RevisionNo="#x00010000"></Type>
</Device>
</Devices>
</Descriptions>
</EtherCATInfo>
$ /opt/etherlab/bin/ethercat sii_read -v -p 0
SII Area:
08 00 02 04 f0 00 00 00 e9 03 00 00 00 00 d1 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
$ /opt/etherlab/bin/ethercat sii_read -v -p 1
SII Area:
08 00 02 04 f0 00 00 00 ea 03 00 00 00 00 aa 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
$ /opt/etherlab/bin/ethercat sii_read -v -p 2
SII Area:
08 00 02 04 f0 00 00 00 eb 03 00 00 00 00 83 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
$ /opt/etherlab/bin/ethercat sii_read -v -p 3
SII Area:
08 00 02 04 f0 00 00 00 ec 03 00 00 00 00 5c 00
5a 01 00 00 01 00 01 03 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 18 2e 02 2e 1a 2e 02 06 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00
The EEPROM dumps from TwinCAT are as follows:
$ hexdump Drive\ 1*\).bin
0000000 0008 0402 00f0 0000 0000 0000 0000 00b0
0000010 015a 0000 0001 0301 0000 0001 0000 0000
0000020 0000 0000 0000 0000 0000 0000 0000 0000
0000030 1800 022e 1a2e 022e 0006 0000 0000 0000
0000040 0000 0000 0000 0000 0000 0000 0000 0000
*
0000070 0000 0000 0000 0000 0000 0000 0000 0001
0000080
$ hexdump Drive\ 2*\).bin
0000000 0008 0402 00f0 0000 03ea 0000 0000 00aa
0000010 015a 0000 0001 0301 0000 0001 0000 0000
0000020 0000 0000 0000 0000 0000 0000 0000 0000
0000030 1800 022e 1a2e 022e 0006 0000 0000 0000
0000040 0000 0000 0000 0000 0000 0000 0000 0000
*
0000070 0000 0000 0000 0000 0000 0000 0000 0001
0000080
$ hexdump Drive\ 3*\).bin
0000000 0008 0402 00f0 0000 03eb 0000 0000 0083
0000010 015a 0000 0001 0301 0000 0001 0000 0000
0000020 0000 0000 0000 0000 0000 0000 0000 0000
0000030 1800 022e 1a2e 022e 0006 0000 0000 0000
0000040 0000 0000 0000 0000 0000 0000 0000 0000
*
0000070 0000 0000 0000 0000 0000 0000 0000 0001
0000080
$ hexdump Drive\ 4*\).bin
0000000 0008 0402 00f0 0000 03ec 0000 0000 005c
0000010 015a 0000 0001 0301 0000 0001 0000 0000
0000020 0000 0000 0000 0000 0000 0000 0000 0000
0000030 1800 022e 1a2e 022e 0006 0000 0000 0000
0000040 0000 0000 0000 0000 0000 0000 0000 0000
*
0000070 0000 0000 0000 0000 0000 0000 0000 0001
0000080
I'm thankful for any suggestions or comments.
Uwe.
--
http://www.hermann-uwe.de | http://www.holsham-traders.de
http://www.crazy-hacks.org | http://www.unmaintained-free-software.org
/******************************************************************************
*
* Copyright (C) 2009 Uwe Hermann <u...@hermann-uwe.de>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*****************************************************************************/
#include <linux/module.h>
#include <linux/timer.h>
#include <linux/spinlock.h>
#include <linux/interrupt.h>
#include "../../include/ecrt.h" // EtherCAT realtime interface
/*****************************************************************************/
// Module parameters
#define FREQUENCY 100
// Optional features
#define CONFIGURE_PDOS 1
#define EXTERNAL_MEMORY 1
#define SDO_ACCESS 0
#define PFX "ec_omnidrive: "
/*****************************************************************************/
// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};
spinlock_t master_lock = SPIN_LOCK_UNLOCKED;
static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};
static ec_slave_config_t *sc_motor0 = NULL;
static ec_slave_config_t *sc_motor1 = NULL;
static ec_slave_config_t *sc_motor2 = NULL;
static ec_slave_config_t *sc_motor3 = NULL;
static ec_slave_config_state_t sc_motor0_state = {};
static ec_slave_config_state_t sc_motor1_state = {};
static ec_slave_config_state_t sc_motor2_state = {};
static ec_slave_config_state_t sc_motor3_state = {};
// Timer
static struct timer_list timer;
/*****************************************************************************/
// process data
static uint8_t *domain1_pd; // process data memory
#define Motor0SlavePos 0, 0
#define Motor1SlavePos 0, 1
#define Motor2SlavePos 0, 2
#define Motor3SlavePos 0, 3
#define Baumueller_BMxxxx 0x0000015a, 0x03010001
// offsets for PDO entries
static unsigned int off_motor0_velocity;
static unsigned int off_motor1_velocity;
static unsigned int off_motor2_velocity;
static unsigned int off_motor3_velocity;
static unsigned int off_motor0_current_torque;
static unsigned int off_motor1_current_torque;
static unsigned int off_motor2_current_torque;
static unsigned int off_motor3_current_torque;
// (slave alias address, slave pos), (VID, PID), pdo entry index, pdo entry subindex, pointer, bit position
const static ec_pdo_entry_reg_t domain1_regs[] = {
{Motor0SlavePos, Baumueller_BMxxxx, 0x6042, 0, &off_motor0_velocity},
{Motor1SlavePos, Baumueller_BMxxxx, 0x6042, 0, &off_motor1_velocity},
{Motor2SlavePos, Baumueller_BMxxxx, 0x6042, 0, &off_motor2_velocity},
{Motor3SlavePos, Baumueller_BMxxxx, 0x6042, 0, &off_motor3_velocity},
{}
};
static unsigned int counter = 0;
static unsigned int speed = 10;
/*****************************************************************************/
#if CONFIGURE_PDOS
// index. subindex, size in bits
static ec_pdo_entry_info_t foo_pdo_entries[] = {
{0x6042, 0, 16}, // vl_target_velocity
};
// PDO index, #entries, array of entries to map
static ec_pdo_info_t foo_pdos[] = {
{0x1600, 1, foo_pdo_entries},
};
// sync manager index, s.m. direction, #pdos, arrays with PDOs to assign
static ec_sync_info_t foo_syncs[] = {
{2 /* SM2 */, EC_DIR_OUTPUT, 1, foo_pdos},
{3 /* SM3 */, EC_DIR_INPUT},
{0xff}
};
#endif
/*****************************************************************************/
#if SDO_ACCESS
static ec_sdo_request_t *sdo;
#endif
/*****************************************************************************/
void check_domain1_state(void)
{
ec_domain_state_t ds;
spin_lock(&master_lock);
ecrt_domain_state(domain1, &ds);
spin_unlock(&master_lock);
if (ds.working_counter != domain1_state.working_counter)
printk(KERN_INFO PFX "Domain1: WC %u.\n", ds.working_counter);
if (ds.wc_state != domain1_state.wc_state)
printk(KERN_INFO PFX "Domain1: State %u.\n", ds.wc_state);
domain1_state = ds;
}
/*****************************************************************************/
void check_master_state(void)
{
ec_master_state_t ms;
spin_lock(&master_lock);
ecrt_master_state(master, &ms);
spin_unlock(&master_lock);
if (ms.slaves_responding != master_state.slaves_responding)
printk(KERN_INFO PFX "%u slave(s).\n", ms.slaves_responding);
if (ms.al_states != master_state.al_states)
printk(KERN_INFO PFX "AL states: 0x%02X.\n", ms.al_states);
if (ms.link_up != master_state.link_up)
printk(KERN_INFO PFX "Link is %s.\n", ms.link_up ? "up" : "down");
master_state = ms;
}
/*****************************************************************************/
// FIXME: de-uglify code
void check_slave_config_states(void)
{
ec_slave_config_state_t s;
spin_lock(&master_lock);
ecrt_slave_config_state(sc_motor0, &s);
spin_unlock(&master_lock);
if (s.al_state != sc_motor0_state.al_state)
printk(KERN_INFO PFX "motor0: State 0x%02X.\n", s.al_state);
if (s.online != sc_motor0_state.online)
printk(KERN_INFO PFX "motor0: %s.\n", s.online ? "online" : "offline");
if (s.operational != sc_motor0_state.operational)
printk(KERN_INFO PFX "motor0: %soperational.\n",
s.operational ? "" : "Not ");
sc_motor0_state = s;
spin_lock(&master_lock);
ecrt_slave_config_state(sc_motor1, &s);
spin_unlock(&master_lock);
if (s.al_state != sc_motor1_state.al_state)
printk(KERN_INFO PFX "motor1: State 0x%02X.\n", s.al_state);
if (s.online != sc_motor1_state.online)
printk(KERN_INFO PFX "motor1: %s.\n", s.online ? "online" : "offline");
if (s.operational != sc_motor1_state.operational)
printk(KERN_INFO PFX "motor1: %soperational.\n",
s.operational ? "" : "Not ");
sc_motor1_state = s;
spin_lock(&master_lock);
ecrt_slave_config_state(sc_motor2, &s);
spin_unlock(&master_lock);
if (s.al_state != sc_motor2_state.al_state)
printk(KERN_INFO PFX "motor2: State 0x%02X.\n", s.al_state);
if (s.online != sc_motor2_state.online)
printk(KERN_INFO PFX "motor2: %s.\n", s.online ? "online" : "offline");
if (s.operational != sc_motor2_state.operational)
printk(KERN_INFO PFX "motor2: %soperational.\n",
s.operational ? "" : "Not ");
sc_motor2_state = s;
spin_lock(&master_lock);
ecrt_slave_config_state(sc_motor3, &s);
spin_unlock(&master_lock);
if (s.al_state != sc_motor3_state.al_state)
printk(KERN_INFO PFX "motor3: State 0x%02X.\n", s.al_state);
if (s.online != sc_motor3_state.online)
printk(KERN_INFO PFX "motor3: %s.\n", s.online ? "online" : "offline");
if (s.operational != sc_motor3_state.operational)
printk(KERN_INFO PFX "motor3: %soperational.\n",
s.operational ? "" : "Not ");
sc_motor3_state = s;
}
/*****************************************************************************/
#if SDO_ACCESS
void read_sdo(void)
{
switch (ecrt_sdo_request_state(sdo)) {
case EC_SDO_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo); // trigger first read
break;
case EC_SDO_REQUEST_BUSY:
printk(KERN_INFO PFX "Still busy...\n");
break;
case EC_SDO_REQUEST_SUCCESS:
printk(KERN_INFO PFX "SDO value: 0x%04X\n",
EC_READ_U16(ecrt_sdo_request_data(sdo)));
ecrt_sdo_request_read(sdo); // trigger next read
break;
case EC_SDO_REQUEST_ERROR:
printk(KERN_INFO PFX "Failed to read SDO!\n");
ecrt_sdo_request_read(sdo); // retry reading
break;
}
}
#endif
/*****************************************************************************/
void cyclic_task(unsigned long data)
{
int16_t current_velocity;
// receive process data
spin_lock(&master_lock);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
spin_unlock(&master_lock);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// calculate new process data
if (speed < 15)
speed++;
else
speed = 10;
current_velocity = EC_READ_S16(domain1_pd + off_motor0_velocity);
printk(KERN_INFO PFX "Motor0 velocity: %d.\n", current_velocity);
// check for master state (optional)
check_master_state();
// check for slave configuration state(s) (optional)
check_slave_config_states();
#if SDO_ACCESS
// read process data SDO
read_sdo();
#endif
}
// write process data
EC_WRITE_U16(domain1_pd + off_motor0_velocity, speed);
// send process data
spin_lock(&master_lock);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
spin_unlock(&master_lock);
// restart timer
timer.expires += HZ / FREQUENCY;
add_timer(&timer);
}
/*****************************************************************************/
int request_lock(void *data)
{
spin_lock(&master_lock);
return 0; // access allowed
}
/*****************************************************************************/
void release_lock(void *data)
{
spin_unlock(&master_lock);
}
/*****************************************************************************/
int __init init_mini_module(void)
{
#if CONFIGURE_PDOS
ec_slave_config_t *sc;
#endif
#if EXTERNAL_MEMORY
unsigned int size;
#endif
printk(KERN_INFO PFX "Starting...\n");
if (!(master = ecrt_request_master(0))) {
printk(KERN_ERR PFX "Requesting master 0 failed!\n");
goto out_return;
}
ecrt_master_callbacks(master, request_lock, release_lock, NULL);
printk(KERN_INFO PFX "Registering domain...\n");
if (!(domain1 = ecrt_master_create_domain(master))) {
printk(KERN_ERR PFX "Domain creation failed!\n");
goto out_release_master;
}
// master, (slave alias, slave pos), (VID, PID)
if (!(sc_motor0 = ecrt_master_slave_config(
master, Motor0SlavePos, Baumueller_BMxxxx))) {
printk(KERN_ERR PFX "Failed to get slave configuration for motor0.\n");
goto out_release_master;
}
#if CONFIGURE_PDOS
printk(KERN_INFO PFX "Configuring PDOs for motor0...\n");
// slave config, sync manager index, index of the pdo to assign
if (ecrt_slave_config_pdos(sc_motor0, EC_END, foo_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs for motor0.\n");
goto out_release_master;
}
// master, (slave alias, slave pos), (VID, PID)
if (!(sc_motor1 = ecrt_master_slave_config(
master, Motor1SlavePos, Baumueller_BMxxxx))) {
printk(KERN_ERR PFX "Failed to get slave configuration motor1.\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Configuring PDOs for motor1...\n");
// slave config, sync manager index, index of the pdo to assign
if (ecrt_slave_config_pdos(sc_motor1, EC_END, foo_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs for motor1.\n");
goto out_release_master;
}
// master, (slave alias, slave pos), (VID, PID)
if (!(sc_motor2 = ecrt_master_slave_config(
master, Motor2SlavePos, Baumueller_BMxxxx))) {
printk(KERN_ERR PFX "Failed to get slave configuration motor2.\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Configuring PDOs for motor2...\n");
// slave config, sync manager index, index of the pdo to assign
if (ecrt_slave_config_pdos(sc_motor2, EC_END, foo_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs for motor2.\n");
goto out_release_master;
}
// master, (slave alias, slave pos), (VID, PID)
if (!(sc_motor3 = ecrt_master_slave_config(
master, Motor3SlavePos, Baumueller_BMxxxx))) {
printk(KERN_ERR PFX "Failed to get slave configuration motor3.\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Configuring PDOs for motor3...\n");
// slave config, sync manager index, index of the pdo to assign
if (ecrt_slave_config_pdos(sc_motor3, EC_END, foo_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs for motor3.\n");
goto out_release_master;
}
#endif
// Unused.
#if SDO_ACCESS
printk(KERN_INFO PFX "Creating SDO requests...\n");
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x3102, 2, 2))) {
printk(KERN_ERR PFX "Failed to create SDO request.\n");
goto out_release_master;
}
ecrt_sdo_request_timeout(sdo, 500); // ms
#endif
printk(KERN_INFO PFX "Registering PDO entries...\n");
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
printk(KERN_ERR PFX "PDO entry registration failed!\n");
goto out_release_master;
}
#if EXTERNAL_MEMORY
if ((size = ecrt_domain_size(domain1))) {
if (!(domain1_pd = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
printk(KERN_ERR PFX "Failed to allocate %u bytes of process data"
" memory!\n", size);
goto out_release_master;
}
ecrt_domain_external_memory(domain1, domain1_pd);
}
#endif
printk(KERN_INFO PFX "Activating master...\n");
if (ecrt_master_activate(master)) {
printk(KERN_ERR PFX "Failed to activate master!\n");
#if EXTERNAL_MEMORY
goto out_free_process_data;
#else
goto out_release_master;
#endif
}
#if !EXTERNAL_MEMORY
// Get internal process data for domain
domain1_pd = ecrt_domain_data(domain1);
#endif
printk(KERN_INFO PFX "Starting cyclic sample thread.\n");
init_timer(&timer);
timer.function = cyclic_task;
timer.expires = jiffies + 10;
add_timer(&timer);
printk(KERN_INFO PFX "Started.\n");
return 0;
#if EXTERNAL_MEMORY
out_free_process_data:
kfree(domain1_pd);
#endif
out_release_master:
printk(KERN_ERR PFX "Releasing master...\n");
ecrt_release_master(master);
out_return:
printk(KERN_ERR PFX "Failed to load. Aborting.\n");
return -1;
}
/*****************************************************************************/
void __exit cleanup_mini_module(void)
{
printk(KERN_INFO PFX "Stopping...\n");
del_timer_sync(&timer);
#if EXTERNAL_MEMORY
kfree(domain1_pd);
#endif
printk(KERN_INFO PFX "Releasing master...\n");
ecrt_release_master(master);
printk(KERN_INFO PFX "Unloading.\n");
}
/*****************************************************************************/
MODULE_LICENSE("GPL");
MODULE_AUTHOR("Uwe Hermann <u...@hermann-uwe.de>");
MODULE_DESCRIPTION("Omnidrive robot base control application");
module_init(init_mini_module);
module_exit(cleanup_mini_module);
/*****************************************************************************/
_______________________________________________
etherlab-users mailing list
etherlab-users@etherlab.org
http://lists.etherlab.org/mailman/listinfo/etherlab-users
