The times they are a-changin’.
This post seems to be older than 15 years—a long time on the internet. It might be outdated.
Over the 2003 summer, I had the pleasure of completing a two-week course on robotics at the UW. During those two weeks, I developed the following code:
// This program by Andrew Ferguson
// Copyright 2003 Andrew Ferguson
// The list below tells where the stuff should be plugged in
//Begin
int lt_bump = 7; //left bump sensor
int rt_bump = 8; //right bump sensor
int left_motor = 0; //left motor
int mill_motor = 1; //mill motor
int right_motor = 3; //right motor
int rear_bump = 9; //rear bump sensor
int alpha_pol = 0; //1st polarized light sensor
int beta_pol = 1; //2nd polarized light sensor, rotated 90 degrees
int lt_ir = 4; // first IR light sensor/emiter
int rt_ir = 3;// second IR light sensor/emiter
int break_beam_a = 15; //break beam sensor for detecting the balls
int break_beam_b = 14; //break beam sensor for detecting the balls
int rotation_left = 6; //left rotation sensor
int rotation_right = 5
; //right rotation sensor
//End
//The list below declares global variables, the varible does NOT need to be global, then do NOT put it here
//Begin
int foo_right = 0; // how many times bot has turned right
int foo_left = 0; // how many times bot has turned left
int last_turn = 0; // Which way the bot turned last; 0 = Left and 1 = Right
int lt_speed = 25; //how fast the left side is going
int rt_speed = 25; //how fast the right side is going
int ball_in_hopper = 1; //whether the a ball has been collected - 0 for no, 1 for yes
int pol_home = 1; //Which polorized light the sensor should look for/care about; possible values are 1 and -1
int sensor_value; //polarized sensor delta value
float system_time = seconds(); //time since epoch of system restart
float master_time = 0.; //This is the master start time, which is used to determine when it should start looking for home
int rt_cnt; //How many times the right rotation sensor has been hit
int lt_cnt; //How many times the left rotation sensor has been hit
int line_found; //This determines if the bot has detected the line for a given set of balls trying to be dropped...please no jokes
int in_motion = 1; //If the bot is in motion. This is normal set to 1 (i.e. Yes) until the game is over (180 seconds after boot), in which case the value is changed to 0 (no)
//End
void count_both (void) {
int lt_previous_hole; //if the previous check on the left side was hole or not
int rt_previous_hole; //if the previous check on the right side was a hole or not
while(1) {
if(lt_previous_hole == 0){ //the previous check was not a hole...
if (analog(rotation_left) < 100){//... if the there is a hole now, then count that
lt_cnt++; //counting the hole
lt_previous_hole = 1; //setting the check to say there was a hole this time around
}
}
else{
if (analog(rotation_left) >= 100){ //if the previous check was a hole, then check to see if it's not a hole
lt_cnt++;// counting the hole, or lack thereof
lt_previous_hole = 0; //setting the check to say there was not a hole this time
}
}
if(rt_previous_hole == 0){//the previous check was not a hole...
if (analog(rotation_right) < 100){//... if the there is a hole now, then count that
rt_cnt++;//counting the hole
rt_previous_hole = 1;//setting the check to say there was a hole this time around
}
}
else{
if (analog(rotation_right) >= 100){//if the previous check was a hole, then check to see if it's not a hole
rt_cnt++; //counting the hole, or lack thereof
rt_previous_hole = 0; //setting the check to say there was not a hole this time
}
}
defer();
}
}
void stuck_master (void){
int i;
while(in_motion){
lt_cnt = 0; //reset the left count to zero
rt_cnt = 0; //reset the right count to zero
sleep(2.0); //wait 2 seconds...now we have fresh count
if ( (lt_cnt < 20) || (rt_cnt < 20) ) //if either the left or the right count is less then 10, then we just might be stuck
{
beep();
stop();
while ( (lt_cnt < 50) && (rt_cnt < 50) ){ //reverse for 20 counts
lt_speed = -100;
rt_speed = -100;
defer();
}
rt_speed = 25;
lt_speed = 25;
}
defer();
}
}
void go_home (void) //when time is up, try and find home!
{
while(1) {
while (seconds() <= 120.) { //if there are at least 2 minutes (120 seconds) left, we can go about our business
defer();
//beep();
}
while (seconds() <= 150. && (ball_in_hopper == 1) ) { //when there is a minute left, keep looking for balls
defer();
//beep();
}
while (seconds() <= 150. && (ball_in_hopper == 0) ) { //when there is a minute left, start looking for the hole to drop of whatever balls we have...assuming we have balls
ir_sense();
//in_motion = 0;
defer();
//beep();
}
while (seconds() <180.) { // when there are a 30 seconds left, start looking for the polarized light...note: there is no defer() or sleep(x.x) becuase this is a priority 1 process
printf("Phoning Home\n");
//in_motion = 0;
beep();
pol_sensor();
defer();
}
while (seconds() > 180.) { //when the game is over, shut down all motors and beep...how nice
//printf("Shutting down\n");
//beep();
in_motion = 0;
beeper_on();
ao();
defer();
}
defer();
}
}
/*void ball (void){ //This checks to see if a ball is in the hopper or not
while(1) {
if (_raw_analog(break_beam) < 10)
{
ball_in_hopper = 0; // ball is NOT in hopper
}
else if (_raw_analog(break_beam) > 10) //ball IS in hopper
{
ball_in_hopper = 1;
sleep(5.0);
}
sleep(.01);
}
}
*/
void pol_sensor (void) // detects the polarization of the light source
{
sensor_value = (_raw_analog(beta_pol) - _raw_analog(alpha_pol) ); // sets the delta value for the polarization
lt_speed = 100; //full speed, left side
rt_speed = 100; //full speed, right side
if ( (sensor_value < -60) && (pol_home == 1 ) ){ //if the sensor value is less than 60 and the light is NOT home, turn around
rt_speed = 100;
lt_speed = -100;//lt_speed - 15;
}
if ( (sensor_value > -30) && (pol_home == -1) ){//if the sensor value is greater than -30 and the light is NOT home, turn around
lt_speed = 100;
rt_speed = -100;//rt_speed - 15;
}
sleep(.2);
printf("%d\n", sensor_value);
}
void ir_sense(void) { //this tries to find the black line, which hopefully will lead to the hole. If not, the robot will turn around and go the other way, which HAS to lead to the black hole
beep();
printf("%d\n", line_found);
while ( (analog(lt_ir) > 100) && (analog(rt_ir) > 100) ){ //if there are black lines under both the IR sensors :: this would happen if the robot approaced the line at a 90 degree angle
lt_speed = 20;
rt_speed = 20;
//sleep(2.0);
system_time = seconds();
defer();
}
while( (analog(lt_ir) < 100) && (analog(rt_ir) < 100) && (line_found == 0) ){ //If there are no black lines and the line has NOT been found before
lt_speed = 15;
rt_speed = 25;
defer();
}
while ( (analog(rt_ir) > 100) && (line_found == 0) ){ //if there is a black line under the right IR sensor and the line has not been found yet...
rt_speed = 20;
lt_speed = 20;//lt_speed - 15;
system_time = seconds();
line_found = 1;
while(analog(lt_ir) < 100){ //...wait until the left IR sensor passes the line...
defer();
}
while(analog(lt_ir) > 100){ //...then when it does hit the line, turn the other way
rt_speed = -20;
lt_speed = 20;
sleep(1.5);
}
defer();
}
while ( (analog(lt_ir) > 100) && (line_found == 0) ){ //if there is a black line under the left IR sensor and the line has not been found yet...
lt_speed = 20;
rt_speed = 20;//rt_speed - 15;
system_time = seconds();
line_found = 1;
while(analog(rt_ir) < 100){//...wait until the right IR sensor passes the line...
defer();
}
while(analog(rt_ir) > 100){//...then when it does hit the line, turn the other way
lt_speed = -20;
rt_speed = 20;
sleep(1.5);
}
defer();
}
while ( (analog(rt_ir) > 100) && (line_found == 1) && (analog(lt_ir) < 100) ){ //if the right IR sensor sees the black line, but the left one doesn't and the line has been found before, turn a bit the other way to get back on track
rt_speed = 30;
lt_speed = -10;//lt_speed - 15;
system_time = seconds();
defer();
}
while ( (analog(lt_ir) > 100) && (line_found == 1) && (analog(rt_ir) < 100) ){//if the left IR sensor sees the black line, but the right one doesn't and the line has been found before, turn a bit the other way to get back on track
lt_speed = 30;
rt_speed = -10;//rt_speed - 15;
system_time = seconds();
defer();
}
while( (analog(lt_ir) < 100) && (analog(rt_ir) < 100) && (line_found == 1) ) { // if both IR sensors don't see the black line :: this would happen if: A) The robot was on track or B) if the robot reached the end of the black line
if( (seconds() - system_time) > 1.0) { // If more than 1 second has passed since the last course correction, start going in circles to try and reaquire the line
lt_speed = -20;
rt_speed = 20;
while(analog(rt_ir)< 100) //while the right IR sensor doesn't seen the black line...
{
if( (seconds() - system_time) > 6.0) {//...and six seconds have passed, stop looking for the line...it's probably gone...
system_time = seconds();
break;
}
if (analog(lt_ir)> 100){//...or if the left line is seen, turn the other way :: the bot is really on track...just going straight
lt_speed = 20;
rt_speed = -20;
system_time = seconds();
while(analog(lt_ir)> 100){ //...wait while the left IR sensor is still on the black line...
defer();
}
while(analog(lt_ir)< 100) { //...when the left IR sensor gets off the back line...
if( (seconds() - system_time) > 4.) { //...wait until four seconds have passed and then break out
break;
}
defer();
}
break;
}
//system_time = seconds();
break;
defer();
}
}
defer();
}
}
void turn_left(void){ //turn the robot left
//printf("Turning left:%d\n", foo_left);
fd(left_motor);
bk(right_motor);
sleep(0.3);
ao ();
fd(mill_motor);
}
void turn_right(void){ //turn the robot right
//printf("Turning right:%d\n", foo_right);
bk(left_motor);
fd(right_motor);
sleep(0.3);
ao ();
fd(mill_motor);
}
void go_forward(void){ //make the robot go forward
//printf("Going forward\n");
motor(left_motor, lt_speed);
motor(right_motor, rt_speed);
}
void go_backward(void){ //make the robot go backwards
//printf("Going backwards\n");
bk(left_motor);
bk(right_motor);
sleep (.5);
ao ();
fd(mill_motor);
}
void stop (void){ //make the robot stop
//printf("Stopped\n");
ao ();
fd(mill_motor);
}
void main_movement (void) //main set of robot commands, pretty much just make sure it doesn't hit and thing and what to do if it does
{
while (in_motion)
{
go_forward();
//stuck_master(); //the ultimate in stuckness prevention!!
/*while ( (digital(lt_bump) == 0) & (digital(rt_bump) == 0) )
{
//This section is commented out becuase it will be really hard for the robot get stuck on anything, esp. with the new physical collision avoidance system
if( (seconds() - system_time) > 10.0)
{
stop();
printf("10 second are up. I'm going backwards");
go_backward();
printf("Turning %d\n", last_turn);
if (last_turn == 1)
{
turn_right();
last_turn = 0;
system_time = seconds();
}
else{
turn_left();
last_turn = 1;
system_time = seconds();
}
go_forward();
system_time = seconds();
}
defer();
}
*/
if ( (digital(lt_bump) == 1) ) //left turn
{
foo_left ++;
stop();
go_backward();
if (foo_left%3 == 0)
{
turn_right();
foo_right = 0;
foo_left = 0;
}
else{
turn_left();
}
last_turn = 0;
system_time = seconds();
}
if ( (digital(rt_bump) == 1) ) //right turn
{
foo_right ++;
stop();
go_backward();
if (foo_right%3 == 0)
{
turn_left();
foo_right = 0;
foo_left = 0;
}
else {
turn_right();
last_turn = 1;
system_time = seconds();
}
}
defer();
}
}
void main() //Begin the main program
{
printf("JCN v20.0 by Andrew and Mariah\n");
//sleep(1.0);
beep ();
fd(mill_motor); //start the mill motor thingy
start_process(main_movement() ); //start the main set of movements, including collision avoidance
start_process(go_home() ); //start the master clock, and prepare to go home
start_process(count_both() );
start_process(stuck_master() );
while (1)
{
line_found = 0;
while ( (in_motion) && ( (digital(break_beam_a) == 0) || (digital(break_beam_b) == 0) )){
//printf("%d - %d\n", digital(14), digital(15) );
ir_sense(); //starts the black-line finding sub-routine
sleep(.01);
}
printf("%d - %d\n", digital(14), digital(15) );
//printf("Ball NOT in hopper\n");
sleep(.01);
}
}
I really like this part:
//stuck_master(); //the ultimate in stuckness prevention!!
Note: This was code that I originally had in a ‘Code’ page, but I’m moving it to a post now. This code is probably really old (years and years old). I enjoy laughing at myself and so should you.
0