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;
    }