Add wasm tacle-bench targets
This commit is contained in:
277
targets/wasm-tacle/app/lift/liftlibcontrol.c
Executable file
277
targets/wasm-tacle/app/lift/liftlibcontrol.c
Executable file
@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user