Newer
Older
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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
// setp lcec.0.0.drivecontrol-1 1
// setp lcec.0.0.drivecontrol-2 1
// net state-op <= lcec.state-op
// net X-amp => lcec.0.0.drivecontrol-3 => lcec.0.0.drivecontrol-0
// net X-poscmd => lcec.0.0.poscommand
// net X-posfb <= lcec.0.0.pos
// net X-fault <= lcec.0.0.drivestatus-3
// net X-stat-pos <= lcec.0.0.modestatus-3
// net X-stat-home <= lcec.0.0.modestatus-2
// net X-homingok <= lcec.0.0.drivestatus-12
// net X-athome <= lcec.0.0.drivestatus-10
// net X-ctrl-home => lcec.0.0.modecontrol-1 => lcec.0.0.modecontrol-2
// net X-homestart => lcec.0.0.drivecontrol-4
// net X-ctrl-pos => lcec.0.0.modecontrol-3
// net X-PoT <= lcec.0.0.inputs-1
// net X-NoT <= lcec.0.0.inputs-0
component lcec_simu "ESTUN drive simulator";
author "Electrolab";
pin out bit state_op;
pin in bit drivecontrol_0;
pin in bit drivecontrol_1;
pin in bit drivecontrol_2;
pin in bit drivecontrol_3;
pin in bit drivecontrol_4;
pin in float poscommand;
pin out float pos;
pin out bit drivestatus_3;
pin out bit drivestatus_10;
pin out bit drivestatus_12;
pin out bit modestatus_2;
pin out bit modestatus_3;
pin in bit modecontrol_0;
pin in bit modecontrol_1;
pin in bit modecontrol_2;
pin in bit modecontrol_3;
pin in bit modecontrol_4;
pin out bit inputs_0;
pin out bit inputs_1;
pin out s32 debug;
pin out s32 state;
pin out float motion_length;
function _;
license "GPL"; // indicates GPL v2 or later
;;
#define HOMING_LENGTH 100
#define LATCH_LENGTH 5
#define HOMING_SPEED_MM_S 10
#define SERVO_PERIOD_MILI 1
FUNCTION(_) {
state_op = 1;
drivestatus_3 = 0;
inputs_0 = 0;
inputs_1 = 0;
debug += 1;
modestatus_2 = modecontrol_2;
modestatus_3 = modecontrol_3;
if (!drivecontrol_4) {
motion_length = HOMING_LENGTH;
drivestatus_12 = 0;
state = 0;
}
if (drivecontrol_0 && drivecontrol_1 && drivecontrol_2 && drivecontrol_3) { // Drive enable
if (!modecontrol_2 && modecontrol_3) { // Position mode
pos = poscommand;
}
if (modecontrol_2 && !modecontrol_3 && drivecontrol_4) { // Homing mode
switch (state) {
case 0: // idle
state = 1;
break;
case 1: // home
if (motion_length > 0) {
float step = HOMING_SPEED_MM_S * SERVO_PERIOD_MILI / 1000.0;
motion_length -= step;
pos += step;
}
else {
motion_length = LATCH_LENGTH;
state = 2;
}
break;
case 2: // latch
if (motion_length > 0) {
float step = HOMING_SPEED_MM_S * SERVO_PERIOD_MILI / 1000.0;
motion_length -= step;
pos -= step;
}
else {
state = 3;
}
break;
case 3: // homed
drivestatus_12 = 1;
break;
}
}
}
}