diff options
Diffstat (limited to 'test/monniaux/tacle-bench-lift/liftlibcontrol.c')
| -rw-r--r-- | test/monniaux/tacle-bench-lift/liftlibcontrol.c | 277 |
1 files changed, 277 insertions, 0 deletions
diff --git a/test/monniaux/tacle-bench-lift/liftlibcontrol.c b/test/monniaux/tacle-bench-lift/liftlibcontrol.c new file mode 100644 index 00000000..5e491276 --- /dev/null +++ b/test/monniaux/tacle-bench-lift/liftlibcontrol.c @@ -0,0 +1,277 @@ +#include "liftlibio.h" +#include "liftlibcontrol.h" + +/* Global variables */ +int lift_levelPos[16]; +int lift_one_level; + +/** + Is the counter valid for level positioning? +*/ +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; + + +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]; +} + +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; +} + + +void lift_check_level() +{ + int i; + int middle = lift_one_level >> 2; + if ( lift_cntValid ) { + _Pragma( "loopbound min 14 max 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 min 14 max 14" ) + for ( i = 0; i < 14; ++i ) + lift_ctrl_io_led[i] = ( i == lift_level - 1 ); +} + + +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; +} + + +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; + } +} + + +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; + } +} + + +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]; + } +} + + +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; +} |