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