simple_test
Then let’s move to next step. The simple_test is the example program to control motors. ‘-h’ or ‘–help’ option will show the usages of this program.
$ rosrun minas_control simple_test -h
MINAS Simple Test using SOEM (Simple Open EtherCAT Master)
Usage: simple_test [options]
Available options
-i, --interface NIC interface name for EtherCAT network
-p, --position_mode Sample program using Position Profile (pp) mode (Default)
-c, --cycliec_mode Sample program using cyclic synchronous position(csp) mode
-h, --help Print this message and exit
On default settings, simple_test will servo on, rotate about 360 degree and servo off. The simple_test program basically follow the instruction described in the manual, i.e Start up guide in p.3 and Motion of pp control mode in p. 107. Basic flow of the cpp program as follows.
minas_control::MinasInput input = client->readInputs();
int32 current_position = input.position_actual_value;
// set target position
minas_control::MinasOutput output;
output.target_position = (current_position > 0)?
(current_position - 0x100000):(current_position + 0x100000);
output.max_motor_speed = 120; // rad/min
output.target_torque = 500; // 0% (unit 0.1%)
output.max_torque = 500; // 50% (unit 0.1%)
output.controlword = 0x001f; // move to operation enabled +
// new-set-point (bit4) +
// change set immediately (bit5)
output.operation_mode = 0x01; // (pp) position profile mode
// set profile velocity
client->setProfileVelocity(0x20000000);
// pp control model setup (see statusword(6041.h) 3) p.107)
client->writeOutputs(output);
while ( ! (input.statusword & 0x1000) ) {// bit12 (set-point-acknowledge)
input = client->readInputs();
}
output.controlword &= ~0x0010; // clear new-set-point (bit4)
client->writeOutputs(output);
To run simple_test with pp mode, use -p option.
$ rosrun minas_control simple_test -p -i eth1
MINAS Simple Test using SOEM (Simple Open EtherCAT Master)
Initializing etherCAT master
wkc = 2
SOEM found and configured 2 slaves
len = 9
len = 9
len = 9
len = 9
RxPDO mapping object index 1 = 1603 ret=3
TxPDO mapping object index 1 = 1a03 ret=6
RxPDO mapping object index 2 = 1603 ret=3
TxPDO mapping object index 2 = 1a03 ret=6
SOEM IOMap size: 100
Slave:1
Name:MADHT1105B01
Output size: 200bits
Input size: 200bits
State: 8
Delay: 0[ns]
Has DC: 1
DCParentport:0
Activeports:1.1.0.0
Configured address: 1001
Slave:2
Name:MADHT1107B21
Output size: 200bits
Input size: 200bits
State: 8
Delay: 680[ns]
Has DC: 1
DCParentport:1
Activeports:1.0.0.0
Configured address: 1002
PDO syncmode 00, cycle time 0 ns (min 17000), sync0 cycle time 0 ns,ret = 4
PDO syncmode 00, cycle time 0 ns (min 17000), sync0 cycle time 0 ns,ret = 4
overrun: 0.000596
overrun: 0.000572
overrun: 0.002370
Set interpolation time period 4000 us (4000000/4)
overrun: 0.005399
1c32h: cycle time 0
60c2h: interpolation time period value 25
Statusword(6041h): 0a70
Switch on disabled
Internal limit active
Following error
Drive follows command value
overrun: 0.007179
overrun: 0.006475
overrun: 0.000108
Statusword(6041h): 0e37
Operation enabled
Internal limit active
Following error
Set-point acknowledge
Target reached
overrun: 0.000403
target position = 000e912d
overrun: 0.000011
overrun: 0.000191
Set interpolation time period 4000 us (4000000/4)
overrun: 0.000659
1c32h: cycle time 0
60c2h: interpolation time period value 25
Statusword(6041h): 0a70
Switch on disabled
Internal limit active
Following error
Drive follows command value
Statusword(6041h): 0e31
Ready to switch on
Internal limit active
Following error
Set-point acknowledge
Target reached
overrun: 0.001740
overrun: 0.004097
target position = 000c2bba
overrun: 0.003520
err = 0000, ctrl 000f, status 0237, op_mode = 1, pos = fffe9196, vel = 00000cb2, tor = 00000017
Tick 1488782766.167119670
Input:
603Fh 00000000 :Error code
6041h 00000237 :Statusword
6061h 00000001 :Modes of operation display
6064h fffe9196 :Position actual value
606Ch 00000cb2 :Velocity actual value
6077h 00000017 :Torque actual value
60B9h 00000000 :Touch probe status
60BAh 00000000 :Touch probe pos1 pos value
60FDh c0000000 :Digital inputs
Output:
6040h 0000000f :Controlword
6060h 00000001 :Mode of operation
overrun: 0.002877
6071h 000001f4 :Target Torque
6072h 000001f4 :Max Torque
607Ah 000e912d :Target Position
6080h 00000078 :Max motor speed
60B8h 00000000 :Touch Probe function
60FFh 00000000 :Target Velocity
60B0h 00000000 :Position Offset
overrun: 0.002274
err = 0000, ctrl 000f, status 1237, op_mode = 1, pos = fffc2bb6, vel = fffffe0c, tor = 00000000
Tick 1488782766.167119670
Input:
603Fh 00000000 :Error code
6041h 00001237 :Statusword
6061h 00000001 :Modes of operation display
6064h fffc2bb6 :Position actual value
606Ch fffffe0c :Velocity actual value
6077h 00000000 :Torque actual value
60B9h 00000000 :Touch probe status
60BAh 00000000 :Touch probe pos1 pos value
60FDh c0000000 :Digital inputs
You can see some erros in the first a few seconds, until the motors servo on, but that’s expected behavior and you can ingreo for now.
If you run simple_test with -c option, it will servo on, rotate about 180 degree back and forth with sin curve and servo off. Basic flow of the cpp program as follows.
client->setInterpolationTimePeriod(4000); // 4 msec
minas_control::MinasInput input = client->readInputs();
int32 current_position = input.position_actual_value;
// set target position
minas_control::MinasOutput output;
output.target_position = current_position;
output.max_motor_speed = 120; // rad/min
output.target_torque = 500; // 0% (unit 0.1%)
output.max_torque = 500; // 50% (unit 0.1%)
output.controlword = 0x001f; // move to operation enabled + new-set-point (bit4) + change set immediately (bit5)
output.operation_mode = 0x08; // (csp) cyclic synchronous position mode
client->writeOutputs(output);
struct timespec tick;
clock_gettime(CLOCK_REALTIME, &tick);
while ( 1 ) {
output.position_offset = 0x80000*sin(i/200.0);
client->writeOutputs(output);
// sleep for next tick
timespecInc(tick, period);
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
}
main (ROS controlelr program)
The main executable is ROS based controller program. ‘-h’ or ‘–help’ option will show the usages of this program.
$ rosrun minas_control main -h
Usage: main [options]
Available options
-i, --interface NIC interface name for EtherCAT
-l, --loopback Use loopback interface for Controller (i.e. simulation mode)
-p, --period RT loop period in msec
-s, --stats Publish statistics on the RT loop jitter on
"node_name/realtime" in seconds
-h, --help Print this message and exit
If you do not have MINAS-A5B hardwre, you can run with simulation mode
$ rosrun minas_control main -l
[ INFO] [1488677269.130094946]: Minas Hardware Interface in simulation mode
and check the realtime capability of the ros control program by listening /diagnostics ROS topic.
To run controllers with physical MINAS A-5 Hardware connecting at eth1 EtherCAT network, you can main program as follows. Please change eth1 to your settings.
$ rosrun minas_control main -i eth1
Initializing etherCAT master
wkc = 2
SOEM found and configured 2 slaves
len = 9
len = 9
len = 9
len = 9
RxPDO mapping object index 1 = 1603 ret=3
TxPDO mapping object index 1 = 1a03 ret=6
RxPDO mapping object index 2 = 1603 ret=3
TxPDO mapping object index 2 = 1a03 ret=6
SOEM IOMap size: 100
Slave:1
Name:MADHT1105B01
Output size: 200bits
Input size: 200bits
State: 8
Delay: 0[ns]
Has DC: 1
DCParentport:0
Activeports:1.1.0.0
Configured address: 1001
Slave:2
Name:MADHT1107B21
Output size: 200bits
Input size: 200bits
State: 8
Delay: 680[ns]
Has DC: 1
DCParentport:1
Activeports:1.0.0.0
Configured address: 1002
PDO syncmode 00, cycle time 0 ns (min 17000), sync0 cycle time 0 ns, ret = 4
PDO syncmode 00, cycle time 0 ns (min 17000), sync0 cycle time 0 ns, ret = 4
Finished configuration successfully
[ERROR] [1488776588.629694406]: Minas Hardware Interface expecting 6 clients
overrun: 0.000117
overrun: 0.000442
overrun: 0.000259
Statusword(6041h): 0e33
Switched on
Internal limit active
Following error
Set-point acknowledge
Target reached
Statusword(6041h): 0a37
Operation enabled
Internal limit active
Following error
Set-point acknowledge
Target reached
[ WARN] [1488776588.870953939]: target position = 00000000
[ WARN] [1488776588.871001884]: position offset = fffc2bb3
[ERROR] [1488776588.871041451]: Could not find EtherCAT client
[ERROR] [1488776588.871057483]: Minas Hardware Interface uses Dummy joint 3
[ERROR] [1488776588.871073659]: Could not find EtherCAT client
[ERROR] [1488776588.871084746]: Minas Hardware Interface uses Dummy joint 4
[ERROR] [1488776588.871099793]: Could not find EtherCAT client
[ERROR] [1488776588.871110595]: Minas Hardware Interface uses Dummy joint 5
[ERROR] [1488776588.871122447]: Could not find EtherCAT client
[ERROR] [1488776588.871132278]: Minas Hardware Interface uses Dummy joint 6
You can see some erros, specially if you do not set connect 6 motors on your EtherCAT network, but still the controlle software is able to run as they use loopback driver for these joints.
To check current realtime capabiliy of ROS control, you can run rostopic echo /diagnostics.
$ rostopic echo /diagnostics
---
header:
seq: 200
stamp:
secs: 1488776789
nsecs: 50168139
frame_id: ''
status:
-
level: 0
name: Realtime Control Loop
message: Realtime loop used too much time in the last 30 seconds.
hardware_id: ''
values:
-
key: Max EtherCAT roundtrip (us)
value: 4030.91
-
key: Avg EtherCAT roundtrip (us)
value: 13.41
-
key: Max Controller Manager roundtrip (us)
value: 383.95
-
key: Avg Controller Manager roundtrip (us)
value: 5.41
-
key: Max Total Loop roundtrip (us)
value: 5127.10
-
key: Avg Total Loop roundtrip (us)
value: 1000.01
-
key: Max Loop Jitter (us)
value: 1136.49
-
key: Avg Loop Jitter (us)
value: 71.25
-
key: Control Loop Overruns
value: 11
-
key: Recent Control Loop Overruns
value: 0
-
key: Last Control Loop Overrun Cause
value: ec: 1221.71us, cm: 2.58us
-
key: Last Overrun Loop Time (us)
value: 281.10
-
key: Realtime Loop Frequency
value: 971.6667
