Octobot - My Axon robot character

This robot has been assembled and programmed to introduce myself to the AXON and some of the latest robots sensors. I had done some robotics about 15 years ago.

I took many off-the-shelf components along with the AXON and have put together a robot that does the following:

- light following: it will follow light based upon sampling 3 different directions in front by using a turret mounted sensor and picking a direction (partially working needs to be tweaked a bit)

- wandering: it will basically wander around avoiding bumping into things by keeping the turret centered and reading both IR and SONAR. Works as a random walk of the environment.

- singing: chooses a song to sing

- dancing: plays a sound effect and performs small movement routine with various turns

- waiting: when waiting it randomly chooses and plays sound effects.

 

Three Software Layers

All the above behaviors are available all the time and are utilized in this highest level motivation software layer. The 'bot randomly chooses (although it's weighted) a behavior to follow and the behavior supplies a list of atomic operations (like move forward for X tics, stop for so long to plan another forward move) to a batch command processor. The command processor takes the commands and executes each step for the supplies duration. This highest level of software follows a sense-plan-act strategy.

 

In order to avoid ramming headlong into things I use a subsumption-light architecture that preempts the commands in the command processor. These overriding reflexes supplies new orders to the command processor for a temporary period. The motivation behavior then takes back over and supplies new orders. Some of the various sensors are read very often in the interrupt routine.

 

The lowest level of software is an interrupt routine that reads various sensors, sets various conditions and otherwise updates basic world and robot state. This is used by the middle layers of reflexes to anticipate collisions. This layers also used a sampling and smoothing algorithm to help with noise and spurious readings in the various sensors. The routine uses scaling and biasing to lower the influence that a single ( or few) faulty readings have.

 

Current physical elements on board at this time:

- 3-axis accelerometer - being read by interrupt routine but not otherwise utilized

- AXON controller

- Sharp IR sensor

- Sonar

- light level sensor

- light sensor

- Servo turret to point IR/SONAR And light sensor in different directions

- 2X16 Serial LCD for various information

- amplified speakers for->

- Rogue Robotics uMP3

- continuous rotation servos for drive wheels

- 3-tier chassis

 

 

 

To Be completed:

1) rear IR collision sensor

2) utilize the connected to 3 axis accelerometer to determine if movement appropriate for desired state, i.e. sense when no forward acceleration when expect movement or sense sideways acceleration when should only be forward

3) better mapping and movement algorithm for both wander and light seek mode

4) connect touch sensors to the pins programmed for them (I have bump sensor reflexes programmed but not in use)

5) battery monitor circuit to either shutdown or complain about recharge needed.

6) capacitance touch circuits on large areas to sense 'petting' and react with sound

7) continuous motion; right now I move forward so much, stop and scan the environment and then plan the next move

8) I have a movement sensor on the bot but it's been removed I may put a couple back on to have some way to to follow movement and torment the cats


Other ideas:

1) Love to do a recharge station

2) Zigbee communication to a home base

3) line of site follow/communication to another bot


I've spent a lot of time getting the sound and music to work since this robot is being made to interact with people (especially children) quire a bit. It'll get some initial use in my daughters Kindergarten class along with a bumpbot that I built.


 

Early picture:

Early Octobot

 

Current Picture with all basic elements mounted:

Octobot Version 1.0

 

Octobot Source Code

Here is the (messy) source as of January 11 2009, it'll keep changing for sure:

 

/****************************************************************************
*
* Octobot - TDS2.0
*
****************************************************************************/


// ***********************************************************************************

//void test_code(void);
void go_forward(void);
void go_backward(void);
void turn_left(void);
void turn_right(void);
void mot_stop(void);
void dance(void);

// ***********************************************************************************

void print_sep(void);

// ***********************************************************************************

void sample_sensors(void);
void sample_sensors_report(int,int);
int sample_digital(char port, int pin, int *avg, int thresh, int debug);

void sample_accelerometer(void); //

int sensorscaleweight(int *, int, int, int, int, int, int);

// ***********************************************************************************

void init_servos(void);
void init_sensors(void);

// ***********************************************************************************

#define SERVO_ON            1 // set to 1 to run servos and 0 to not run them
#define SERVO_CENTER         188
#define SERVO_RIGHT            120
#define SERVO_LEFT            220
#define SERVO_FULL_SPEED     100

// ***********************************************************************************


// ***********************************************************************************

//light conditions
void sample_light(int);
void init_light(void);
#define SINGLE_SAMPLE_GENERAL_LIGHT a2dConvert10bit(ADC_CH_ADC1)

#define DARK     0
#define AMBIENT 1
#define BRIGHT    2
#define AMBIENT_THRESH     150
#define BRIGHT_THRESH    600
#define LIGHT_SCALE        4
#define LIGHT_WEIGHT    8
#define LIGHT_FWEIGHT    32 // this MUST equal LIGHT_SCALE * LIGHT_WEIGHT
int light_condition; // this is a fuzzy condition that tracks the lighting level
int avg_light;

//****************************************************************

//movement - PIR
#define MOVE_THRESH 5 // 10 samples of movement needed
#define MOVE_ADC_VALUE 650 //what ADC reads for logic 1 off this device
#define SINGLE_SAMPLE_PIR1 a2dConvert10bit(ADC_CH_ADC10)
void sample_movement(int);
void init_movement(void);
int move_detected; // will be true or false
int avg_move; // goes up to MOVE_THRESH and down to 0 , as long as eqult to 10 then move_detected = TRUE
//****************************************************************

//Accelerometer
#define ACCEL_TILT_THRESH .05 // over 5.4% different than ZERO point
//#define ACCEL_TILT_COMPUTED_THRESH 17
#define ACCEL_ZERO  336 // change below to value for autocalibration
int accel_tilt_computed_thresh;
int accel_zero;
int accel_xadjust;
int accel_yadjust;
int accel_zadjust;
//#define ACCEL_XADJUST -5
//#define ACCEL_YADJUST  0
//#define ACCEL_ZADJUST -60 // subtract for gravity
#define ACCEL_SCALE        4
#define ACCEL_WEIGHT    8
#define ACCEL_FWEIGHT    32 // this MUST equal SCALE * WEIGHT

#define SINGLE_SAMPLE_ACCEL_X a2dConvert10bit(ADC_CH_ADC12)
#define SINGLE_SAMPLE_ACCEL_Y a2dConvert10bit(ADC_CH_ADC13)
#define SINGLE_SAMPLE_ACCEL_Z a2dConvert10bit(ADC_CH_ADC14)

void sample_accel(int);
void init_accel(void);
int  accel_detect(int, int);

int accel_detected_x; // will be -1 for negative, 0 for none, 1 for positive
int accel_detected_y; // will be -1 for negative, 0 for none, 1 for positive
int accel_detected_z; // adjusted for gravity
int avg_accel_x; // will be scaled and weighted to smooth response
int avg_accel_y; // will be scaled and weighted to smooth response
int avg_accel_z; // will be scaled and weighted to smooth response

//****************************************************************

// contact switches and bumpers
#define CONTACT_THRESH 10 // samples to count it set to true
void sample_switches(int debug);
void init_switches(void);
// these are all on port C 0 - 7
int switch_detected[7];
int avg_switch[7];
#define USER_INPUT     0
#define BUMPFRONT    1
#define BUMP2        2
#define BUMP7        7

#define MOTORSWITCH 7 // this is on A7
int motorson;

//****************************************************************

//World Mapping non-contact sensors
#define WORLDINCREMENTS 12
// sonar[0] is directly ahead
// sonar[1] is 30 degrees right
// sonar[6] is directly behind ( 180 degrees)
// obstacle[x] = distance in inches to something in that direction -1 = none detected
int sonar[WORLDINCREMENTS];
int infrared[WORLDINCREMENTS];
int obstacle[WORLDINCREMENTS];
void dump_world_map(void);

void update_world_map(int,int);
//state = state + 1 % 6
#define SWEEPCENTERREAD    0
#define SWEEPLEFT         1
#define SWEEPLEFTREAD    2
#define SWEEPRIGHT         3
#define SWEEPRIGHTREAD    4
#define SWEEPCENTER      5
int sweepstate;
void init_world_mapping(void);

#define SWEEPFORWARD    0
#define SWEEPRIGHT        1
#define SWEEPLEFT        11
#define SWEEPTOOCLOSE    10 //meant to be inches right now

// these are related to turret positioning
#define TURRETPOSLEFT     0
#define TURRETPOSCENTER 1
#define TURRETPOSRIGHT     2
#define TURRETPOSITIONS 3
int turret_pos;

// turret mounted light sensor
//#define WORLDLIGHTSAMPLES TURRETPOSITIONS
#define SINGLE_SAMPLE_TURRET_LIGHT a2dConvert10bit(ADC_CH_ADC2);

int light_samples[TURRETPOSITIONS];
void sample_turret_light(int, int);

//IR distance - Sharp Sharp GP2Y0A21YK0F Distance Sensor
#define SINGLE_SAMPLE_TURRET_IR a2dConvert10bit(ADC_CH_ADC2) // front turret IR
int ir_samples[TURRETPOSITIONS];
void sample_turret_ir(int, int);
#define MAXIRRANGE 15

//LV-MaxSonar®-EZ0 High Performance Sonar Range Finder
#define SINGLE_SAMPLE_TURRET_SONAR a2dConvert10bit(ADC_CH_ADC3) // front turret sonar
int sonar_samples[TURRETPOSITIONS];
void sample_turret_sonar(int, int);
#define MAXSONARRANGE 254
// ***********************************************************************************

// worldtime
#define TICSPERSECOND 1000
#define SAMPLEMODULUS 100 // means 10 samples per second
unsigned int time;
unsigned int seconds_elapsed;
unsigned int minutes_elapsed;
unsigned int hours_elapsed;
void init_clock(void);

// ***********************************************************************************

//Move processing and queueing
// this is the set of commands that can be queued up for move processing
#define COMMANDDEPTH     50
#define COMMANDEMPTY     0
#define COMMANDSTOP         0 // this doubles as stop and empty we keep 0's in list
#define COMMANDFORWARD     2
#define COMMANDRIGHT     3
#define COMMANDLEFT     4
#define COMMANDBACKWARD 5
#define COMMANDFRONT    0 // front is always 0 for now


int commandType[COMMANDDEPTH]; // used for type of move command - see above
int commandDuration[COMMANDDEPTH]; // used for duration of move command
int commandEnd; // where we can add to the list
int move_commanded; // track if we are supposed to be moving, used to stop turret sweep
int motor_jumper_on;

int     command_add(int type, int duration);
void     command_clear_all(void);
void     move_processor(int debug);
int     command_pop(void);
int     command_full(void);
int     command_empty(void);
void     init_command(void);
int     command_moving(void);

#define STOPPED (commandType[0] == 0)

// ***********************************************************************************

//reflexes here
void handle_front_bump(int);
int collision_imminent(void);
void emergency_stop(void);
void avoid_front_bump(int);
void compute_safe_turn(int); // compute which turn has less obstruction


// ***********************************************************************************
//behaviors
int behav_timer;
int behavior;
int dancetimer;

//put all reflexive actions here
void check_reflexes(void);
void init_reflexes(void);

void init_behaviors(void);
void behavior_wait(void);
void behavior_sing(void);
void behavior_wander(void);
void behavior_dance(void);
void behavior_lightseek(void);

int decide_to_dance(void); //used for light-induced dancing
//void play_track(char, int);
void decide_random_behavior(int);
void run_behavior(int debug);
void compute_course(void);
void compute_light_seek(int);

#define WAITBEHAVIOR         0
#define SINGBEHAVIOR         1
#define WANDERBEHAVIOR         2
#define DANCEBEHAVIOR         3
#define LIGHTSEEKBEHAVIOR     4
#define NUMBEHAVIORS        6 // any not defined will end up as wait

//seconds to allow behavior to occur
// sounds triggered should also be less than this or they might get cut off
#define WAITBEHAVIOR_RETRIGGER             10
#define SINGBEHAVIOR_RETRIGGER             30
#define WANDERBEHAVIOR_RETRIGGER         30
#define DANCEBEHAVIOR_RETRIGGER         20
#define LIGHTSEEKBEHAVIOR_RETRIGGER     30



// all tracks have duration less than behavior retrigger limit
#define SING_TRACK_NUM        5   // s1.mp3-s"num".mp3
#define WANDER_TRACK_NUM      3    // w1.mp3-w"num".mp3
#define LIGHTSEEK_TRACK_NUM 1    //l1.mp3...
#define DANCE_TRACK_NUM     1    //d1.mp3...
#define SOUND_EFFECT_NUM     21    //e1.mp3...
#define COLLISION_WARNING_NUM 4 //c1.mp3...



// ***********************************************************************************
//uMP3 control
void init_uMP3(void);
int calc_track(int);
int check_for_playing(void);
int wait_for_ready(void);
int consume_to_ready(void);
void play_next_track(char , int , int, int);
void play_track(char, int, int);
int non_preempt_playing;

#define uMP3READY         62 //">" char
#define CARRIAGERETURN     13 // " "
#define COLLISIONTRACKPREAMBLE 'c'
//#define PLAYING           


int uMP3init; // tracks whether initialized successfully
void print_second_line(char *);



// ***********************************************************************************
//enter your code in control here
//photovore is just a default sample program and can be deleted

 
void control(void)
    {

    int t;
    int seconds;
    int boredom;
    int debug;
    int baseline_boredom;
    //int sample;


    // used for command duration timing like moving
    t = seconds = time = boredom = 0;

    debug = TRUE;
    // inits
    //while(!button_pressed())
    while(!bit_is_set(PINC, 5))
        {
        rprintf("Octobot ");
        }
    init_clock();
    init_servos();
    init_sensors();
    init_switches();
    init_command();
    init_world_mapping();
    init_behaviors();
    init_reflexes();
    init_uMP3();
    uart2Init();
    uart2SendByte(12); //clear screen

    delay_ms(1000);
    play_track('z', 1, TRUE);
    delay_ms(2000);

    srand(avg_light);
    baseline_boredom = (rand() % 4) + 1;


    // wait a moment before starting
    delay_ms(1000);
    rprintf(" C in ");

    while(1)
        {
        // check reflexes every time through the loop
        check_reflexes();

        //run normal routiness at 1 Hz
        if(seconds_elapsed != seconds) // if timer ticks over print time
            {
            if (collision_imminent() == TRUE) play_track(COLLISIONTRACKPREAMBLE, (minutes_elapsed % COLLISION_WARNING_NUM) + 1,TRUE);

            if(!(seconds_elapsed % 5))// only update once in a while
                {
                uart2SendByte(128); //position to first char of first line
                rprintfInit(uart2SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf
                delay_ms(10);
                rprintf("H:%d M:%d S:%d  ",hours_elapsed, minutes_elapsed, seconds_elapsed);
                uart2SendByte(148); //position to first char of second line
                delay_ms(10);
                rprintf("Main Loop     ");
                rprintfInit(uart1SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf
                }
            //sample = SINGLE_SAMPLE_TURRET_IR;
            //rprintf("IR Ranging     raw %d cooked %d ", sample, sharp_IR_interpret_GP2Y0A21YK(sample));

            //sample = SINGLE_SAMPLE_TURRET_SONAR;
            //rprintf("Sonic Ranging     raw %d cooked %d ", sample, sonar_MaxSonar(sample)/100);

            //blink LED as heartbeat
            t = (t + 1) % 2;
            if(t == 0) LED_off();
            else        LED_on();

            //adjust and report time
            seconds = seconds_elapsed;
            if((seconds_elapsed % 5) == 0)
                {
                rprintf("Time elapsed seconds %d, minutes %d hours %d ", seconds_elapsed, minutes_elapsed ,hours_elapsed );
                }

            if(seconds_elapsed < 20) // sweep in the first part of a minute
                update_world_map(!move_commanded, FALSE); // sweep if stopped otherwise scan front
            else
                update_world_map(FALSE, FALSE); // don't sweep too often



            if(decide_to_dance()) boredom = 0;


            if(boredom+baseline_boredom > 10)
                {
                if(debug) rprintf("I am so bored ");
                decide_random_behavior(TRUE);
                boredom = 0;
                }

            //run subprocessors
            move_processor(FALSE);
            run_behavior(FALSE);
            boredom += 1;
            } // end of 1hz routines

            //sample_movement(TRUE);
            //sample_switches(TRUE);
            //sample_light(TRUE);

            //for(int i = 0; i < 7; ++i)
            //    {
            //    if (switch_detected[i] == TRUE)
            //        rprintf("Sw %d on ", i);
            //    }

        delay_ms(1); // delay just a very small amount
        }
    } // end main loop

void init_reflexes(void)
    {
    // nothing to do for now
    }

//check front sensor readings
int collision_imminent(void)
    {
    int result = FALSE;
    // ignore collision warnings for the first 5 seconds to stabilize the readings
    if( (seconds_elapsed + minutes_elapsed + hours_elapsed) < 5) return result;
    if(ir_samples[TURRETPOSCENTER] + sonar_samples[TURRETPOSCENTER] < 10) // each agree 5 or less inches of space
        {
        rprintf("Collision Warning ");
        result = TRUE;
        }
    if(ir_samples[TURRETPOSCENTER] < 6)
        {
        rprintf("Collision Warning ");
        result = TRUE;
        }
    if(sonar_samples[TURRETPOSCENTER] < 7)
        {
        rprintf("Collision Warning ");
        result = TRUE;
        }
    return result;
    }

int rear_collision_imminent(void)
    {
    return FALSE;
    }

int calc_track(int limit)
    {
    return( (rand() % limit) + 1);
    }

void emergency_stop(void)
    {
    mot_stop();
    }

void print_second_line(char *comment)
    {
    //return;
    uart2SendByte(148); //position to first char of second line
    rprintfInit(uart2SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf
    delay_ms(10);
    for(int i = 0; i < 16; ++i)
        {
        rprintf(" ");
        delay_ms(5);
        }
    rprintf("%s",comment);
    rprintfInit(uart1SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf
    }

void check_reflexes(void)
    {
    if(switch_detected[BUMPFRONT] == TRUE)
        handle_front_bump(FALSE);

    if (collision_imminent() == TRUE)
        {
        emergency_stop();
        avoid_front_bump(FALSE);
        update_world_map(!move_commanded, TRUE); // sweep if stopped otherwise scan front
        compute_safe_turn(FALSE);
        }

    if(switch_detected[USER_INPUT] == TRUE)
        sample_sensors_report(time, TRUE);

    if(button_pressed()) // if button pressed after initialization
        {
        int count = 1;

        while(button_pressed())
            {
            delay_ms(1);
            count = count + 1;
            }

        if(count > 5)
            {
            dump_world_map();
            play_track('z', 1,TRUE);
            }
        }
    }

void compute_safe_turn(int debug)
    {
    if(ir_samples[TURRETPOSLEFT] + sonar_samples[TURRETPOSLEFT] > 10)
        command_add(COMMANDLEFT,1);
    else
        if(ir_samples[TURRETPOSRIGHT] + sonar_samples[TURRETPOSRIGHT] > 10)
            command_add(COMMANDRIGHT,1);
        else
            command_add(COMMANDBACKWARD,1);
    }


int decide_to_dance(void)
    {
    // check for dancing
    if (light_condition == BRIGHT && dancetimer == 0)
        {
        dance();
        dancetimer = 30;// no dancing for another 30 seconds
        }
    else
        {
        if (dancetimer > 0) --dancetimer;
        return FALSE;
        }
    return TRUE;
}

void init_behaviors(void)
    {
    behavior = WAITBEHAVIOR;
    behav_timer = -1;
    dancetimer=0;
    }


void run_behavior(int debug)
    {
    if ( debug) rprintf("run before behavior = %d, behav_timer = %d ", behavior, behav_timer);

    if ( behav_timer == 0)
        {
        behavior = WAITBEHAVIOR;
        behav_timer = -1;
        }
    else
        if(behav_timer > 0)
            behav_timer -=1;

    if ( debug) rprintf("run after behavior choice = %d ", behavior);

    switch(behavior) {

    case WAITBEHAVIOR:
            behavior_wait();
            break;
    case SINGBEHAVIOR:
            behavior_sing();
            break;
    case WANDERBEHAVIOR:
            behavior_wander();
            break;
    case DANCEBEHAVIOR:
            behavior_dance();
            break;
    case LIGHTSEEKBEHAVIOR:
            behavior_lightseek();
            break;
    default: // can use this to shape towards waiting for undefined numbers
            behavior_wait();
            break;
        }
    if ( debug) rprintf("run after behave = %d, behav_timer = %d ", behavior, behav_timer);
    }

void decide_random_behavior(int debug)
    {
    if(behav_timer > 0) return; // if one still running stick with it while timer

    //srand(seconds_elapsed);
    if ( debug) rprintf("random = %d ",rand());
    behavior = (rand() % NUMBEHAVIORS);
    //behavior = (behavior + 1) % NUMBEHAVIORS; // run through each one for now
   
    behav_timer = -1; // reset it so behavior can set to what it wants
    if (debug) rprintf("decide behavior = %d, behav_timer = %d ", behavior, behav_timer);
    }

// these need to state driven and completely contained
void behavior_wait(void)
    {
    if(behav_timer == -1)
        {
        rprintf("*** Waiting *** ");
        print_second_line("Wait\0");
        //if( (seconds_elapsed %20) == 0)
        play_track('e', calc_track(SOUND_EFFECT_NUM),FALSE);
        behav_timer += WAITBEHAVIOR_RETRIGGER;
        }
    play_next_track('e', 30, 2,SOUND_EFFECT_NUM);
    }

void behavior_sing(void)
    {
    if(behav_timer == -1)
        {
        rprintf("*** Singing *** ");
        print_second_line("Sing");
        behav_timer += SINGBEHAVIOR_RETRIGGER;
        play_track('s', calc_track(SING_TRACK_NUM),FALSE);
        }

    play_next_track('s', 20, 30,SING_TRACK_NUM);
    }

void behavior_dance(void)
    {
    if(behav_timer == -1)
        {
        rprintf("*** Dancing *** ");
        print_second_line("Dance");
        play_track('d', calc_track(DANCE_TRACK_NUM),FALSE);
        behav_timer += DANCEBEHAVIOR_RETRIGGER;
        dance();
        }
    play_next_track('d', 5, 20,DANCE_TRACK_NUM);
    }

void behavior_lightseek(void)
    {
    if(behav_timer == -1)
        {
        play_track('l', calc_track(LIGHTSEEK_TRACK_NUM),FALSE);
        behav_timer += LIGHTSEEKBEHAVIOR_RETRIGGER;
        rprintf("*** LightSeek *** ");
        print_second_line("Light Seek");
        }
    play_next_track('l', 10, 10,LIGHTSEEK_TRACK_NUM);
    compute_light_seek(FALSE);
    }

void behavior_wander(void)
    {
    if(behav_timer == -1)
        {
        rprintf("*** Wandering *** ");
        print_second_line("Wander");
        play_track('w', calc_track(WANDER_TRACK_NUM),FALSE);
        behav_timer += WANDERBEHAVIOR_RETRIGGER;
        command_add(COMMANDSTOP, 6); // get an update of the world
        }
    else
        {
        compute_course();
        play_next_track('w', 10, 15,WANDER_TRACK_NUM);
        }
    }

// look to play next track not too soon and not too late
void play_next_track(char preamble, int timer, int left, int num)
    {
    //rprintf("Play Next Track ");
    if(check_for_playing()) return;
    if((behav_timer % timer) != 0) return;
    if(behav_timer - left < 0) return;
    play_track(preamble, calc_track(num),FALSE);
    }



int wait_for_ready(void)
    {
    int temp;
    while (uart0GetByte() != -1) {delay_ms(1);}; // drain any characters
    for(int k = 0; k < 5; ++k)
        {
        uart0SendByte(CARRIAGERETURN);// send carriage return
        delay_ms(2);
        temp = uart0GetByte();
        if(temp == uMP3READY) return TRUE;

        for(int j = 0; ((j < 20) && (temp == -1)); ++j)
            {
            delay_ms(2);
            temp=uart0GetByte();   
            if(temp == uMP3READY) return TRUE;
            }
        }
    return FALSE;
    }

int consume_to_ready(void)
    {
    int temp;
    temp = uart0GetByte();
    //rprintf("temp = %d ",temp);
    if(temp == uMP3READY) return TRUE;
    for(int j = 0; ((j < 20) && (temp != uMP3READY)); ++j)
        {
        delay_ms(2);
        temp = uart0GetByte();
        //rprintf("temp = %d ",temp);
        }
        //rprintf("temp final = %d ",temp);
    if(temp == uMP3READY) return TRUE;
    return FALSE;
    }

int check_for_playing(void)
    {
    int temp;

    // doesn't seem to work on uMP3
    //if(bit_is_set(PINC,5)) rprintf("Playing Pin is high ");

    for(int j = 0; ((j < 20) && (!wait_for_ready())); ++j)
        {
        rprintf("Not Ready ");
        delay_ms(1);
        }

    rprintfInit(uart0SendByte);// initialize rprintf system and configure uart0 (USB) for rprintf
    rprintf("PC Z ");
    rprintfInit(uart1SendByte);
    temp=uart0GetByte();
    //rprintf("check_for_playing : Temp = %d ", temp);
    for(int j = 0; ((j < 20) && (temp == -1)); ++j)
        {
        delay_ms(2);
        temp=uart0GetByte();   
        //rprintf("check_for_playing : Temp = %d ", temp);
        }
    //if(temp == -1) return FALSE;
    if(temp == 80) //"P"
        {
        //rprintf("Playing something ");
        //rprintf("check_for_playing final: Temp = %d ", temp);
        //while (uart0GetByte() != -1); // drain any other characters
        consume_to_ready();
        return TRUE;
        }
    else
        {
        //rprintf("Not playing something ");
        //rprintf("check_for_playing final: Temp = %d ", temp);
        //while (uart0GetByte() != -1); // drain any other characters
        consume_to_ready();
        }
    return FALSE;
    }


void init_uMP3(void)
    {
    //int temp;
    uMP3init = FALSE;
    non_preempt_playing = FALSE;

    for(int i = 0; i < 32; ++i)
        {
        if(wait_for_ready())
        //if(temp == uMP3READY)
            {
            rprintf("uMP3 ready ");
            //rprintf("Setting Volume ");
            rprintfInit(uart0SendByte);//change UART to uMP3
            rprintf("ST V ");
            uart0SendByte(254);
            rprintf(" ");
            consume_to_ready();
            rprintf("ST H ");
            uart0SendByte(1);
            rprintf(" ");
            rprintfInit(uart1SendByte);
            if(consume_to_ready())
                {
                uMP3init = TRUE;
                rprintf("uMP3 ready! ");
                return;// if we get ready response ">" we are through
                }
            else
                {
                rprintf("failed setting command ");
                }
            }
        else
            {
            rprintf("uMP3 NOT ready ");
            }
        delay_ms(250);
        }
        //rprintfInit(uart1SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf
    }

void play_track(char preamble, int track, int no_preempt)
    {
    // don't preempt a non_preempt track even for another no_preempt
    if ( non_preempt_playing && check_for_playing()) return;

    if( (!non_preempt_playing) && no_preempt)
        non_preempt_playing = TRUE;
    else
        non_preempt_playing = FALSE;

    if(uMP3init == TRUE)
        {
        rprintf("Playing track %c", preamble);
        rprintf("%d.mp3 ",track);
        rprintfInit(uart0SendByte);//change UART to uMP3
        rprintf("PC F /");
        rprintf("%c%d.MP3 ", preamble, track);
        rprintfInit(uart1SendByte);
        }
    else
        rprintf("uMP3 not initalized ");
    }



void compute_course(void)
    {
    if(obstacle[SWEEPFORWARD]>SWEEPTOOCLOSE)
        {
        command_add(COMMANDFORWARD, 3); // go forward one click
        command_add(COMMANDSTOP,6);
        }
    else
        {
        // turn in place a random amount
        if(obstacle[SWEEPLEFT]>SWEEPTOOCLOSE)
            command_add(COMMANDLEFT, (rand()% 3)+ 1);

        else
            command_add(COMMANDRIGHT, (rand()% 3)+ 1);
        command_add(COMMANDSTOP,6); // get a world sample
        }
    }

void handle_front_bump(int debug)
    {
    avoid_front_bump(FALSE);// too late but call same behavior
    }

void avoid_front_bump(int debug)
    {
    // try and reverse 2 seconds
    if (debug) rprintf("Emergency Reverse ");
    command_clear_all();
    command_add(COMMANDBACKWARD, 2);
    move_processor(FALSE); // don't wait act immediately
    }

void init_command(void)
    {
    command_clear_all();
    }

void init_clock(void)
    {
    //initialize clock
    hours_elapsed = minutes_elapsed = seconds_elapsed = 0;
    }

int command_moving(void)
    {
    if( command_empty()) return FALSE;
    return (commandType[commandEnd-1] && TRUE);
    }

int command_pop(void)
    {
    for(int i = 0; i < commandEnd+1; ++i)
        {
        if((i +1) < COMMANDDEPTH)
            {
            commandType[i] = commandType[i+1];
            commandDuration[i] = commandDuration[i+1];
            commandType[i+1] = commandDuration[i+1] = 0;
            }
        }
    if(commandEnd > 0) commandEnd -= 1;
    return TRUE;
    }

int command_full(void)
    {
    if(commandEnd == COMMANDDEPTH ) return TRUE;
    return FALSE;
    }

int command_empty(void)
    {
    if(commandEnd == 0) return TRUE;
    return FALSE;
    }

int command_add(int type, int duration)
    {
    if(command_full())
        return FALSE;

    //rprintf("add %d for %d seconds ",type,duration);

    commandType[commandEnd]     = type;
    commandDuration[commandEnd] = duration;
    commandEnd += 1;
    return TRUE;
    }


void command_clear_all(void)
    {
    for(int i = 0; i < COMMANDDEPTH; ++i)
        commandType[i] = commandDuration[i] = 0;
    commandEnd = 0;
    }

// must be called at same rate the duration ticks represent
// i.e. if commandDuration[i] = 10 and that represents 10 seconds then this must be called
// about 1/second to be accurate
void move_processor(int debug)
    {
    int current;
    if(debug) rprintf("Move Processing ");
       
    if (command_empty())
        {
        if(debug) rprintf("Stop - empty list ");
        mot_stop(); // stop if no commands
        move_commanded = FALSE;
        }
    else
        {
        current = commandEnd - 1;
        if(debug)
            {
            rprintf("CommandEnd = %d commandType[x] = %d, commandDuration[x] = %d ",commandEnd, commandType[COMMANDFRONT], commandDuration[COMMANDFRONT]);
            }
        if ( commandDuration[COMMANDFRONT] == 0) command_pop();

        if(commandType[COMMANDFRONT] != COMMANDSTOP)
            move_commanded = TRUE;
        else
            move_commanded = FALSE;

        if(motor_jumper_on)
            {
            switch(commandType[COMMANDFRONT])
                {
                case COMMANDSTOP:
                        mot_stop();
                        break;
                case COMMANDFORWARD:
                        go_forward();
                        break;
                case COMMANDRIGHT:
                        turn_right();
                        break;
                case COMMANDLEFT:
                        turn_left();
                        break;
                case COMMANDBACKWARD:
                        go_backward();
                        break;
                default:
                        rprintf("Default case in move_commander - oops ");
                        break;
                }
            }
        commandDuration[COMMANDFRONT] -= 1;
        }
    }

void print_sep(void)
    {
    for (int i = 0; i < 20; ++i)
        rprintf("*");
    rprintf(" ");
    }

void dump_world_map(void)
    {
    rprintf("Sonar    | ");
    for(int i = 0; i < TURRETPOSITIONS;++i)
        rprintf("%d |",sonar_samples[i]);
    rprintf(" ");
    rprintf("IR       | ");
    for(int i = 0; i < TURRETPOSITIONS;++i)
        rprintf("%d |",ir_samples[i]);
    rprintf(" ");
    rprintf("Obstacle | ");
    rprintf("%d |",obstacle[WORLDINCREMENTS-1]);
    rprintf("%d |",obstacle[0]);
    rprintf("%d |",obstacle[1]);
    rprintf(" ");


    rprintf("Light | ");
    for(int i = 0; i < TURRETPOSITIONS;++i)
        rprintf("%d |",light_samples[i]);
    rprintf(" ");
    rprintf("General Light condition = %d average = %d ",light_condition, avg_light);
    rprintf("General Light condition = %d average = %d ",light_condition, avg_light);
    rprintf("Switches and bumpers |");
    for(int i = 0; i < 7; ++i)
        {
        if (switch_detected[i])
            rprintf(" on |");
        else
            rprintf(" off|");
        }
    rprintf(" ");
    rprintf("ACCEL | xdet = %d | ydet = %d | zdet = %d | ",accel_detected_x,accel_detected_y,accel_detected_z);
    rprintf("ACCEL | avgx = %d | avgy = %d | avgz = %d | ",avg_accel_x,avg_accel_y,avg_accel_z);

    }

void init_world_mapping(void)
    {
    sweepstate = SWEEPCENTER;
    //assume no initial collisions otherwise robot jumps backward on start
    motor_jumper_on = bit_is_set(PINA,7);

    if(!SERVO_ON)
        {
        rprintf("Motors off in software ");
        return; // servos set to not run in software
        }
    if(!motor_jumper_on)
        {
        rprintf("Motors off in hardware ");
        return; // servos set to not run in hardware (jumper)
        }

    for(int i = 0; i < WORLDINCREMENTS;++i)
        sonar[i] = infrared[i] = obstacle[i] = 999;

    // turret tracking only
    for(int i = 0; i < TURRETPOSITIONS; ++i)
        light_samples[i] = ir_samples[i] = sonar[i] = 999;

    turret_pos = TURRETPOSCENTER;
    PWM_timer3_Set_E5(SERVO_CENTER);
    }


// called 1/second in FSM context
// called many times per second in a moving forward context
void update_world_map(int sweepon, int debug)
    {
    // sensor sweep using a FSM to control/track what we are doing

    // if not sweeping then just scanning forward at a higher rate
    if(sweepon == FALSE )
        {
        if(debug) rprintf("sweepon false ");
        // if not centered, take the hit to wait for it to center
        // later this can use a countdown timer
        if(turret_pos != TURRETPOSCENTER)
            {
            PWM_timer3_Set_E5(SERVO_CENTER);
            delay_ms(500);
            }
        turret_pos = TURRETPOSCENTER;
        sweepstate = 0; // just watch front
        }


    // take readings from turret sensors
    switch (sweepstate) {
    case 0:
        // take reading from front
        sample_turret_light(FALSE, turret_pos);
        sample_turret_ir(FALSE, turret_pos);
        sample_turret_sonar(FALSE, turret_pos);
        break;
    case 1:
        PWM_timer3_Set_E5(SERVO_LEFT);
        turret_pos = TURRETPOSLEFT;
        break;       
    case 2:
        // take reading from left
        sample_turret_light(FALSE, turret_pos);
        sample_turret_ir(FALSE, turret_pos);
        sample_turret_sonar(FALSE, turret_pos);
        break;
    case 3:
        PWM_timer3_Set_E5(SERVO_RIGHT);
        turret_pos = TURRETPOSRIGHT;
        // take readings from side sensors here while large swing occuring
        break;       
    case 4:
        // take reading from right
        sample_turret_light(FALSE, turret_pos);
        sample_turret_ir(FALSE, turret_pos);
        sample_turret_sonar(FALSE, turret_pos);
        break;
    case 5:
        PWM_timer3_Set_E5(SERVO_CENTER);
        turret_pos = TURRETPOSCENTER;
        break;       
    }
    sweepstate = (sweepstate + 1) % 6; // set new state


    if(debug) rprintf("sweepstate = %d ",sweepstate);

    // take readings from side sensors


    // combine all readings into minimum worldstate distances
    //for(int i = 0; i < WORLDINCREMENTS; ++i)
    //    {
    //    obstacle[i] = MIN(sonar[i],infrared[i]);
    //    }
    obstacle[0]                 = MIN(sonar_samples[TURRETPOSCENTER], ir_samples[TURRETPOSCENTER]);
    obstacle[WORLDINCREMENTS-1] = MIN(sonar_samples[TURRETPOSLEFT],   ir_samples[TURRETPOSLEFT]);
    obstacle[1]                 = MIN(sonar_samples[TURRETPOSRIGHT],  ir_samples[TURRETPOSRIGHT]);

    }


void init_servos(void)
    {
    PWM_Init_timer3_E3(10);
    PWM_Init_timer3_E4(10);
    PWM_Init_timer3_E5(10);
    delay_ms(50);

    PWM_timer3_On_E3();
    PWM_timer3_On_E4();
    PWM_timer3_On_E5();
    delay_ms(50);

    PWM_timer3_Set_E3(SERVO_CENTER);
    PWM_timer3_Set_E4(SERVO_CENTER);
    PWM_timer3_Set_E5(SERVO_CENTER);
    delay_ms(50);
    }

void init_switches(void)
    {
    for(int i = 0; i < 8; ++i)
        {
        switch_detected[i] = 0;
        avg_switch[i]=0;
        }
    }

void init_sensors(void)
    {
    // lights
    init_light();
    init_accel();
    init_movement();

    //set up interrupt to start watching sensors
    timerAttach(TIMER2OVERFLOW_INT, &sample_sensors );
    }

void sample_sensors(void) // this exists to call in the timers, no input allowed
    {
    // track time
    time = (time + 1) % TICSPERSECOND; // get next tic and scale to 1 second increments
    if (time == 0)
        seconds_elapsed = seconds_elapsed + 1;
    if (seconds_elapsed == 60)
        {
        minutes_elapsed = minutes_elapsed + 1;
        seconds_elapsed = 0;
        }
    if (minutes_elapsed == 60)
        {
        hours_elapsed = hours_elapsed + 1;
        minutes_elapsed = 0;
        }
    sample_sensors_report(time, FALSE);
    }

void sample_sensors_report(int time, int debug)
    {
    int slot;

    // sample switches every time, should be really fast anyway
    // switches are debounced so need to read them more often otherwise slow response time
    if (!debug) // sample digital inputs often
        {
        sample_switches(debug);
        sample_movement(debug);
        }

    slot = time % SAMPLEMODULUS; // divide into SAMPLEMODULUS samples per second for non-digital inputs
    //assume time in range of 0 -999
    // sample each set of sensors in a different period to remain fast
    if(slot == 0 && !debug)
        sample_light(debug);

    if(slot == 1 && !debug)
        sample_accel(debug);


    // switches are debounced so need to read them more often otherwise slow response time
    //if((slot > 3  and slot 10 < ) && !debug)
    //    sample_switches(debug);

    if(debug) // sample them all at once for debug not called in realtime
        {
        sample_light(debug);
        sample_accel(debug);
        sample_movement(debug);
        sample_switches(debug);
        }

    }

void init_light(void)
    {
    light_condition = 0;
    avg_light        = 0;
    // attach to timer
    }

void init_movement(void)
    {
    // need warm up time
    //for (int hh = 0; hh < 5; ++hh)
    //    delay_ms(5000);

    move_detected = FALSE; // will be true or false
    avg_move = 0; // will be scaled and weighted to smooth response
    }


// set up globals and autocalibrate
// hopefully the sensor is level or this will be off
void init_accel(void) // not time sensitive
    {
    accel_detected_x =0; // will be -1 for one direction, 0 for none, +1 for other direction
    accel_detected_y =0; // will be true or false
    accel_detected_z =0; // adjusted for gravity
    avg_accel_x=ACCEL_ZERO; // will be scaled and weighted to smooth response
    avg_accel_y=ACCEL_ZERO; // will be scaled and weighted to smooth response
    avg_accel_z=ACCEL_ZERO; // will be scaled and weighted to smooth response

    for ( int i = 0; i < 50; ++i)
        {
        // take 50 samples average and set zero point based upon that
        sample_accel(FALSE);
        }
    sample_accel(TRUE);

    // if we are tilted when we autocalibrate this will be wrong!
    // also this weights to the average of X and Y so hopefully close together
    accel_zero = ((avg_accel_x + avg_accel_y) / 2)/ACCEL_FWEIGHT;
    accel_xadjust = accel_zero - (avg_accel_x/ACCEL_FWEIGHT);
    accel_yadjust = accel_zero - (avg_accel_y/ACCEL_FWEIGHT);
    accel_zadjust = accel_zero - (avg_accel_z/ACCEL_FWEIGHT); // this should factor out gravity
    accel_tilt_computed_thresh = accel_zero * ACCEL_TILT_THRESH;

    rprintf("accel_zero = %d accel_tilt_computed_thresh = %d ", accel_zero,accel_tilt_computed_thresh);
    rprintf("accel_xadjust = %d accel_yadjust = %d accel_zadjust = %d ", accel_xadjust,accel_yadjust,accel_zadjust);
    }

int sensorscaleweight(int * sens_avg, int weight, int scale, int fweight, int adjust, int sens, int debug)
    {
    int avg;
    avg = (*sens_avg); // get glocal variable


    if ( debug == TRUE)
        {
        rprintf("int sens_avg = %d, int weight = %d, int scale = %d ", avg, weight, scale);
        rprintf("int fweight = %d, int adjust = %d, int sens =%d ",fweight, adjust, sens);
        }

    // adaptive algorithm from :
    // http://www.hempeldesigngroup.com/lego/pbLua/tutorial/pbLuaGyro.html
    // scale the sample and weight the average
    avg = avg - (avg / weight) + ((sens + adjust) * scale );

    if ( debug == TRUE)
        {
        rprintf("sens_avg = %d (sens_avg / weight) = %d ", avg, (avg / weight) );
        rprintf("((sens + adjust) * scale ) %d ", ((sens + adjust) * scale ));
        }
           
    (*sens_avg) = avg; //set global

    return avg / fweight; // descale and deweight
    }



//used to sample the turret light sensor
void sample_turret_light(int debug, int turret_pos)
    {
    int avg_light,light;
    avg_light = SINGLE_SAMPLE_TURRET_LIGHT;
    for ( int i = 0; i < 4; ++i)
        {
        light = SINGLE_SAMPLE_TURRET_LIGHT;
        light = sensorscaleweight(&avg_light, 1, 1, 1, 0,light, FALSE);
        delay_ms(20);
        }
    light_samples[turret_pos] = light;
    }

//used to sample the turret IR
void sample_turret_ir(int debug, int turret_pos)
    {
    int avg_dist,dist;
    avg_dist = sharp_IR_interpret_GP2Y0A21YK(SINGLE_SAMPLE_TURRET_IR);
    //rprintf("Ranging %d ", sharp_IR_interpret_GP2Y0A21YK(SINGLE_SAMPLE_TURRET_IR));

    for ( int i = 0; i < 4; ++i)
        {
        dist = sharp_IR_interpret_GP2Y0A21YK(SINGLE_SAMPLE_TURRET_IR);
        dist = sensorscaleweight(&avg_dist, 1, 1, 1, 0,dist, FALSE);
        delay_ms(20);
        }
    if(dist > MAXIRRANGE) dist = 9999; // only good out to this range then not trustworthy
    ir_samples[turret_pos] = dist;
    }

//used to sample the turret SONAR
void sample_turret_sonar(int debug, int turret_pos)
    {
    int avg_dist,dist;
    avg_dist = sonar_MaxSonar(SINGLE_SAMPLE_TURRET_SONAR)/100;
    //rprintf("Ranging %d ", sharp_IR_interpret_GP2Y0A21YK(SINGLE_SAMPLE_TURRET_IR));

    for ( int i = 0; i < 4; ++i)
        {
        dist = sonar_MaxSonar(SINGLE_SAMPLE_TURRET_SONAR)/100;
        dist = sensorscaleweight(&avg_dist, 1, 1, 1, 0,dist, FALSE);
        delay_ms(20);
        }
    if(dist > MAXSONARRANGE) dist = 9999; // only good out to this range then not trustworthy
    sonar_samples[turret_pos] = dist;
    }


void compute_light_seek(int debug)
    {
    int current = TURRETPOSITIONS/2; // start at center

    for ( int i = 0; i < TURRETPOSITIONS; ++i)
        if(light_samples[i] > light_samples[current]) current = i; // find max

    // now compute course based upon where is brightest
    // assume light_samples[WORLDLIGHTSAMPLES/2] is dead ahead
    // i.e. WORLDLIGHTSAMPLES = 5 so 5/2 = 2 so light_samples[2] would be ahead

    current = current - (TURRETPOSITIONS/2); //scale to center

    // assume we are called once a second so for now don't compute more than 1 second ahead
    // have to stop and take measurements so often
    if(current < 0) // go left
        {
        command_add(COMMANDLEFT, 1); // go left one click
        }
    else
        {
        if(current > 0) //go right
            {
            command_add(COMMANDRIGHT, 1); // go left one click
            }
        else
            {
            command_add(COMMANDFORWARD, 2); // go forward one click
            command_add(COMMANDSTOP, 6); // add in a sensor sweep
            }
        }
   
    }

//This is the general (non-turret) light sampler that is facing up
void sample_light(int debug)
    {
    unsigned int light;

    if(debug == TRUE) rprintf("General Light sensor ");

    //light = a2dConvert10bit(ADC_CH_ADC1);
    light = SINGLE_SAMPLE_GENERAL_LIGHT;
    light = sensorscaleweight(&avg_light, LIGHT_WEIGHT, LIGHT_SCALE, LIGHT_FWEIGHT, 0,light, debug);

    if ( light < AMBIENT_THRESH )
        light_condition = DARK;
    else
        {
        if ( light > BRIGHT_THRESH )
            light_condition = BRIGHT;
        else
            light_condition = AMBIENT;
        }

    if(debug == TRUE)
        {
        rprintf("la = %d l = %d lc = %d ",avg_light/LIGHT_FWEIGHT, light,light_condition);
        }
    if(debug == TRUE) print_sep();
    }

//Accelerometer
void sample_accel(int debug)
    {
    int accelx, accely, accelz;

    if(debug == TRUE) rprintf("Accel Sensor 1 ");

    //accelx = a2dConvert10bit(ADC_CH_ADC12);
    //accely = a2dConvert10bit(ADC_CH_ADC13);
    //accelz = a2dConvert10bit(ADC_CH_ADC14);

    accelx = SINGLE_SAMPLE_ACCEL_X;
    accely = SINGLE_SAMPLE_ACCEL_Y;
    accelz = SINGLE_SAMPLE_ACCEL_Z;

    if(debug == TRUE)
        {
        rprintf("Before scale/weight x = %d y = %d z = %d ",accelx,accely,accelz);
        }

    accelx = sensorscaleweight(&avg_accel_x, ACCEL_WEIGHT, ACCEL_SCALE, ACCEL_FWEIGHT,accel_xadjust,accelx, debug);
    accely = sensorscaleweight(&avg_accel_y, ACCEL_WEIGHT, ACCEL_SCALE, ACCEL_FWEIGHT,accel_yadjust,accely, debug);
    accelz = sensorscaleweight(&avg_accel_z, ACCEL_WEIGHT, ACCEL_SCALE, ACCEL_FWEIGHT,accel_zadjust,accelz, debug);

    if(debug == TRUE)
        {
        rprintf("After scale/weight x = %d y = %d z = %d ",accelx,accely,accelz);
        }
   
    accel_detected_x = accel_detect(accelx, accel_tilt_computed_thresh);
    accel_detected_y = accel_detect(accely, accel_tilt_computed_thresh);
    accel_detected_z = accel_detect(accelz, accel_tilt_computed_thresh);

    if(debug == TRUE)
        {
        rprintf("xdet = %d ydet = %d zdet = %d ",accel_detected_x,accel_detected_y,accel_detected_z);
        }
    if(debug == TRUE) print_sep();
    }

int accel_detect(int accel, int thresh)
    {
    if ( accel > (accel_zero + (thresh)))
        {
        return 1;
        }
    else
        {
    if ( accel < (accel_zero - (thresh)))
            {
            return -1;
            }
        else
            {
            return 0;
            }
        }
    }

// seperate routine from other digital inputs since we have the PIR on the ADC port
void sample_movement(int debug)
    {
    unsigned int move;

    if(debug == TRUE) rprintf("PIR ");

    if(seconds_elapsed > 20 || minutes_elapsed > 1) // wait at least 30 seconds before trusting PIR
        move = SINGLE_SAMPLE_PIR1;
    else
        move = 0;
   
    if( move > MOVE_ADC_VALUE)
        {
        avg_move = MIN(avg_move + 1,MOVE_THRESH);
        }
    else
        {
        avg_move = MAX(avg_move - 1,0);
        }

    if (avg_move == MOVE_THRESH) // 10 samples in a row
        move_detected = TRUE;
    else
        move_detected = FALSE;

    if(debug == TRUE)
        {
        rprintf("avg_move = %d move = %d move_detected = %d ", avg_move, move, move_detected);
        }

    if(debug == TRUE) print_sep();

    }

void sample_switches(int debug)
    {
    if(debug == TRUE) rprintf("sample switches ");

    for(int i = 0; i < 8; ++i)
        {
        //if(debug == TRUE) rprintf("sample before call %d ", bit_is_set(PINC,i));
        switch_detected[i] = sample_digital(PINC, i, &(avg_switch[i]), CONTACT_THRESH, debug);
        if(debug == TRUE) rprintf("switch detected %d ", switch_detected[i]);
        }
    if(debug == TRUE) print_sep();
    }

//returns true or false based upon a number of readings over time
int sample_digital(char port, int pin, int *avg, int thresh, int debug)
    {
    unsigned int sample;

    //if(debug == TRUE) rprintf("sample digital port = %x pin = %d ", port, pin);

    sample = bit_is_set(port,pin);
    //if(debug == TRUE) rprintf("immediate sample = %d ",sample);
   
    if( sample )
        {
        *avg = MIN((*avg) + 1,thresh);
        }
    else
        {
        *avg = MAX((*avg) - 1,0);
        }

    if ((*avg) == thresh) // thresh samples in a row to debounce
        sample = TRUE;
    else
        sample = FALSE;

    //if(debug == TRUE)
    //    rprintf("avg = %d thresh = %d detected = %d ", *avg, thresh, sample);


    //if(debug == TRUE) print_sep();
    return (sample);
    }

//old version
void sample_accelerometer(void)
    {
    unsigned int z;
    unsigned int y;
    unsigned int x;

    z = accelerometer_SEN00741(a2dConvert10bit(ADC_CH_ADC2));
    y = accelerometer_SEN00741(a2dConvert10bit(ADC_CH_ADC3));
    x = accelerometer_SEN00741(a2dConvert10bit(ADC_CH_ADC4));

    rprintf("ax = %d ay = %d az = %d ",x,y,z);
    }

// basic queued movement routine
void dance(void)
    {
    if(command_moving())
        {
        command_clear_all();
        command_add(COMMANDSTOP,     1);
        }
    else
        command_clear_all();
               
    command_add(COMMANDLEFT,     2);
    command_add(COMMANDSTOP,     1);
    command_add(COMMANDRIGHT,     4);
    command_add(COMMANDSTOP,     1);
    command_add(COMMANDLEFT,     2);
    command_add(COMMANDSTOP,     1);
    command_add(COMMANDBACKWARD,2);
    command_add(COMMANDSTOP,     1);
    command_add(COMMANDFORWARD, 2);
    command_add(COMMANDSTOP,     1);
    }

void mot_stop(void)
    {
    PWM_timer3_Set_E3(SERVO_CENTER);
    PWM_timer3_Set_E4(SERVO_CENTER);
    move_commanded = FALSE;
    }


void go_forward(void)
    {
    PWM_timer3_Set_E3(SERVO_CENTER-SERVO_FULL_SPEED);
    PWM_timer3_Set_E4(SERVO_CENTER+SERVO_FULL_SPEED);
    move_commanded = TRUE;
    }


void go_backward(void)
    {
    PWM_timer3_Set_E3(SERVO_CENTER+SERVO_FULL_SPEED);
    PWM_timer3_Set_E4(SERVO_CENTER-SERVO_FULL_SPEED);
    move_commanded = TRUE;
    }

void turn_left(void)
    {
    PWM_timer3_Set_E3(SERVO_CENTER-SERVO_FULL_SPEED);
    PWM_timer3_Set_E4(SERVO_CENTER-SERVO_FULL_SPEED);
    move_commanded = TRUE;
    }

void turn_right(void)
    {
    PWM_timer3_Set_E3(SERVO_CENTER+SERVO_FULL_SPEED);
    PWM_timer3_Set_E4(SERVO_CENTER+SERVO_FULL_SPEED);
    move_commanded = TRUE;
    }