268 lines
7.1 KiB
C
268 lines
7.1 KiB
C
#include "liftlibcontrol.h"
|
|
#include "liftlibio.h"
|
|
|
|
/* Global variables */
|
|
int lift_levelPos[16];
|
|
int lift_one_level;
|
|
|
|
/**
|
|
Is the counter valid for level positioning?
|
|
*/
|
|
|
|
// Wasm loop bounds
|
|
|
|
__attribute__((import_module("__pragma"), import_name("loopbound"))) extern void
|
|
__pragma_loopbound(unsigned int min_bound, unsigned int max_bound);
|
|
|
|
int lift_cntValid;
|
|
|
|
/**
|
|
Position absolute or relative<.
|
|
*/
|
|
int lift_cnt;
|
|
|
|
/**
|
|
Last stoped level (1..13) if position is absolute else 0.
|
|
*/
|
|
int lift_level;
|
|
|
|
/**
|
|
load position in level, 0 means we don't know
|
|
*/
|
|
int lift_loadLevel;
|
|
|
|
/**
|
|
we're going TOP or BOTTOM, but stop at load position.
|
|
*/
|
|
int lift_loadPending;
|
|
|
|
/**
|
|
we're waiting for the load sensor to go.
|
|
*/
|
|
int lift_loadSensor;
|
|
|
|
/**
|
|
cmd keeps the value of the command until the command is finished.
|
|
It is only updated by the switches if it's current value is CMD_NONE.
|
|
*/
|
|
int lift_cmd;
|
|
|
|
int lift_timMotor;
|
|
|
|
int lift_timImp;
|
|
|
|
/**
|
|
Remember last direction for impuls count after motor off;
|
|
*/
|
|
int lift_directionUp;
|
|
|
|
/**
|
|
Last value of impuls sensor.
|
|
*/
|
|
int lift_lastImp;
|
|
|
|
int lift_dbgCnt;
|
|
|
|
/**
|
|
stop value for the counter.
|
|
*/
|
|
int lift_endCnt;
|
|
|
|
__attribute__((always_inline)) static inline void
|
|
lift_ctrl_init() {
|
|
int i;
|
|
lift_checksum = 0;
|
|
|
|
lift_io_init();
|
|
lift_cntValid = 0;
|
|
lift_cnt = 0;
|
|
lift_cmd = lift_CMD_NONE;
|
|
lift_timMotor = 0;
|
|
lift_timImp = 0;
|
|
lift_directionUp = 1;
|
|
lift_lastImp = 0;
|
|
lift_loadLevel = 0;
|
|
lift_loadPending = 0;
|
|
lift_loadSensor = 0;
|
|
i = 0;
|
|
lift_levelPos[i++] = 0;
|
|
lift_levelPos[i++] = 58;
|
|
lift_levelPos[i++] = 115;
|
|
lift_levelPos[i++] = 173;
|
|
lift_levelPos[i++] = 230;
|
|
lift_levelPos[i++] = 288;
|
|
lift_levelPos[i++] = 346;
|
|
lift_levelPos[i++] = 403;
|
|
lift_levelPos[i++] = 461;
|
|
lift_levelPos[i++] = 518;
|
|
lift_levelPos[i++] = 576;
|
|
lift_levelPos[i++] = 634;
|
|
lift_levelPos[i++] = 691;
|
|
lift_levelPos[i++] = 749;
|
|
lift_levelPos[i++] = 806;
|
|
lift_levelPos[i++] = 864;
|
|
lift_one_level = lift_levelPos[1];
|
|
}
|
|
|
|
__attribute__((always_inline)) static inline void
|
|
lift_ctrl_loop() {
|
|
if (lift_cmd == lift_CMD_NONE)
|
|
lift_check_cmd();
|
|
else {
|
|
lift_do_impulse(lift_ctrl_io_in[lift_SENS_IMPULS],
|
|
lift_ctrl_io_out[lift_MOTOR_ON],
|
|
lift_ctrl_io_in[lift_SENS_BOTTOM]);
|
|
lift_do_cmd();
|
|
}
|
|
lift_check_level();
|
|
lift_ctrl_io_led[13] = (lift_dbgCnt & 0x80) != 0;
|
|
++lift_dbgCnt;
|
|
}
|
|
|
|
__attribute__((always_inline)) static inline void
|
|
lift_check_level() {
|
|
int i;
|
|
int middle = lift_one_level >> 2;
|
|
if (lift_cntValid) {
|
|
__pragma_loopbound(14, 14);
|
|
for (lift_level = 1; lift_level < 14; ++lift_level) {
|
|
if (lift_cnt < lift_levelPos[lift_level] - middle)
|
|
break;
|
|
}
|
|
} else
|
|
lift_level = 0;
|
|
__pragma_loopbound(14, 14);
|
|
for (i = 0; i < 14; ++i)
|
|
lift_ctrl_io_led[i] = (i == lift_level - 1);
|
|
}
|
|
|
|
__attribute__((always_inline)) static inline void
|
|
lift_check_cmd() {
|
|
if (lift_loadPending) {
|
|
if (lift_ctrl_io_in[lift_SENS_BOTTOM])
|
|
lift_cmd = lift_CMD_TOP;
|
|
} else if (lift_ctrl_io_in[lift_GO_UP]) {
|
|
if (!lift_ctrl_io_in[lift_SENS_TOP] && lift_level != 14)
|
|
lift_cmd = lift_CMD_UP;
|
|
} else if (lift_ctrl_io_in[lift_GO_DOWN]) {
|
|
if (!lift_ctrl_io_in[lift_SENS_BOTTOM] && lift_level != 1)
|
|
lift_cmd = lift_CMD_DOWN;
|
|
} else if (lift_ctrl_io_in[lift_GO_LOAD]) {
|
|
if (lift_loadLevel != 0 && lift_level < lift_loadLevel)
|
|
lift_cmd = lift_CMD_TOP;
|
|
else
|
|
lift_cmd = lift_CMD_BOTTOM;
|
|
lift_loadPending = 1;
|
|
lift_loadSensor = 0;
|
|
} else if (lift_ctrl_io_in[lift_GO_TOP]) {
|
|
if (!lift_ctrl_io_in[lift_SENS_TOP])
|
|
lift_cmd = lift_CMD_TOP;
|
|
} else if (lift_ctrl_io_in[lift_GO_BOTTOM]) {
|
|
if (!lift_ctrl_io_in[lift_SENS_BOTTOM])
|
|
lift_cmd = lift_CMD_BOTTOM;
|
|
}
|
|
if (lift_cmd != lift_CMD_NONE)
|
|
lift_timMotor = 50;
|
|
}
|
|
|
|
__attribute__((always_inline)) static inline void
|
|
lift_do_impulse(int val, int motor, int reset) {
|
|
if (val && !lift_lastImp) {
|
|
if (motor || lift_timImp > 0) {
|
|
if (lift_directionUp)
|
|
++lift_cnt;
|
|
else
|
|
--lift_cnt;
|
|
}
|
|
}
|
|
if (reset) {
|
|
lift_cnt = 0;
|
|
lift_cntValid = 1;
|
|
}
|
|
lift_lastImp = val;
|
|
if (lift_timImp > 0) {
|
|
--lift_timImp;
|
|
if (lift_timImp == 0 && lift_cmd != lift_CMD_NONE)
|
|
lift_cmd = lift_CMD_NONE;
|
|
}
|
|
}
|
|
|
|
__attribute__((always_inline)) static inline void
|
|
lift_do_cmd() {
|
|
int run = 0;
|
|
if (lift_timMotor > 0)
|
|
lift_wait_for_motor_start();
|
|
else {
|
|
run = lift_check_run();
|
|
if (lift_ctrl_io_out[lift_MOTOR_ON] && !run) {
|
|
/* motor stopped: */
|
|
lift_cmd = 99;
|
|
lift_timImp = 50;
|
|
}
|
|
lift_ctrl_io_out[lift_MOTOR_ON] = run;
|
|
}
|
|
}
|
|
|
|
__attribute__((always_inline)) static inline void
|
|
lift_wait_for_motor_start() {
|
|
int newLevel = 0;
|
|
--lift_timMotor;
|
|
lift_directionUp = (lift_cmd == lift_CMD_UP || lift_cmd == lift_CMD_TOP);
|
|
lift_ctrl_io_out[lift_MOTOR_UP] = lift_directionUp;
|
|
if (!lift_cntValid) {
|
|
lift_cnt = 0; /* use relative counter */
|
|
if (lift_cmd == lift_CMD_UP)
|
|
lift_endCnt = lift_one_level;
|
|
else
|
|
lift_endCnt = -lift_one_level;
|
|
} else {
|
|
lift_endCnt = lift_cnt;
|
|
newLevel = -99;
|
|
if (lift_cmd == lift_CMD_UP)
|
|
newLevel = lift_level + 1;
|
|
else if (lift_cmd == lift_CMD_DOWN)
|
|
newLevel = lift_level - 1;
|
|
--newLevel; /* lift_level is one based */
|
|
if (newLevel >= 0 && newLevel < 14)
|
|
lift_endCnt = lift_levelPos[newLevel];
|
|
}
|
|
}
|
|
|
|
__attribute__((always_inline)) static inline int
|
|
lift_check_run() {
|
|
if (lift_cmd == lift_CMD_UP) {
|
|
if (lift_cnt < lift_endCnt - 1 && !lift_ctrl_io_in[lift_SENS_TOP])
|
|
return 1;
|
|
} else if (lift_cmd == lift_CMD_DOWN) {
|
|
if (lift_cnt > lift_endCnt + 1 && !lift_ctrl_io_in[lift_SENS_BOTTOM])
|
|
return 1;
|
|
} else if (lift_cmd == lift_CMD_TOP) {
|
|
if (lift_loadPending && lift_ctrl_io_in[lift_SENS_LOAD]) {
|
|
/* we are at lift_load position */
|
|
lift_loadLevel = lift_level;
|
|
lift_loadPending = 0;
|
|
return 0;
|
|
}
|
|
if (!lift_ctrl_io_in[lift_SENS_TOP])
|
|
return 1;
|
|
/* safe fallback if lift_load sensor does not work */
|
|
lift_loadPending = 0;
|
|
} else if (lift_cmd == lift_CMD_BOTTOM) {
|
|
if (lift_loadPending) {
|
|
if (lift_loadSensor) {
|
|
if (!lift_ctrl_io_in[lift_SENS_LOAD]) {
|
|
lift_loadSensor = 0;
|
|
/* we are at lift_load position */
|
|
lift_loadPending = 0;
|
|
lift_loadLevel = lift_level;
|
|
return 0;
|
|
}
|
|
}
|
|
lift_loadSensor = lift_ctrl_io_in[lift_SENS_LOAD];
|
|
}
|
|
if (!lift_ctrl_io_in[lift_SENS_BOTTOM])
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|