Enhanced SRS Workshop Robot

by tomdean

SRS Workshop Kit.

I purchased an SRS Workshop Robot as a learning tool and easy platform to develop initial software while designing a more capable machine. Refer to Seattle Robotics Society for purchase details. The kit is an excellent starting platform. The parts fit together easily and the instructions are excellent. I purchased both the level 1 and level 2 kits plus the wiring accessories.

Assembling the kit took one day.

The software with the kit loaded easily and worked as described.

The Robot under development.

Another view.

SRS Workshop Robot

Test driving around a square without PID.

Improved Turning Radius, driving around a square without PID.

The motor control does not stop the motor before reversing, creating obvious distress in the gearhead. Odometry uses the geographical sense of 000 North, and, right turns are positive.
If the requested turn is more than 45 degrees, one wheel is stopped and reversed.

Added Motor Stop-Before-Reverse and Improved Math. No PID.

The location deadband is 1.0 inch and the heading deadband is 10 degrees. The motor command stops the motor before changing direction to reduce wear on the gearhead.
If the requested turn is more than 45 degrees, one wheel is stopped and reversed.

Telemerty Data vs Measured Location

I ran the SRS Workshop Robot around a 24 inch square, on the smooth side of a C-D sheet of plywood. I repeated the runs 6 times. Two of the runs were offset significantly at the first point and ran off the plywood between the first and second points.

All the tests started at the same location marked by a dot. The robot was placed on the plywood with the dot approximately centered between the wheels. The rear edge of the robot was aligned perpendicular to the edge of the plywood using a 24 inch square. The robot was facing parallel to the long edge of the plywood. The first point was 90 degrees to the left and each subsequent point was rotated 90 degrees from the previous. The (x,y) locations of the points were (0,-24), (-24,24), (-24,0), (0,0). The robot stopped at each point for three seconds and a mark was make on the plywood approximately centered between the wheels. A future improvement will be to use an active sensor to measure the location. The first graph shows the telemetry data.

Telemetry Data

Measured Position

Corrected Problems with Noise on the Interrupt Lines

I added 3 ea 0.1uf caps to each motor, one between the leads and one from each lead to the case. The case is not connected to the controller or power circuitry, but, is connected to the shaft and rotor.

I use a 24 inch square on the smooth side of a C-D plywood sheet, for testing, starting at (0,0) pointed at 000, and traveling to (0,24), (-24,24), (-24,0), and (0,0) in succession, stopping st each point for 3 seconds to allow me to manually record the position, by placing a mark beneath the center of the axle. I initially align with the axis using a large carpender's square.

I made two runs around the 24 inch square, almost perfectly.

I measured the position at each stopping point and summed the vectors. The sum was (1.250,1.124) and (1.250,-0.125), respectively, for the two runs. Dividing by the nominal 96 inches traveled, this is about 1%!

On the first run, the robot made a funny turn at (0,24), first rotating left, then right, then almost a full rotation to the left. It seemed to not slip since it arrived at (-24,24) with little error. This funny turn was not repeated in the second run. I believe this turn was because the direction is Pi.

Here are the results. I connected the points at the 4 corners with lines to make visual correlation easier.
After Adding Motor Capacitors

SRS Workshop Interconnect Board

I keep knocking the jumpers out of the prototype board and I need to add some sensors. I am making an interconnect board to replace the prototype board.

SRS Workshop Interconnect

Enhanced Software

My development computer(s) run FreeBSD 7.0, from the BSd wing of Unix. I use the avr-gcc compiler from Avrfreaks and avr-libc from Avrfreaks. Avrfreaks has an excellent introduction and a tutorial, as well as versions of WINAVR, etc. A tarball of the application is at Source Tarball.

RTOS

I wrote an RTOS a few years ago for an mc68hc11 project.
Porting this to the ATmega16/32 processor seemed a lot of work.

AvrX

Googling 'avr rtos', I found AvrX, written by Larry Barello.
Downloading the source for AvrX 2.6, I compiled and programmed the Timer example into an ATmega16 in an Atmel STK5000v2 with avrdude, also available from Avrfreaks.
Using an oscilloscope, I verified the timing was correct.
Connecting a serial port, the monitor task was running. After some experimenting, I used avr-objdump -Dz main.elf and located the control blocks.
This made it possible to examine task states, stack, and address space.
It would be nice if the monitor task allowed displaying the timer queue and task control blocks with a single command.
Also, dumping flash may be useful.
However, the monitor allows an experienced developer to debug task problems.
Inserting a while(1) loop in a task, it was fairly simple to determine the cycle hog among several tasks or the same priority.

Preliminary Design

The preliminary design of the SRS Workshop Robot was loosely based on the SR04 Mobile Robot by David Anderson, as described in an SRS Encoder Article.
David Anderson documents his robots on the SMU Gelogy Department Page.
He has written an excellent article on system software design, see Dallas Personal Robots Group.

Motion Control

The motion of the Robot is controlled by the output of the behaviors as determined by the sensors and user input. The sensors include physical contact switches, light sensors, IR sensors and sonar sensors. The user input is determined at compile time of the application.

The primary control mechanism is contained in the behaviors periodic task, executing the functions listed in the Behaviors Table.

Motor Control

Motor control commands are from the motion control tasks, primarily behaviors. The input to the motor function is includes a speed value and a turn argument. The speed value is from -100% to 100%. The turn value is a degree value, negative for left and positive for right.

The basic motion of the robot can be reduced to the left and right wheel encoder counts. The wheel diameter and the wheel base are the constants of proportionality. The distance moved by each wheel can be calculated from the respective encoder counts. The encoder is connected to an ATmega external interrupt pin and the interrupt sense is set to any logic level change. The wheel encoders have 32 light and dark stripes, giving 64 interrupts per wheel revolution. The wheel diameter is 2.1 inches and the wheel base is 5.3 inches. The robot travels 6.6 inches per wheel revolution.

If the robot is traveling straight, either forward or backwards, the wheel counts should be equal. If the robot is turning, the wheel counts should differ by an amount determined by the number of degrees the robot has turned. This applies equally to the robot rotating in place, one drive wheel turning forward and the other turning backwards.
The difference in encoder counts can be calculated from the number of degrees desired, Encoder Scilab.

Applying the encoder counts per revolution, wheel diameter and wheel base, the number of encoder count versus the number of degrees turned can be calculated:

Counts = 0.897 * Degrees.

Determining Encoder Counts From Motion Commands
Direction	Turn	Encoder Relationships	Target
Forward	Straight	Left Encoder == Right Encoder	0
	Left	Left Encoder < Right Encoder	-- Counts
	Right	Left Encoder > Right Encoder	+ Counts
Reverse	Straight	Left Encoder == Right Encoder	0
	Left	Left Encoder > Right Encoder	+ Counts
	Right	Left Encoder < Right Encode	-- Counts
Rotate	Left	Left Encoder < Right Encode	-- Counts
	Right	Left Encoder > Right Encoder	+ Counts
Stop	N/A	Left Encoder == Right Encoder == 0	0

Task Organization

The task organization is loosely based on the SR04 system design.
A series of behaviors read the robot status and decide indivudual actions to take.
Arbitration selects the highest priority behavior and passes its decision to the motor function.
The motor function uses a PID to decide on the appropriate drive to each of the motors.

The behaviors are

Behaviors
Behavior	Priority	Reacts To	Description	Pseudo Code
Escape	1	Physical contact.	Uses contact switches on the front of the robot. As a ballistic behavior, it is an augmented finite state machine. Upon initial contact, the robot is commanded to back up for 1 second. Then, to turn left and go forward for one-half second. Then to go forward to maximum speed. If the contact repeats within a dead band time frame, the robot starts a turn in a random direction by a random amount and selects forward or backwards in a random manner until it is free.	Behaviors()
Steps	2	Light sensors looking down.	Uses the analog voltage from light sensors mounted on the bottom front of the robot. As a ballistic behavior, it is an augmented finite state machine. Upon detecting a step or an increased distance from the bottom of the robot to a lower reflective surface, the robot stops, spins in place to the left ninety degrees. If the distance has returned to normal, the robot goes forward at full speed. If the distance has not returned to normal, the robot repeats the turn and detection. If the robot does not detect a normal surface after a full turn, it stops and cries for help.	Steps()
Distance	3	IR sensors.	Uses the analog voltage from IR distance sensors mounted on the front corners of the robot.	Distance()
Boundary	4	Abstract navigation.	Uses artificial lines from an initial point.	Boundary()
Sonar	5	Acoustic ranging.	Uses ranging data from a sonic distance sensor mounted on the front of the robot.	Sonar()
Xlate	6	Turn and rotate.	Turns and offsets the robot position, based on current position and direction parameters.	Xlate()
Navigate	7	Abstract navigation.	Go from the current position to a position defined in x-y coordinates.	Navigate()
Cruise	8	Go straight.	Go straight ahead at full speed.	Cruise()
Prowl	9	Wander around.	Go in random directions for random times.	Prowl()
Passive	10	Hide and watch.	Go to a point of least sensor detectsion and stop.	Passive()

The Periodic Tasks are run by AvrX from the timer queue, in priority order. Those tasks with lower priority will be run before those with higher priority. Tasks with equal priorities will be run in a round-robin manner.

Periodic Tasks
Task	Rate	Priority	Description	Pseudo Code
Behaviors	Runs at 20 Hz	6	Primary Control task, calls each behavior function, selects the highest priority and passes the command to the motor function.	Behaviors()
Datalog	Runs every 30 seconds	8	Telemetry to a local computer for later analysis of inputs and reactions.	Datalog()
Display	Runs ar 2 Hz	10	Displays selected data on an onboard LCD.	Display()
Location	Runs at 20 Hz	3	Calculates the relative location of a given target.	Location()
Odometer	Runs at 20Hz	2	Calculates the current location relative to the last reset.	Odometer()
Health	Runs every 1 minute	14	Checks things like battery voltage and puts the result in the telemetry stream	Health()
Range	Runs every 50 msec	4	Pulses the sonar and waits for a return which may take up to 40 msec.	Range()
Trace	If debugging, runs every 50 msec	15	For debugging, as an alternative to the datalog task, provides a more detailed data stream.	Trace()

The Support functions perform miscelleous functions for the periodic tasks, behaviors, and run the timer.

Support Functions
adc(channel)	Return the adc value for the requested channel
odometer_reset()	Resets the odometer. Future locations are relative to the position when the odometer was cleared.
timer_clear()	Clears the time counter.
ISR(SIG_OVERFLOW0)	Interrupts every 1 msec, incrementing the time counter. Calls AvrXTimerHandler() to possibly do a context switch.
motor(*motor_CB)	The motor_CB argument is the control block from the highest priority behavior needing serevice. Use the control block to change the respective motor PWM and direction values, thus controlling the motion of the robot.	Motor()

Pseudo Code

The Behaviors periodic task is the main action loop of the application. Behaviors is scheduled to run at an intervals controlled by a define in Sys.h. The task calls each of the behaviors in priority order, retaining the control block of the highest proority task. A delay of 2 msec is inserted to move the motor control algorithms into another timer cycle. Then, Behaviors calls the motor_cmd function to enact the commands of the highest priority behavior. The result is to call each of the behaviors at nearly constant intervals and to call the motor_cmd function in at nearly constant intervals. To accomplish this, Behaviors uses two timers, one determines the overall loop interval and the second delays the call to the motor_cmd function.

behaviors() {
while(1) {
    AvrXStartTimer(1)
    escape()
    steps()
    distance()
    boundary()
    sonar()
    xlate()navigate()
    cruise()
    prowl()
    passive()
    arbitrate()
    AvrXStartTimer(2) - wait 2 ms to get into another timer block
    AvrXWaitTimer(2)
    motor_cmd()
    AvrXWaitTimer(1)
}
}

The Datalog Periodic task.

datalog() {
while (1) {
AvrXStartTimer()
write the data stream to serial I/O
AvrXWaitTimer()
}
}

The Display Periodic Task

display() {
while (1) {
AvrXStartTimer()
write data to the LCD
AvrXWaitTimer()
}
}

The Health Periodic Task.

health() {
while (1) {
AvrXStartTimer()
Check battery voltage, etc.
AvrXWaitTimer()
}
}

The Location Periodic Task uses the global variables x_target and Y_target to calculate the global variables target_bearing, target_heading_error, and, target_distance.

location() {
while (1) {
    AvrXStartTimer()
x = x_target - x_position
    y = y_target - y_position
    target_distance = sqrt(x*x + y*y)
    target_bearing = atan2(y,x)
    target_heading_error = fmod(target_bearing - theta,2.0*M_PI);
AvrXWaitTimer()
}
}

The Odometer Periodic Task uses the global variables left_distance and right_distance to calculate the global variables X_position, Y_position, odometer, and theta. Odometer it the accumulated distance traveled in inches. Theta is the current heading in radians.

odometer() {
while (1) {
    AvrXStartTimer()
    lcount   = left_distance;
    rcount   = right_distance;
    left_inches = (double)(lcount - last_lcount)/LEFT_COUNT_PER_INCH;
    right_inches = (double)(rcount - last_rcount)/RIGHT_COUNT_PER_INCH;
    last_lcount = lcount;
    last_rcount = rcount;
    this_inches = (left_inches + right_inches)/2;
    odometer += this_inches;
    this_theta = atan2(left_inches - right_inches,WHEEL_BASE);
    X_position += this_inches*cos(this_theta);
    Y_position += this_inches*sin(this_theta);
    theta += this_theta;
    theta = fmod(theta,2.0*M_PI);
AvrXWaitTimer()
}
}

The Range Periodic Task.

range() {
while (1) {
    AvrXStartTimer()
    write data to the LCD
    AvrXWaitTimer()
}
}

The Trace Periodic Task.

trace() {
if (debug) {
while (1) {
AvrXStartTimer()
    write data steam to serial I/O
      AvrXWaitTimer()
    }
}
}

The Escape Behavior.

escape() {
if (bumper) {
set escape_cb->cmd
set escape_cb->arg
set escape_cb->flag
}
}

The Steps Behavior.

steps() {
if (no light) {
set steps_cb->cmd
set steps_cb->arg
set steps_cb->flag
}
}

The Distance Behavior.

distance() {
if (distance < maximum) {
set distance_cb->cmd
set distance_cb->arg
set distance_cb->flag
}
}

The Boundary Behavior.

boundary() {
set boundar_cb->cmd
set boundar_cb->arg
set boundar_cb->flag
}

The Sonar Behavior.

sonar() {
if (sonar < maximum) {
set sonar_cb->cmd
set sonar_cb->arg
set sonar_cb->flag
}
}

The Xlate Behavior.

xlate() {
set xlate_cb->cmd
set xlate_cb->arg
set xlate_cb->flag
}

The Navigate Behavior.

navigate() {
set navigate_cb->cmd
set navigate_cb->arg
set navigate_cb->flag
}

The Cruise Behavior.

cruise() {
set cruise_cb->cmd
set cruise_cb->arg
set cruise_cb->flag
}

The Prowl Behavior.

prowl() {
set prowl_cb->cmd
set prowl_cb->arg
set prowl_cb->flag
}

Scilab code to calculate encoder counts versus turning
// scilab calculation of the encoder count slope.
// wheel diameter 2.1 inches
dia=2.1
// wheelbase 5.3 inches
wb=5.3
// 64 counts/revolution
count_per_rev=64

//
// Calculate the angle turned from the distance each wheel has moved.
// Ldist = (radius + wb)*theta
// Rdist = radius*theta
// Ldist - Rdist = wb*theta
// theta = (Ldist - Rdist)/wb
// median = (radius + wb/2)*theta
// Ldist + Rdist = (2*radius + wb)*theta = 2*(radius + wb/2)*theta
// median = (Ldist + Rdist)/2
//
// Ldist = ((Lenc counts)/(64 counts/rev))*(dia*%pi inches/rev)
// = dia*%pi*Lenc/64 inches
// Rdist = dia*%pi*Renc/64 inches
//
// theta = (dia*%pi*Lenc/64 - dia*%pi*Renc/64)/wb
// = (Lenc - Renc)*dia*%pi/count_per_rev/wb
//
// encoder counts to turn 360 degrees
//
// the difference in Lenc and Renc to turn 360 degrees
counts_per_circle=(360*%pi/180)/(dia*%pi/count_per_rev/wb)
//
// This is linear, match it
// intercept = 0. Slope = counts_per_dircle/360
slope=counts_per_circle/360
//
// avoid floating point math. We want a 16 bit result, so
// multiply the slope by 2^16 and keep the high 16 bits of the result
//
mult=round(slope*(2^16))
//
// the differenct in encoder counts to turn D degrees is
// mult*D/(2^16)
//
function [e]=enc(d)
e=mult*d/2^16;
endfunction;

d=[0:10:360];
printf("%3d %3d\n",[d',enc(d')])

The Passive Behavior.

passive() {
set passive_cb->cmd
set passive_cb->arg
set passive_cb->flag
}

The Motor Function.

motor() {
}

Code

Sys.h

// sys.h -

// 20080519 tomdean - initial version

// $Id: sys.h,v 1.2 2008/05/28 20:00:51 tomdean Exp $

#pragma once

#include <avr/io.h>

//////////////////////////////////////////////////////////////////////
// types
typedef enum { READY,
                           WAIT } sys_state_t;
typedef struct behav_CB_t {
sys_state_t state;
char        cmd;
char        arg;
char        flag;
} behav_CB_t;

//////////////////////////////////////////////////////////////////////
// prototypes
void initialize(void);

//////////////////////////////////////////////////////////////////////
// macros
#define L_BUMPER     _BV(L_BUMPER_PIN)
#define R_BUMPER     _BV(R_BUMPER_PIN)

//////////////////////////////////////////////////////////////////////
// global definitions
#define FALSE 0
#define TRUE 1

#define TASK_LOOP_TIME 50
#define DATALOG_TIME   1000

#define LOOP_DELAY(us) \
     { volatile uint8_t lc = (uint8_t)(us*CPUCLK/1000000UL); \
       while (lc--); \
         }

#define DELAY_MS(d) \
        AvrXStartTimer(&display_lcd_timer, d); \
        AvrXWaitTimer(&display_lcd_timer)

Hardware.h

// hardware.h -

// 20080519 tomdean - initial version

// $Id: hardware.h,v 1.2 2008/05/28 20:00:51 tomdean Exp $

#pragma once

//////////////////////////////////////////////////////////////////////
// prototypes
void hardware_init(void);

//////////////////////////////////////////////////////////////////////
// timer definitions
#define CPUCLK 16000000L     // CPU xtal
#define TICKRATE 1000       // AvrX timer queue Tick rate
#define BAUDRATE 19200L     // Debug monitor baudrate
#define TCNT0_INIT (0xFF-CPUCLK/256/TICKRATE)

#define UBRR_INIT (CPUCLK/(16*BAUDRATE)-1)

#define TMC8_CK256 (1<<CS02)

//////////////////////////////////////////////////////////////////////
// Port connections:

//   PORTA 00 adc0   left_front_distance Sharp GD2D12
#define L_DIST_PORT PORTA
#define L_DIST_PIN 0
//   PORTA 01 adc1   left_light_sensor
#define L_LIGHT_PORT PORTA
#define L_LIGHT_PIN 1
//   PORTA 02 adc2   batt_volts
#define BATT_PORT PORTA
#define BATT_PIN 2
//   PORTA 03 adc3   right_light_sensor
#define R_LIGHT_PORT PORTA
#define R_LIGHT_PIN 3
//   PORTA 04 adc4   left_side_distance Sharp GD2D12
//   PORTA 05 adc5   right_side_distance Sharp GD2D12
//   PORTA 06 adc6
//   PORTA 07 adc7   right_ftont_distance Sharp GD2D12
#define R_DIST_PORT PORTA
#define R_DIST_PIN 7

//   PORTB 00 output distance_start Devantech SRF04
//   PORTB 01 output Tilt SPI Slave Select
#define TILT_SELECT_PORT PORTB
#define TILT_SELECT_PIN 1
//   PORTB 02 INT2    distance_int Devantech SRF04
#define RANGE_TRIG_PORT PORTB
#define RANGE_TRIG_PIN 2
//   PORTB 03 output lcd_enable
#define LCD_E_PORT PORTB
#define LCD_E_PIN 3
//   PORTB 04 output lcd_ss   SPI BUS
#define LCD_SELECT_PORT PORTB
#define LCD_SELECT_PIN 4
//   PORTB 05 MOSI    spi_mosi SPI BUS
#define MOSI_PORT PORTB
#define MOSI_PIN 5
//   PORTB 06 MISO    spi_miso SPI BUS
#define MISO_PORT PORTB
#define MISO_PIN 6
//   PORTB 07 SCK     spi_sck SPI BUS
#define SCK_PORT PORTB
#define SCK_PIN 7

//   PORTC 00 output lcd r/w
#define BUMPER_PORT PORTC
#define L_BUMPER_PORT PORTC
#define L_BUMPER_PIN 0
//   PORTC 01 output lcd rs
#define R_BUMPER_PORT PORTC
#define R_BUMPER_PIN 1
//   PORTC 02 input   left_encoder_chan_b
#define L_ENC_B_PORT PORTC
#define L_ENC_B_PIN 2
//   PORTC 03 output left_motor_dir
#define L_MOTOR_DIR_PORT PORTC
#define L_MOTOR_DIR_PIN 3
//   PORTC 04 output right_motor_dir
#define R_MOTOR_DIR_PORT PORTC
#define R_MOTOR_DIR_PIN 4
//   PORTC 05 input   right_encoder_chan_b
#define R_ENC_B_PORT PORTC
#define R_ENC_B_PIN 5
//   PORTC 06
#define TILT_CS_PORT PORTC
#define TILT_CS_PIN 6
//   PORTC 07
#define C_BUMPER_PORT PORTC
#define C_BUMPER_PIN 7

//   PORTD 00 rxd     rxd
//   PORTD 01 txd     txd
//   PORTD 02 INT0    right_encoder_chan_a_int
#define R_ENC_A_PORT PORTD
#define R_ENC_A_PIN 2
//   PORTD 03 INT1    left_encoder_chan_a_int
#define L_ENC_A_PORT PORTD
#define L_ENC_A_PIN 3
//   PORTD 04 OC1B    left_motor_pwm
#define L_MOTOR_PWM_PORT PORTD
#define L_MOTOR_PWM_PIN 4
//   PORTD 05 OC1A    right_motor_pwm
#define R_MOTOR_PWM_PORT PORTD
#define R_MOTOR_PWM_PIN 5
//   PORTD 06
//   PORTD 07 OC2     servo_pwm
#define SERVO_PWM_PORT PORTD
#define SERVO_PWM_PIN 7

Main()

// main.c - use the avrx rtos to run the SRS sorkshop robot -*-C-*-

// see the individual files for details

// 20080519 tomdean - initial version

// $Id: main.c,v 1.2 2008/05/28 20:00:51 tomdean Exp $

/

//////////////////////////////////////////////////////////////
// includes
#include <avr/io.h>         // io ports
#include <avr/interrupt.h> // interrupt definitions
#include <inttypes.h>       // uint16_t
#include <sys.h>            // system definitions for this application
#include <hardware.h>       // port definitions for this application
#include <sys.h>            // system definitions for this application
// rtos definitions
#include <avrx.h>           // rtos definitions
// definitions for initialization functions
#include <adc.h>            // analog definitions
#include <arbitrate.h>      // Arbitrate task
#include <audio.h>          // audio definitions
#include <behaviors.h>      // behaviors periodic task
#include <boundary.h>       // Boundary behavior
#include <cruise.h>         // Cruise behavior
#include <datalog.h>        // Datalog periodic task
#include <display.h>        // Display periodic task
#include <distance.h>       // distance behavior
#include <encoder.h>        // encoder definitions
#include <escape.h>         // escape behavior
#include <health.h>         // Health periodic task
#include <motor.h>          // motor definitions
#include <navigate.h>       // Navigate behavior
#include <odometer.h>       // odometer definitions
#include <passive.h>        // Passive behavior
#include <prowl.h>          // Prowl behavior
#include <range.h>          // range periodic task
#include <serialio.h>       // serialio definitions
#include <servo.h>          // servo definitions
#include <sonar.h>          // Sonar behavior
#include <spi.h>            // spi definitions
#include <steps.h>          // Steps behavior
#include <tilt.h>           // 3D accelerator
#include <trace.h>          // Trace debugging task
#include <xlate.h>          // Xlate behavior

//////////////////////////////////////////////////////////////
// Define the task TCB's, stack, etc.
/**
* The task tcb's are defined in the task source files and
* declared extern in main.c
*/
/**
* periodic tasks, some have associated message helpers
*/
AVRX_EXTERNTASK(behaviors);
extern TimerControlBlock behaviors_timer;        // timer control block

AVRX_EXTERNTASK(datalog);
AVRX_EXTERNTASK(datalog_msg);
extern MessageControlBlock datalog_message;        // message control block
extern TimerControlBlock datalog_timer;        // timer control block

AVRX_EXTERNTASK(display);
AVRX_EXTERNTASK(display_msg);
extern MessageControlBlock display_message;        // message control block
extern TimerControlBlock display_timer;        // timer control block

AVRX_EXTERNTASK(health);
AVRX_EXTERNTASK(health_msg);
extern MessageControlBlock health_message;        // message control block
extern TimerControlBlock health_timer;        // timer control block

AVRX_EXTERNTASK(range);
extern TimerControlBlock range_timer;          // timer control block
extern MessageControlBlock range_message;          // message control block

AVRX_EXTERNTASK(trace);
extern TimerControlBlock trace_timer;          // timer control block
extern MessageControlBlock trace_message;          // message control block
/**
* message based tasks
*/
AVRX_EXTERNTASK(audio);
extern MessageControlBlock audio_message;          // message control block
/**
* debugging tasks
*/
#ifdef INCLUDE_MONITOR
AVRX_GCC_TASK(Monitor, 20, 0);
#endif

//////////////////////////////////////////////////////////////
// prototypes
void InitSerialIO(uint16_t);
void timer0_init(void); // initialize timer/counter 0

//////////////////////////////////////////////////////////////
// behavior task control blocks
behav_CB_t boundary_CB; // boundary Control Block
behav_CB_t behav_CB;    // behavior Control Block
behav_CB_t cruise_CB;   // cruise Control Block
behav_CB_t distance_CB; // distance Control Block
behav_CB_t escape_CB;   // escape Control Block
behav_CB_t navigate_CB; // navigate Control Block
behav_CB_t passive_CB; // passive Control Block
behav_CB_t prowl_CB;    // prowl Control Block
behav_CB_t sonar_CB;    // sonar Control Block
behav_CB_t steps_CB;    // steps Control Block
behav_CB_t xlate_CB;    // xlate Control Block

//////////////////////////////////////////////////////////////
// initialize
void initialize(void) {

//////////////////////////
// timer counter 0, system tick
timer0_init();

//////////////////////////
// AvrX tasks
InitSerialIO(UBRR_INIT);
//AvrXRunTask(TCB(Monitor));

//////////////////////////
// Behavoirs
escape_init(&escape_CB);      // Priority 1
steps_init(&steps_CB);        // Priority 2
distance_init(&distance_CB); // Priority 3
boundary_init(&boundary_CB); // Priority 4
sonar_init(&sonar_CB);        // Priority 5
xlate_init(&xlate_CB);        // Priority 6
navigate_init(&navigate_CB); // Priority 7
cruise_init(&cruise_CB);      // Priority 8
prowl_init(&prowl_CB);        // Priority 9
passive_init(&passive_CB);    // Priority 10

//////////////////////////
// periodic tasks - xxx_init() calls AvrXRunTask() as needed
behaviors_init(); // behaviors task
datalog_init();   // datalog task
spi_init();       // initialize the SPI bus BEFORE the LCD
display_init();   // initialize the LCD
health_init();    // health task
range_init();     // initialize the SRF04
trace_init();     // debugging

//////////////////////////
// message based tasks
audio_init();     // beeps and songs

//////////////////////////
// devices
adc_init();       // initialize ADC channels
encoder_init();   // initialize encoder inputs and interrupts
motor_init();     // initialize the motor PWM
servo_init();     // initialize the servo PWM
tilt_init();      // initialize the LIS3L

//////////////////////////
// supprt functions
odometer_init(); // odometers.c initialize the odometers
}

//////////////////////////////////////////////////////////////
// 32 bit time counter, incremented every 1 msec
uint32_t time_count;

#define scheduler() Epilog(); while(1)
//////////////////////////////////////////////////////////////
// main
int main() {
DDRB |= _BV(01) | _BV(00);
// zero the time counter
time_count = 0;
// set the kernel stack
AvrXSetKernelStack(0);
// initialize the hardware
initialize();

// return to the system to start scheduling tasks
scheduler();

// never get here
return 0;
}

//////////////////////////////////////////////////////////////
// initialize the timer
void timer0_init() {
TCNT0 = TCNT0_INIT; // (0xFF-CPUCLK/256/TICKRATE)
TCCR0 = _BV(CS02);   //
TIMSK = _BV(TOIE0); // start the timer
}
// timer 0 overflow interrupt
AVRX_SIGINT(SIG_OVERFLOW0) {
IntProlog();        // Switch to kernel stack/context
// keep this near the top of the routine
TCNT0 = TCNT0_INIT; // reset tcnt0, since we are using overflow
                      // to get 1 msec
time_count++;       // increment the 32 bit counter
PORTB ^= _BV(01);   // trace
AvrXTimerHandler(); // Call Time queue manager
Epilog();           // Return to tasks
}