/****************************************************************************/ /* */ /* A simple line tracker demo. */ /* */ /* Lines are assumed to curve to the right, and initially encountered */ /* from the right (for example, from the inside of the test pad). */ /* Straight line tracking needs to define STRAIGHT_LINE. */ /* */ /* Assumes motors on A,C, light sensors on port 2 */ /* */ /* Iain McInnes 2004-01-02: */ /* Based on a demo program by Markus L. Noga */ /* Modified to use user installed timer, and event queue. */ /* */ /****************************************************************************/ #include #if defined(CONF_DSENSOR) && defined(CONF_DMOTOR) #include // display functions #include #include // task manager #include // sensor routines #include // motor routines #include // OS timing #include "equeue.h" // Event queues #include "sm.h" // State machine engine #include "actobj.h" // Active objects /*---------------------------------------------------------------------------*/ #define LIGHTSENS SENSOR_1 #define NORMAL_SPEED (MAX_SPEED) #define TURN_SPEED (MAX_SPEED) /* Constructopaedia line follower: A motor drives left wheels, C motor drives right wheels */ void m_turn_left (void) { motor_a_speed (TURN_SPEED); motor_c_speed (TURN_SPEED); motor_a_dir (brake); motor_c_dir (fwd); } void m_turn_right (void) { motor_a_speed (TURN_SPEED); motor_c_speed (TURN_SPEED); motor_a_dir (fwd); motor_c_dir (brake); } void m_forward (void) { motor_a_speed (NORMAL_SPEED); motor_c_speed (NORMAL_SPEED); motor_a_dir (fwd); motor_c_dir (fwd); } void m_reverse_left (void) { motor_a_speed (TURN_SPEED); motor_c_speed (TURN_SPEED); motor_a_dir (brake); motor_c_dir (rev); } void m_stop (void) { motor_a_dir (brake); motor_c_dir (brake); } /* Calibrated sensor thresholds */ static unsigned short CAL_DARK, CAL_BRIGHT; /* Line tracker events: Sensor needs to know these */ enum { E_DARK = E_APP, E_BRIGHT }; /*---------------------------------------------------------------------------*/ /* Active objects */ static active_object_t * p_line_det; /* Detect on or off line */ static active_object_t * p_line_track; /* Track the line */ /*---------------------------------------------------------------------------*/ /* Sensor Handler: keep track of whether the sensor input is bright or dark. - When cross DARK_THRESH from bright to dark, send an E_DARK event. - When cross BRIGHT_THRESH from dark to bright, send an E_BRIGHT event. A more sophisticated handler could do more signal conditioning. Creating an active object: 1) define the input events. Assign the first to E_APP 2) Declare the state handler functions - one per state. The first should be an init function 3) Create and initialise a state table containing pointers to the handlers 4) Declare an enumeration for the states that maps on to the state table. 5) Declare any auxiliary variables that will be used by the state machine. The first (init) handler function should initialise them. 6) Define the handler functions */ fsm_handler sense_init; /* Set up and settle */ fsm_handler sense_cal; /* find and recognise line; calibrate thresholds */ fsm_handler sense_dark; /* Detect off->on line */ fsm_handler sense_bright; /* Detect on-> off line */ /* Line follower states */ static fsm_handler * Sensor_states [] = { sense_init, sense_cal, sense_dark, sense_bright }; enum { SENSE_INIT, SENSE_CAL, SENSE_DARK, SENSE_BRIGHT }; static unsigned short sensor_min = 10000; static unsigned short sensor_max = 0; /* Uncalibrated; Activate sensor and settle */ void sense_init (sm_t * psm, event_t e) { switch (e) { /* Make sensor active, and settle 100 ms */ case E_ENTRY: ds_active (&LIGHTSENS); ao_timer_start (p_line_det, 100); break; case E_TIME: ao_timer_repeat (p_line_det, 5); /* sample every 5 ms */ sm_goto (psm, SENSE_CAL); break; } } /* go forward until get dark, and then bright. record the extrema. CAL_DARK = light_min + (light_max - light_min) / 3. CAL_BRIGHT = light_min + 2 *(light_max - light_min) / 3 */ void sense_cal (sm_t * psm, event_t e) { unsigned short read = LIGHT (LIGHTSENS); cputw (read); sensor_min = (read < sensor_min) ? read : sensor_min; sensor_max = (read > sensor_max) ? read : sensor_max; if (read > sensor_min + 5) /* have gone past darkest */ { CAL_DARK = sensor_min + (sensor_max - sensor_min) / 3; CAL_BRIGHT = sensor_min + 2 *(sensor_max - sensor_min) / 3; ao_send_event (p_line_track, E_BRIGHT); /* tell line tracker */ sm_goto (psm, SENSE_BRIGHT); ao_timer_repeat (p_line_det, 20); /* sample every 20 ms */ } } void sense_dark (sm_t * psm, event_t e) { switch (e) { case E_ENTRY: { // cputs ("dark"); break; } case E_TIME: { if (LIGHT (LIGHTSENS) > CAL_BRIGHT) { ao_send_event (p_line_track, E_BRIGHT); sm_goto (psm, SENSE_BRIGHT); } break; } } } void sense_bright (sm_t * psm, event_t e) { switch (e) { case E_ENTRY: { // cputs ("bri"); break; } case E_TIME: { if (LIGHT (LIGHTSENS) < CAL_DARK) { ao_send_event (p_line_track, E_DARK); sm_goto (psm, SENSE_DARK); } break; } } } /*===========================================================================*/ /* Line Tracker state machine */ fsm_handler state_cal; fsm_handler state_on_line; fsm_handler state_off_line; fsm_handler state_lost_line; /* States */ /* Look up table for handlers: these must match the enum */ static fsm_handler * Line_states [] = { state_cal, state_on_line, state_off_line, state_lost_line }; enum { S_CAL, S_ON_LINE, S_OFF_LINE, S_LOST_LINE }; /*---------------------------------------------------------------------------*/ /* calibrating: go forward until sensor can recognise line and calibrate. Will get E_BRIGHT msg when success. */ void state_cal (sm_t * psm, event_t e) { switch (e) { case E_ENTRY : m_forward (); cputs ("cal"); break; case E_BRIGHT: sm_goto (psm, S_OFF_LINE); break; } } /* On the line: turn to the right */ void state_on_line (sm_t * psm, event_t e) { switch (e) { case E_ENTRY : m_turn_right (); cputs ("on"); break; case E_BRIGHT: sm_goto (psm, S_OFF_LINE); break; } } /* off line: turn to the left */ void state_off_line (sm_t * psm, event_t e) { switch (e) { case E_ENTRY: m_turn_left(); ao_timer_start (p_line_track, 1000); cputs ("off"); break; case E_DARK : ao_timer_stop (p_line_track); sm_goto (psm, S_ON_LINE ); break; case E_TIME : sm_goto (psm, S_LOST_LINE); break; } } /* Have lost the line: back up until find it */ void state_lost_line (sm_t * psm, event_t e) { switch (e) { case E_ENTRY: m_reverse_left (); cputs ("lost"); break; case E_DARK : sm_goto(psm, S_ON_LINE); break; } } /*---------------------------------------------------------------------------*/ /* Shut everything down: turn off sensor, stop timers, stop motors */ void linetrack_exit (void) { // sys_timer_stop (&Sensor_timer); !!! figure out a solution to this m_stop (); ds_passive (&LIGHTSENS); } /*---------------------------------------------------------------------------*/ /* Create a list of active objects, and put tracker and sensor objects on it Start the objects (put them into their initial states) Then set them running */ /* Event queue size; must be a power of 2; Allocate one more than needed */ #define EQ_SIZE 0x4 int main (void) { active_object_t * p_objects = 0; p_line_track = ao_new_fsm (EQ_SIZE, Line_states ); p_line_det = ao_new_fsm (EQ_SIZE, Sensor_states); ao_add_front (&p_objects, p_line_track); ao_add_front (&p_objects, p_line_det); ao_start_all (p_objects); ao_run (p_objects); linetrack_exit (); ao_delete_all (p_objects); return 0; } /*---------------------------------------------------------------------------*/ #else #warning linetrack.c requires CONF_DSENSOR && CONF_DMOTOR #warning linetrack demo will do nothing int main(int argc, char *argv[]) { return 0; } #endif // defined(CONF_DSENSOR) && defined(CONF_DMOTOR) /*---------------------------------------------------------------------------*/