ErrorID_t wait_for_wiimotedata(volatile WiimoteStatusDataType *wiimote_status,
int32_t timeout)
{
ErrorID_t ret_val = ERR_NONE;
#if _MUTEX_ENABLE
static struct timespec abstime;
if (0 < timeout)
{
update_timeout_value(&abstime, timeout);
ret_val = pthread_cond_timedwait(&cond, &mutex, &abstime);
if (ETIMEDOUT == ret_val)
{
fprintf(stderr, "@%u: Wiimote data timeout.\n", get_tick_count());
}
}
else
{
ret_val = pthread_cond_wait(&cond, &mutex);
}
if (0 <= ret_val)
{
#if (_DEBUG >= 2)
fprintf(stderr, "@%u: Wiimote data received.\n", get_tick_count());
#endif
}
#else
uint32_t start_time = get_tick_count();
wiimote_data_ready = false;
while (!wiimote_data_ready)
{
if (check_for_timeout(get_tick_count(), start_time, timeout))
{
debug_print("%u: wiimote data timeout, timeout = %d\n",
get_tick_count(), timeout);
return ERR_WII_DATA_TIMEOUT;
}
usleep(25 * 1000);
}
#endif
return ret_val;
}
void motor_run_handler(uint32_t current_time)
{
if (get_nvm_error_flag() != ERR_NONE)
return;
switch (motor_state)
{
case MOTOR_STATE_IDLE:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
set_sensor_waiting_for_measurement(true);
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
break;
case MOTOR_STATE_BRAKING:
if (check_for_timeout(current_time, motor_brake_timestamp,
MOTOR_BRAKE_TIME))
{
start_brake_coasting();
}
break;
case MOTOR_STATE_BRAKE_COAST:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
debug_P(PSTR("Stop brake coasting @ %lu\n"), current_time);
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_BRAKE_READ_BACK_EMF;
motor_read_back_emf_count = 0;
}
break;
case MOTOR_STATE_BRAKE_READ_BACK_EMF:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
// if we have receive a brake command we just keep braking
if (motor_levels.speed_channel_level == MOTOR_LEVEL_BRAKE)
{
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
else if (motor_levels.speed_channel_level > 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_FORWARD))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_FORWARD);
}
}
else if (motor_levels.speed_channel_level < 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_REVERSE))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_REVERSE);
}
}
else if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
motor_state = MOTOR_STATE_IDLE;
set_motor_direction(MOTOR_DIRECTION_STOPPED);
}
// motor fault is done
if (motor_state != MOTOR_STATE_BRAKE_READ_BACK_EMF)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else if (++motor_read_back_emf_count == BACK_EMF_READ_COUNT)
{
start_braking();
}
else
{
set_sensor_waiting_for_measurement(true);
}
}
break;
case MOTOR_STATE_COASTING:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_IDLE;
}
break;
case MOTOR_STATE_ACTIVE:
if (check_for_timeout(current_time, motor_timestamp, motor_timeout))
{
motor_levels.speed_channel_level = 0;
start_braking();
debug_P(PSTR("MOTOR Timeout @ %lu\n"), get_current_time());
post_error(ERR_MOTOR_TIMEOUT);
}
else
{
set_speed_motor_pwm_level(motor_levels.speed_channel_level);
set_direction_motor_pwm_level(motor_levels.direction_channel_level);
}
break;
}
}
/*----------------------------------------------------------------------
Read input characters with lots of processing for arrow keys and such (UNIX)
Args: time_out -- The timeout to for the reads
Result: returns the character read. Possible special chars.
This deals with function and arrow keys as well.
The idea is that this routine handles all escape codes so it done in
only one place. Especially so the back arrow key can work when entering
things on a line. Also so all function keys can be disabled and not
cause weird things to happen.
---*/
UCS
read_char(int time_out)
{
UCS status, cc, ch;
int (*key_rec)(int);
key_rec = key_recorder;
if(ps_global->conceal_sensitive_debugging)
key_rec = NULL;
/* Get input from initial-keystrokes */
if(process_config_input(&cc)){
ch = cc;
return(ch);
}
if((ch = check_for_timeout(time_out)) != READY_TO_READ)
goto done;
switch(status = kbseq(pine_simple_ttgetc, key_rec, read_bail,
ps_global->input_cs, &ch)){
case KEY_DOUBLE_ESC:
/*
* Special hack to get around comm devices eating control characters.
*/
if(check_for_timeout(5) != READY_TO_READ){
dprint((9, "Read char: incomplete double escape timed out...\n"));
ch = KEY_JUNK; /* user typed ESC ESC, then stopped */
goto done;
}
else
ch = READ_A_CHAR();
ch &= 0x7f;
/* We allow a 3-digit number between 001 and 255 */
if(isdigit((unsigned char) ch)){
int n = 0, i = ch - '0';
if(i < 0 || i > 2){
dprint((9, "Read char: double escape followed by 1st digit not 0, 1, or 2... (%d)\n", i));
ch = KEY_JUNK;
goto done; /* bogus literal char value */
}
while(n++ < 2){
if(check_for_timeout(5) != READY_TO_READ
|| (!isdigit((unsigned char) (ch = READ_A_CHAR()))
|| (n == 1 && i == 2 && ch > '5')
|| (n == 2 && i == 25 && ch > '5'))){
dprint((9, "Read char: bad double escape, either timed out or too large 3-digit num...\n"));
ch = KEY_JUNK; /* user typed ESC ESC #, stopped */
goto done;
}
i = (i * 10) + (ch - '0');
}
ch = i;
}
else{ /* or, normal case, ESC ESC c means ^c */
if(islower((unsigned char) ch)) /* canonicalize if alpha */
ch = toupper((unsigned char) ch);
ch = (isalpha((unsigned char)ch) || ch == '@'
|| (ch >= '[' && ch <= '_'))
? ctrl(ch) : ((ch == SPACE) ? ctrl('@'): ch);
dprint((9, "Read char: this is a successful double escape...\n"));
}
goto done;
#ifdef MOUSE
case KEY_XTERM_MOUSE:
if(mouseexist()){
/*
* Special hack to get mouse events from an xterm.
* Get the details, then pass it past the keymenu event
* handler, and then to the installed handler if there
* is one...
*/
static int down = 0;
int x, y, button;
unsigned long cmd;
clear_cursor_pos();
button = READ_A_CHAR() & 0x03;
x = READ_A_CHAR() - '!';
y = READ_A_CHAR() - '!';
ch = NO_OP_COMMAND;
if(button == 0){ /* xterm button 1 down */
down = 1;
if(checkmouse(&cmd, 1, x, y))
ch = cmd;
}
else if (down && button == 3){
down = 0;
if(checkmouse(&cmd, 0, x, y))
ch = cmd;
}
goto done;
}
break;
#endif /* MOUSE */
case KEY_UP :
case KEY_DOWN :
case KEY_RIGHT :
case KEY_LEFT :
case KEY_PGUP :
case KEY_PGDN :
case KEY_HOME :
case KEY_END :
case KEY_DEL :
case PF1 :
case PF2 :
case PF3 :
case PF4 :
case PF5 :
case PF6 :
case PF7 :
case PF8 :
case PF9 :
case PF10 :
case PF11 :
case PF12 :
dprint((9, "Read char returning: 0x%x %s\n", status, pretty_command(status)));
return(status);
case CTRL_KEY_UP :
status = KEY_UP;
dprint((9, "Read char returning: 0x%x %s (for CTRL_KEY_UP)\n", status, pretty_command(status)));
return(status);
case CTRL_KEY_DOWN :
status = KEY_DOWN;
dprint((9, "Read char returning: 0x%x %s (for CTRL_KEY_DOWN)\n", status, pretty_command(status)));
return(status);
case CTRL_KEY_RIGHT :
status = KEY_RIGHT;
dprint((9, "Read char returning: 0x%x %s (for CTRL_KEY_RIGHT)\n", status, pretty_command(status)));
return(status);
case CTRL_KEY_LEFT :
status = KEY_LEFT;
dprint((9, "Read char returning: 0x%x %s (for CTRL_KEY_LEFT)\n", status, pretty_command(status)));
return(status);
case KEY_SWALLOW_Z:
status = KEY_JUNK;
case KEY_SWAL_UP:
case KEY_SWAL_DOWN:
case KEY_SWAL_LEFT:
case KEY_SWAL_RIGHT:
do
if(check_for_timeout(2) != READY_TO_READ){
status = KEY_JUNK;
break;
}
while(!strchr("~qz", READ_A_CHAR()));
ch = (status == KEY_JUNK) ? status : status - (KEY_SWAL_UP - KEY_UP);
goto done;
case KEY_KERMIT:
do{
cc = ch;
if(check_for_timeout(2) != READY_TO_READ){
status = KEY_JUNK;
break;
}
else
ch = READ_A_CHAR();
}while(cc != '\033' && ch != '\\');
ch = KEY_JUNK;
goto done;
case BADESC:
ch = KEY_JUNK;
goto done;
case 0: /* regular character */
default:
/*
* we used to strip (ch &= 0x7f;), but this seems much cleaner
* in the face of line noise and has the benefit of making it
* tougher to emit mistakenly labeled MIME...
*/
if((ch & ~0x7f)
&& ((!ps_global->keyboard_charmap || !strucmp(ps_global->keyboard_charmap, "US-ASCII"))
&& (!ps_global->display_charmap || !strucmp(ps_global->display_charmap, "US-ASCII")))){
dprint((9, "Read char sees ch = 0x%x status=0x%x, returns KEY_JUNK\n", ch, status));
return(KEY_JUNK);
}
else if(ch == ctrl('Z')){
dprint((9, "Read char got ^Z, calling do_suspend\n"));
ch = do_suspend();
dprint((9, "After do_suspend Read char returns 0x%x %s\n", ch, pretty_command(ch)));
return(ch);
}
#ifdef MOUSE
else if(ch == ctrl('\\')){
int e;
dprint((9, "Read char got ^\\, toggle xterm mouse\n"));
if(F_ON(F_ENABLE_MOUSE, ps_global)){
(e=mouseexist()) ? end_mouse() : (void) init_mouse();
if(e != mouseexist())
q_status_message1(SM_ASYNC, 0, 2, "Xterm mouse tracking %s!",
mouseexist() ? "on" : "off");
else if(!e)
q_status_message1(SM_ASYNC, 0, 2, "See help for feature \"%s\" ($DISPLAY variable set?)", pretty_feature_name(feature_list_name(F_ENABLE_MOUSE), -1));
}
else
q_status_message1(SM_ASYNC, 0, 2, "Feature \"%s\" not enabled",
pretty_feature_name(feature_list_name(F_ENABLE_MOUSE), -1));
return(NO_OP_COMMAND);
}
#endif /* MOUSE */
done:
#ifdef DEBUG
if(ps_global->conceal_sensitive_debugging && debug < 10){
dprint((9, "Read char returning: <hidden char>\n"));
}
else{
dprint((9, "Read char returning: 0x%x %s\n", ch, pretty_command(ch)));
}
#endif
return(ch);
}
/* not reachable */
return(KEY_JUNK);
}
