int gyro_type = 300; /* 150 for
ADXRS150, 300 for ADXRS300 */
float lsb_ms_per_deg = 256.0; /* nominal
scale factor depending on gyro type */
float scale_factor = 1.017; /* adjust
this for your particular gyro! */
float theta;
/* current angle, units explained in update_angle()
*/
float offset; /* gyro
offset */
int gyro_port = 6; /* gyro analog
input port number */
int num_avgs = 800; /*
number of averages for calibration */
long time_ms;
/* system time the last gyro reading was taken
*/
int posdetermine;
int orientation;
int ypos0,
ypos1, xpos0, xpos1;
int frontpos= 1;
int state = 0;
int
led_threshold;
void update_position(){
if(posdetermine == 0){
ypos0 =
rf_y0;
xpos0 = rf_x0;
ypos1 = rf_y1;
xpos1 = rf_x1;
}
else if
(posdetermine == 1){
ypos0 = rf_y1;
xpos0 =
rf_x1;
ypos1 = rf_y0;
xpos1 = rf_x0;
}
}
int max3(int
first, int second, int third){
if(first
>= second){
if(first >= third){
return first;
}
else return third;
}
else if( second >= third) return
second;
else
return third;
}
int min3(int first, int second, int
third){
if(first <= second){
if(first <= third){
return
first;
}
else
return third;
}
else if(second <= third) return second;
else return third;
}
void
calibrate_led(){
long current = mseconds();
printf("0 for black, 1 for
white\n");
while(mseconds() < current +
5000L){
if(start_button()){
/* start button is the black
*/
led_threshold = min3(analog(22), analog(20), analog(16))
-5;
printf("black");
printf("%d", led_threshold);
}
else if
(stop_button()){
/* stop is white */
led_threshold = max3(analog(22),
analog(20), analog(16)) + 5;
printf("white");
printf("%d",
led_threshold);
}
}
sleep(2.0);
}
void
orient_syncro(){
int led1=
analog(22);
int led2 =
analog(20);
int led3 =
analog(16);
msleep(70L);
led1 = analog(22);
led2
= analog(20);
led3 =
analog(16);
/* first the four cases of
the black area */
if(led1 >=
led_threshold && led2 <led_threshold && led3 >=
led_threshold){ /*robot facing north, just move forwards, led's black, white,
black*/
if(rf_y0
> 0)
{posdetermine = 0;
}
else {posdetermine = 1;} /*assigning the position of my robot and
opponent */
orientation = 0;
frontpos = 1;
state = 0;
printf("Forward\n");
}
else if(led1 < led_threshold
&& led2 >= led_threshold && led3 >= led_threshold){
/*robot facing 90 degress cw from north, led's white, black,
black*/
if(rf_y0
> 0)
{posdetermine = 0;
}
else {posdetermine = 1;}
orientation = 1;
frontpos = 1;
state = 1;
printf("90DegF\n");
}
else if (led1 >=
led_threshold && led2 >= led_threshold && led3 >=
led_threshold){ /* robot facing south, led's black, black, black
*/
if(rf_y0 >
0)
{posdetermine = 0;
}
else {posdetermine = 1;}
printf("Move Backwards!\n"); /* assigning the
position of my robot and opponent */
frontpos = -1;
orientation = 2;
state = 0;
}
else if (led1 >=
led_threshold && led2 >=led_threshold && led3 <
led_threshold){ /* robot facing 270 degrees anticlockwise from north, led's
black, black, white */
/*
assigning the position of my robot and opponent */
if(rf_y0 > 0)
{posdetermine =
0;
}
else
{posdetermine = 1;}
frontpos = -1;
orientation = 3;
state = 1;
printf("90DegB!\n");
}
/*the white side with four cases
*/
else if(led1 < led_threshold
&& led2 < led_threshold && led3 >= led_threshold){ /* the
robot facing north, led's white, white, black */
if(rf_y0 <0)
{posdetermine = 0;
}
else {posdetermine = 1;}
/* assigning the position of my robot and opponent */
orientation = 0;
frontpos = -1;
/* ********** double check this later ************** */
state = 0;
printf("Move
Backward!\n");
}
else if(led1 < led_threshold
&& led2 >= led_threshold && led3 < led_threshold){/*the
robot facing 90 degrees cw from north, led's white, black,
white*/
if(rf_y0
<0)
{posdetermine = 0;
}
else {posdetermine = 1;} /* assigning the position of my robot and
opponent */
orientation = 1;
frontpos= -1; /* ********** double check later *************** */
state = 1;
printf("90DegB\n");
}
else if (led1 >=
led_threshold && led2 < led_threshold && led3 <
led_threshold){ /*the robot facing south, led's black, white, white */
if(rf_y0 <0)
{posdetermine =
0;
}
else
{posdetermine = 1;} /* assigning the position of my robot and opponent */
state =
0;
orientation =
2;
printf("Forwards!\n");
frontpos = -1; /* *******************double check later
***************** */
}
else if (led1 < led_threshold
&& led2 < led_threshold && led3 < led_threshold){ /*the
robot facing 90 ccw from north, led's white, white, white */
if(rf_y0 <0)
{posdetermine = 0;
}
else {posdetermine = 1;}
/* assigning the position of my robot and opponent */
state = 1;
orientation = 3;
frontpos = 1;
/* ********************* double check later ****************** */
printf("90Forwards!\n");
}
}