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
// 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 float physical_pos = 50;
pin out float home_to;
pin out bit home_dir;
function _;
license "GPL"; // indicates GPL v2 or later
;;
#include <math.h>
#define HOMING_POS 100
#define LATCH_LENGTH 5
#define HOMING_SPEED_MM_S 10
#define SERVO_PERIOD_MILI 1
int process_homing(hal_float_t *_home_to, hal_float_t *_physical_pos, hal_float_t *_pos)
{
float delta = *_home_to - *_physical_pos;
if (fabs(delta) < 1e-7) return 1; // Arrive at destination
float step = HOMING_SPEED_MM_S * SERVO_PERIOD_MILI / 1000.0;
if (step > fabs(delta)) step = fabs(delta);
if (delta < 0) step = -step;
*_physical_pos += step;
*_pos += step;
return 0;
}
FUNCTION(_) {
state_op = 1;
drivestatus_3 = 0;
inputs_0 = 0;
inputs_1 = 0;
modestatus_2 = modecontrol_2;
modestatus_3 = modecontrol_3;
if (!drivecontrol_4) {
home_to = HOMING_POS;
if (physical_pos < home_to) {
home_to += LATCH_LENGTH;
home_dir = 1;
}
else {
home_to -= LATCH_LENGTH;
home_dir = 0;
}
state = 0;
}
if (drivecontrol_0 && drivecontrol_1 && drivecontrol_2 && drivecontrol_3) { // Drive enable
if (!modecontrol_2 && modecontrol_3) { // Position mode
physical_pos += poscommand - pos;
pos = poscommand;
}
if (modecontrol_2 && !modecontrol_3 && drivecontrol_4) { // Homing mode
switch (state) {
case 0: // idle
state = 1;
break;
case 1: // home
if (process_homing(&home_to, &physical_pos, &pos)) {
if (home_dir) home_to -= LATCH_LENGTH;
else home_to += LATCH_LENGTH;
state = 2;
}
break;
case 2: // latch
if (process_homing(&home_to, &physical_pos, &pos)) {
pos = 0;