blob: da3fe22697bd40046331721fd0bbf67efc438a3d (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
|
# core HAL config file for simulation - 6 axis
# load RT modules
loadrt genserkins
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD num_joints=[TRAJ]AXES
# add motion controller functions to servo thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
# create HAL signals for position commands from motion module
net J0pos <= axis.0.motor-pos-cmd
net J1pos <= axis.1.motor-pos-cmd
net J2pos <= axis.2.motor-pos-cmd
net J3pos <= axis.3.motor-pos-cmd
net J4pos <= axis.4.motor-pos-cmd
net J5pos <= axis.5.motor-pos-cmd
# loop position commands back to motion module feedback
net J0pos => axis.0.motor-pos-fb
net J1pos => axis.1.motor-pos-fb
net J2pos => axis.2.motor-pos-fb
net J3pos => axis.3.motor-pos-fb
net J4pos => axis.4.motor-pos-fb
net J5pos => axis.5.motor-pos-fb
# estop loopback
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
# create signals for tool loading loopback
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
loadusr -W puma560gui
setp genserkins.A-0 0
setp genserkins.A-1 0
setp genserkins.A-2 17
setp genserkins.A-3 0
setp genserkins.A-4 0
setp genserkins.A-5 0
setp genserkins.ALPHA-0 0
setp genserkins.ALPHA-1 1.570796326
setp genserkins.ALPHA-2 0
setp genserkins.ALPHA-3 1.570796326
setp genserkins.ALPHA-4 -1.570796326
setp genserkins.ALPHA-5 1.570796326
setp genserkins.D-0 26.45
setp genserkins.D-1 -5.5
setp genserkins.D-2 0
setp genserkins.D-3 17.05
setp genserkins.D-4 0
setp genserkins.D-5 2.2
loadrt scale count=6
addf scale.0 servo-thread
addf scale.1 servo-thread
addf scale.2 servo-thread
addf scale.3 servo-thread
addf scale.4 servo-thread
addf scale.5 servo-thread
net J0pos scale.0.in
net J1pos scale.1.in
net J2pos scale.2.in
net J3pos scale.3.in
net J4pos scale.4.in
net J5pos scale.5.in
setp scale.0.gain 1
setp scale.1.gain 1
setp scale.2.gain 1
setp scale.3.gain 1
setp scale.4.gain 1
setp scale.5.gain 1
net J0scaled scale.0.out puma560gui.joint1
net J1scaled scale.1.out puma560gui.joint2
net J2scaled scale.2.out puma560gui.joint3
net J3scaled scale.3.out puma560gui.joint4
net J4scaled scale.4.out puma560gui.joint5
net J5scaled scale.5.out puma560gui.joint6
|
