278 lines
6.5 KiB
C
Executable File
278 lines
6.5 KiB
C
Executable File
#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;
|
|
}
|