// ------- Motor controller state machine -------
//
void motor_step(void) {
switch(m_out.state){
case MSTATE_READY:
// inputs
if(mmsg&MMSG_FAIL){
motor_ready();
m_out.state=MSTATE_FAIL;
report_interval=RUN_REPORT_INTERVAL;
} else if(mmsg&MMSG_POWER) {
left_power=m_in.l_pwr;
right_power=m_in.r_pwr;
motor_set_power();
set_timer(&power_timer);
m_out.state=MSTATE_RUN;
report_interval=RUN_REPORT_INTERVAL;
}
// clear inputs
mmsg=0;
break;
case MSTATE_RUN:
// if Pi is not talking frequently enough (dead?) stop everything
// and do not keep driving towards the cliff
if(interval_elapsed(&power_timer,MAX_POWER_SETTING_INTERVAL)){
mmsg=MMSG_FAIL;
}
// inputs
if(mmsg&MMSG_FAIL) {
motor_ready();
m_out.state=MSTATE_FAIL;
mmsg=0;
} else if(mmsg&MMSG_POWER) {
set_timer(&power_timer);
left_power=m_in.l_pwr;
right_power=m_in.r_pwr;
motor_set_left(left_power);
motor_set_right(right_power);
m_out.state=MSTATE_RUN;
} else if(mmsg&MMSG_STOP) {
motor_ready();
m_out.state=MSTATE_READY;
report_interval=READY_REPORT_INTERVAL;
}
// clear inputs
mmsg=0;
break;
case MSTATE_FAIL:
// inputs
if(mmsg&MMSG_CLEAR){
m_out.state=MSTATE_READY;
report_interval=READY_REPORT_INTERVAL;
}
// clear inputs and keep steady
mmsg=0;
motor_ready();
break;
}
}
static inline void
motor_ready(void){
// set pwm to 0
left_power=right_power=0;
motor_set_power();
// motor in breaking state
P2OUT &= ~(LINA|LINB|RINA|RINB);
}
void state_idle_init()
{
motor_set_power(0);
motor_set_enabled(1);
motor_set_direction(MOTOR_FORWARDS);
steering_set_direction(0);
steering_set_enabled(1);
lcd_set_touch_region(0, 0, 240, 320);
lcd_draw_header("IDLE");
measurer_print_info();
lcd_set_transparent_font(1);
lcd_draw_button(1, 30, 250, 0, 0, 31, 3, 31, 63, 31, 2, 2, " DRIVE ");
lcd_set_transparent_font(0);
}
void state_drive_init()
{
motor_set_power(0);
motor_set_enabled(1);
motor_set_direction(MOTOR_FORWARDS);
steering_set_direction(0);
steering_set_enabled(1);
controller_steering_reset();
controller_motor_reset();
irsens_reset();
tacho_reset();
measurer_reset();
lcd_draw_header("DRIVE");
measurer_print_info();
}
