<![CDATA[
/*MOTORS*/
#define MOTOR_1 0 /*left*/
#define MOTOR_2 1 /*right */
#define MOTOR_3 4 /*chomper*/
#define MOTOR_4 2
#define MOTOR_5 3
#define MOTOR_6 5
/*SENSORS*/
#define LEFT_DIST_PORT 4 /*left*/
#define RIGHT_DIST_PORT 2 /*right*/
#define LEFT_ENCODER 7
#define RIGHT_ENCODER 8
#define TOP_LEFT_SWITCH 22
#define TOP_RIGHT_SWITCH 23
#define BOTTOM_RIGHT_SWITCH 24
#define BOTTOM_LEFT_SWITCH 25
#define START_PORT 5
#define TOP_LEFT_LIGHT 16
#define TOP_RIGHT_LIGHT 17
#define BOTTOM_LEFT_LIGHT 18
#define BOTTOM_RIGHT_LIGHT 20
#define LEFT_LINE 21
#define RIGHT_LINE 22
#define IR_1 12
#define IR_2 13
#define IR_3 14
#define IR_4 15
#define BALL_SWITCH 30
/*sensors*/
#define ENCODER_THRESHOLD 5
#define LIGHT_THRESHOLD 1
#define DIST_SAMPLES 5
int LEFT_WALL_DIST = 95;
int FAR_LEFT_WALL_DIST= 55;
int RIGHT_WALL_DIST= 95;
int FAR_RIGHT_WALL_DIST = 55;
#define DIST_THRESHOLD 5
#define FAR_DIST_THRESHOLD 2
#define FORWARD_THRESHOLD 10
#define VEER_TIME .1
#define DEFAULT_VEER_POWER 66
#define MOTOR_CORRECTION 50
#define LINE_THRESHOLD 3
#define TOP_LEFT_LIGHT_ADJUST 5
#define LIGHT_SAMPLES 25
#define SHAFT_THRESHOLD 2
/*turning*/
#define TICKS_90 65
#define WALL_FOLLOW 1
#define NO_WALL_FOLLOW 0
#define LFORWARD 1
#define RFORWARD 2
#define LEFT 3
#define RIGHT 4
#define BACK 5
#define BACKUP_TICKS 75
/***GLOBALS***********************/
int color=-1; /*1 for blue 0 for white*/
int topleft=-1;
int topright=-1;
int leftline=-1;
int rightline=-1;
int rflag=0;
int lflag=0;
int ttick=0;
int balls=0;
int moving=0;
int rtickstogo=0;
int ltickstogo=0;
int wall_follow_dist=0;
int cflag=0;
/***FUNCTIONS*********************/
void setup() {
ao();
balls=0;
enable_encoder(LEFT_ENCODER);
enable_encoder(RIGHT_ENCODER);
}
void activate_ball_switch()
{
while(balls<4)
{
if (!digital(BALL_SWITCH))
{
++balls;
while(digital(BALL_SWITCH));
}
}
stop_chomp();
}
void stop_chomp()
{
drive(MOTOR_6, 0);
drive(MOTOR_3, 0);
drive(MOTOR_4, 0);
drive(MOTOR_5, 0);
}
void orient()
{
int i;
while(1)
{
if (digital(IR_2))
{
left(90);
beep();
break;
}
else if (digital(IR_3))
{
left(180);
beep();
break;
}
else if (digital(IR_4))
{
right(90);
beep();
break;
}
else if (digital(IR_1))
{
break;
}
}
}
/**********************************************************************
* Drives both motors at the indicated speed. Note that the handyboard
* only has 7 speeds, so 0, 16, 33, 50, 66, 83, and 100 are really
* the only speeds that mean much.
**********************************************************************/
void drive(int port, int speed) /*drive is an alias for motor*/
{
motor(port, speed);
}
void drive_ticks(int motor_port, int encoder_port, int ticks, int power)
{
int currticks=0;
reset_encoder(encoder_port);
drive(motor_port, power);
while (1)
{
currticks=read_encoder(encoder_port);
if (currticks>ticks)
{
drive(motor_port, 0);
++ttick;
break;
}
sleep(.01);
}
}
/**********************************************************************
* Drive forward at full speed for the number of ticks indicated by
* dist.
*********************************************************************/
void chomp()
{
drive(MOTOR_6, 100);
drive(MOTOR_3, 100);
drive(MOTOR_4, 100);
drive(MOTOR_5, 100);
}
void forward(int totaldist, int wall_follow_flag, int port, int ideal) {
int i, avg, j;
int CPID=-1;
int thresh=0;
if ((ideal==FAR_LEFT_WALL_DIST)||(ideal==FAR_RIGHT_WALL_DIST))
{
thresh=FAR_DIST_THRESHOLD;
}
else
{
thresh=DIST_THRESHOLD;
}
wall_follow_dist=ideal;
if (port==LEFT_DIST_PORT)
{
moving=LFORWARD;
}
else
{
moving=RFORWARD;
}
reset_encoder(LEFT_ENCODER);
reset_encoder(RIGHT_ENCODER);
fd(MOTOR_1);
fd(MOTOR_2);
if (!wall_follow_flag)
{
while(1)
{
if ((read_encoder(RIGHT_ENCODER)>=totaldist) && (read_encoder(LEFT_ENCODER)>=totaldist))
{
stop();
break;
}
else if ((read_encoder(RIGHT_ENCODER) - read_encoder(LEFT_ENCODER)) >=FORWARD_THRESHOLD)
{
/*drop power to right side*/
drive(MOTOR_2, DEFAULT_VEER_POWER);
drive(MOTOR_1, 100);
}
else if ((read_encoder(LEFT_ENCODER) - read_encoder(RIGHT_ENCODER)) >=FORWARD_THRESHOLD)
{
/*drop power to left*/
drive(MOTOR_1, DEFAULT_VEER_POWER);
drive(MOTOR_2, 100);
}
}
}
else if (wall_follow_flag)
{
while(1)
{
avg=analog(port);
fd(MOTOR_1);
fd(MOTOR_2);
if (abs(ideal-avg) > thresh)
{ /*we gotta do something*/
if ((ideal-avg) > 0)
{
/*turn into wall*/
if (port==LEFT_DIST_PORT)
{
veer_left(1);
}
else
{
veer_right(1);
}
}
else
{
/*get outta there*/
if (port==LEFT_DIST_PORT)
{
veer_right(1);
}
else
{
veer_left(1);
}
}
if (read_encoder(RIGHT_ENCODER)>totaldist)
{
break;
}
if (cflag)
{
break;
}
ltickstogo=totaldist-read_encoder(LEFT_ENCODER);
rtickstogo=totaldist-read_encoder(RIGHT_ENCODER);
}
}
}
stop();
if (cflag)
{
cflag=0;
forward_cm();
}
moving=0;
}
void forward_cm()
{
tone(800.0, .5);
backup(BACKUP_TICKS);
if (moving==LFORWARD)
{
if (wall_follow_dist<analog(LEFT_DIST_PORT))
/*we're stuck against left wall*/
{
veer_right(1);
}
else
{
veer_left(1);
}
sleep(.2);
stop();
forward(rtickstogo+BACKUP_TICKS, WALL_FOLLOW, LEFT_DIST_PORT, wall_follow_dist);
}
else
{
if (wall_follow_dist<analog(RIGHT_DIST_PORT))
{
veer_left(1);
}
else
{
veer_right(1);
}
sleep(.2);
stop();
forward(rtickstogo+BACKUP_TICKS, WALL_FOLLOW, RIGHT_DIST_PORT, wall_follow_dist);
}
}
void stop()
{
drive(MOTOR_1, 0);
drive(MOTOR_2, 0);
}
/**********************************************************************
* veer right.
**********************************************************************/
void veer_right(int factor) {
stop();
moving=1;
drive(MOTOR_1, factor*100);
drive(MOTOR_2, factor*DEFAULT_VEER_POWER);
sleep(VEER_TIME);
}
/**********************************************************************
* veer left.
**********************************************************************/
void veer_left(int factor) {
stop();
moving=1;
drive(MOTOR_2, factor*100);
drive(MOTOR_1, factor*83);
sleep(VEER_TIME);
}
/**********************************************************************
* Turn left the specified number of degrees. Assumes the robot is
* stopped before this is called. This routine uses the shaft encoder
* to figure out how far to turn.
**********************************************************************/
void left(int angle) {
float angle_multiplier=(float)angle/90.0;
int right_ticks=(int)(angle_multiplier*(float)TICKS_90);
int left_ticks=(int)(angle_multiplier*(float)TICKS_90);
int pid1, pid2;
ttick=0;
moving=LEFT;
pid1=start_process(drive_ticks(MOTOR_1, LEFT_ENCODER, left_ticks, -100));
pid2=start_process(drive_ticks(MOTOR_2, RIGHT_ENCODER, right_ticks, 100));
/**********WAIT!!!***/
while(1)
{
ltickstogo=left_ticks-read_encoder(LEFT_ENCODER);
rtickstogo=right_ticks-read_encoder(RIGHT_ENCODER);
if (ttick==2)
{
moving=0;
break;
}
if (cflag)
{
kill_process(pid1);
kill_process(pid2);
stop();
moving=0;
cflag=0;
left_cm();
break;
}
}
}
void left_cm()
{
veer_left(1);
sleep(.1);
stop();
forward(BACKUP_TICKS, NO_WALL_FOLLOW, 0, 0);
left(90*(rtickstogo/TICKS_90));
}
/**********************************************************************
* Turn right the specified number of degrees. Assumes the robot is
* stopped before this is called. This routine uses the shaft encoder
* to figure out how far to turn.
**********************************************************************/
void right(int angle) {
float angle_multiplier=(float)angle/90.0;
int right_ticks=(int)(angle_multiplier*(float)TICKS_90);
int left_ticks=(int)(angle_multiplier*(float)TICKS_90);
int pid1, pid2;
ttick=0;
moving=RIGHT;
pid1=start_process(drive_ticks(MOTOR_1, LEFT_ENCODER, left_ticks, 100));
pid2=start_process(drive_ticks(MOTOR_2, RIGHT_ENCODER, right_ticks, -100));
/**********WAIT!!!***/
while(1)
{
ltickstogo=left_ticks-read_encoder(LEFT_ENCODER);
rtickstogo=right_ticks-read_encoder(RIGHT_ENCODER);
if (ttick==2)
{
moving=0;
break;
}
if (cflag)
{
kill_process(pid1);
kill_process(pid2);
stop();
moving=0;
cflag=0;
right_cm();
break;
}
}
}
void right_cm()
{
veer_right(1);
sleep(.1);
stop();
forward(BACKUP_TICKS, NO_WALL_FOLLOW, 0, 0);
right(90*(ltickstogo/TICKS_90));
}
void backup(int ticks)
{
int pid1, pid2;
ttick=0;
moving=BACK;
pid1=start_process(drive_ticks(MOTOR_1, LEFT_ENCODER, ticks, -100));
pid2=start_process(drive_ticks(MOTOR_2, RIGHT_ENCODER, ticks, -100));
while(1)
{
ltickstogo=ticks-read_encoder(LEFT_ENCODER);
rtickstogo=ticks-read_encoder(RIGHT_ENCODER);
if (ttick==2)
{
break;
}
if (cflag)
{
kill_process(pid1);
kill_process(pid2);
stop();
cflag=0;
moving=0;
backup_cm();
break;
}
}
}
void backup_cm()
{
forward(BACKUP_TICKS, NO_WALL_FOLLOW, 0, 0);
if (digital(BOTTOM_LEFT_SWITCH))
{
right(5);
}
else
{
left(5);
}
backup(rtickstogo);
}
void calibrate_dist()
{
int sum=0;
int i;
for (i=0; i<DIST_SAMPLES; ++i)
{
sum+=analog(RIGHT_DIST_PORT);
}
RIGHT_WALL_DIST=sum/DIST_SAMPLES;
sum=0;
for (i=0; i<DIST_SAMPLES; ++i)
{
sum+=analog(LEFT_DIST_PORT);
}
LEFT_WALL_DIST=sum/DIST_SAMPLES;
}
void go()
{
/*calibrate_dist();*/
orient();
chomp();
/*start_process(activate_ball_switch());*/
}
void distance_print()
{
while(1)
{
printf("right:%d left:%d\n", analog(2), analog(4));
sleep(.2);
}
}
int robot_in_way()
{
int i;
for(i=0; i<100; ++i)
{
if (digital(IR_4))
{
return 1;
}
}
return 0;
}
void jammed()
{
int l=0;
int r=0;
int i;
while(1)
{
if (moving!=0)
{
l=read_encoder(LEFT_ENCODER);
r=read_encoder(RIGHT_ENCODER);
tone(200.0, .3);
sleep(10.0);
if ((read_encoder(LEFT_ENCODER)==l) || (read_encoder(RIGHT_ENCODER)==r))
{
cflag=1;
/*we're stuck*/
tone(500.0, .3);
stop();
}
}
else
{
cflag=0;
}
}
}
void chomp_music()
{
tone(400.0, 0.3);
sleep(0.2);
tone(400.0, 0.5);
sleep(0.1);
tone(400.0, 0.5);
sleep(0.1);
tone(600.0, 1.0);
}
void play_music()
{
while(1)
{
if (!digital(16))
{
chomp_music();
}
}
}
int calibrate(int port)
{
int sum=0;
int i;
for (i=0; i<DIST_SAMPLES; ++i)
{
sum+=analog(port);
}
return (sum/DIST_SAMPLES);
}
void main() {
int i, PID=-1;
int SAFETYPID=-1;
/*while(!digital(BOTTOM_LEFT_SWITCH));*/
/*FAR_LEFT_WALL_DIST=calibrate(LEFT_DIST_PORT);*/
start_machine(START_PORT);
SAFETYPID=start_process(jammed());
setup();
go();
start_process(play_music());
start_process(distance_print());
/*goto their side remember to fix orient*/
forward(320, NO_WALL_FOLLOW, RIGHT_DIST_PORT, RIGHT_WALL_DIST);
left(90);
/*back into wall*/
PID=start_process(backup(100000));
while(!((digital(BOTTOM_LEFT_SWITCH)) && (digital(BOTTOM_RIGHT_SWITCH))));
kill_process(PID);
stop();
moving=0;
PID=start_process(forward(5000, NO_WALL_FOLLOW, RIGHT_DIST_PORT, FAR_RIGHT_WALL_DIST));
while(!((digital(TOP_LEFT_SWITCH)) && (digital(TOP_RIGHT_SWITCH))));
kill_process(PID);
stop();
moving=0;
backup(20);
/*left(90);*/
/*check beacon and either go forward or go back the way we came*/
/*if (!robot_in_way())
{
forward(800, WALL_FOLLOW, RIGHT_DIST_PORT, RIGHT_WALL_DIST);
}
else
{*/
left(180);
PID=start_process(forward(500, NO_WALL_FOLLOW, LEFT_DIST_PORT, FAR_LEFT_WALL_DIST));
while(!((digital(TOP_LEFT_SWITCH)) && (digital(TOP_RIGHT_SWITCH))));
kill_process(PID);
stop();
moving=0;
backup(20);
right(85);
while(!((digital(TOP_LEFT_SWITCH)) && (digital(TOP_RIGHT_SWITCH)))) {
PID=start_process(forward(800, NO_WALL_FOLLOW, LEFT_DIST_PORT, LEFT_WALL_DIST));
while(!(digital(TOP_LEFT_SWITCH)));
kill_process(PID);
stop();
moving=0;
backup(10);
right(10);
}
/* }*/
}
</body>
]]>