void calibrate_gyro(){
/*
Averaging a
number of readings only works when the gyro
noise is greater
than the converter resolution. Since
the xrs300 noise is
far lower than the 20mV resolution
of the 8-bit converter, a
noise source has been added
to the gyro board. You should
average for about 5
seconds to minimize the effects of the
added noise.
*/
int samples;
float
sum;
long start_time_ms;
long int_time_ms =
1000L;
printf("Stabilizing...\n");
msleep(500L);
/* Wait for robot to stabilize mechanically
*/
printf("Calibrating
offset...\n");
/* average some samples for offset
*/
sum = 0.0;
samples =
0;
start_time_ms = mseconds();
while( ( mseconds()
- start_time_ms ) < int_time_ms ){
sum +=
(float)analog(gyro_port);
samples++;
}
offset = sum /
(float)samples;
theta = 0.0;
} /* calibrate_gyro()
*/
void update_angle(){
/* The units of theta are converter LSBs *
ms
1 deg/s = 5mV = 0.256 LSB for ADXRS300, 12.5mV = 0.64LSB for
ADXRS150
1 deg = 0.256LSB * 1000ms = 256 (stored in
lsb_ms_per_deg)
To convert theta to degrees, divide by
lsb_ms_per_deg. */
long delta_t_ms,
new_time_ms;
int i;
time_ms =
mseconds();
for(;;){
new_time_ms =
mseconds();
delta_t_ms = (new_time_ms -
time_ms);
/* This does the Riemann sum; CCW gyro
output polarity is negative
when the gyro is
visible from the top of the robot. */
theta -=
((float)analog(gyro_port) - offset) * (float)delta_t_ms;
time_ms = new_time_ms;
defer();
}
} /* update_angle() */
void
display_angle(){
for(;;){
printf("%d
degrees\n",(int)(theta / lsb_ms_per_deg));
msleep(200L);
defer();
}
} /*
display_angle() */
int sign(float a){
if ( a > 0.0
) return 1;
else return -1;
} /* sign() */
float
fabs(float a){
if ( a > 0.0 ) return a;
else
return -a;
} /* fabs() */
void straight_syncro_vertical(float angle,
int speed, long duration_ms, int front){
/* The robot turns to the
specified angle, then goes straight for
the specified duration.
Note that speed must be greater than tweak,
and less than 100 -
tweak. */
long start_time;
float tweak =
5.0;
float angle_target;
float
tolerance;
lsb_ms_per_deg = 256.0 * scale_factor;
tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy
*/
/* then go forward under gyro control for the specified
time */
start_time = mseconds();
/*angle_target =
angle * lsb_ms_per_deg; */
while ((mseconds() -
start_time) < duration_ms){
/* move
motors forward a bit */
motor(1, front *
speed);
motor(2, front *
speed);
msleep(100L);
if (theta < (0.0 - tolerance))
{
/* Turn servos CCW a little */
servo_turn(-tweak);
} else if (theta > (0.0 + tolerance)){
/*
Turn servos CW a little */
servo_turn(tweak);
} /* You can check a sensor here and
terminate the while loop */
} /* while */
/* Throw
it in reverse briefly to stop quickly */
motor(1, front* -1 *speed);
motor(2, front * -1 * speed);
msleep(20L);
motor(1,
0);
motor(2, 0);
} /* void go_straight() */
void
straight_syncro_horizontal(float angle, int speed, long duration_ms, int
front){
/* The robot turns to the specified angle, then goes straight for
the specified duration. Note that speed must be greater than
tweak,
and less than 100 - tweak. */
long
start_time;
float tweak = 5.0;
float
angle_target;
float tolerance;
lsb_ms_per_deg = 256.0;
tolerance = lsb_ms_per_deg * 0.5; /*
+/-0.5 degree accuracy */
/* then turn robot
*/
servo_turn(angle);
/* then go forward under gyro
control for the specified time */
start_time =
mseconds();
angle_target = angle * lsb_ms_per_deg;
while ((mseconds() - start_time) <
duration_ms){
/* move motors forward a bit
*/
motor(1, front * speed);
motor(2, front * speed);
msleep(200L);
if (theta < (angle_target - tolerance)) {
/* Turn servos CCW a little */
servo_turn(tweak);
} else if (theta >
(angle_target + tolerance)){
/* Turn servos CW a
little */
servo_turn(-tweak);
} /* You can check a sensor here and
terminate the while loop */
} /* while */
/* Throw
it in reverse briefly to stop quickly */
motor(1, front* -1 *speed);
motor(2, front * -1 * speed);
msleep(20L);
motor(1,
0);
motor(2, 0);
} /* void go_straight() */
void
drive_to_point(int xcoor, int ycoor){
int
threshold = 20;
int xdiff,
ydiff;
servo_horizontal();
if(posdetermine == 0){
ypos0 =
rf_y0;
xpos0 = rf_x0;
}
else if (posdetermine == 1){
ypos0 = rf_y1;
xpos0 = rf_x1;
}
xdiff = xcoor - xpos0;
ydiff = ycoor -
ypos0;
while( abs(xdiff) >=
threshold)
{
if(xdiff
< 0){
straight_syncro_horizontal(0.0, 100, 240L , frontpos *
-1);
if(posdetermine == 0){
ypos0 = rf_y0;
xpos0 =
rf_x0;
}
else if (posdetermine == 1){
ypos0 = rf_y1;
xpos0 = rf_x1;
}
xdiff = xcoor - xpos0;
ydiff = ycoor - ypos0;
}
else if(
xdiff > 0){
straight_syncro_horizontal(0.0, 100, 240L ,
frontpos);
if(posdetermine == 0){
ypos0 = rf_y0;
xpos0 =
rf_x0;
}
else if (posdetermine == 1){
ypos0 = rf_y1;
xpos0 = rf_x1;
}
xdiff = xcoor - xpos0;
ydiff = ycoor - ypos0;
printf(xdiff);
}
else
break;
}
servo_vertical();
while(abs(ydiff) >=
threshold){
if(ydiff <0){
straight_syncro_vertical(0.0, 100, 240L, frontpos
*-1);
if(posdetermine == 0){
ypos0 =
rf_y0;
xpos0 = rf_x0;
}
else if (posdetermine == 1){
ypos0 = rf_y1;
xpos0 = rf_x1;
}
xdiff = xcoor -
xpos0;
ydiff = ycoor - ypos0;
printf(xdiff);
}
else if(ydiff >
0){
straight_syncro_vertical(0.0, 100, 240L,
frontpos);
if(posdetermine == 0){
ypos0 =
rf_y0;
xpos0 = rf_x0;
}
else if (posdetermine == 1){
ypos0 = rf_y1;
xpos0 = rf_x1;
}
xdiff = xcoor -
xpos0;
ydiff = ycoor - ypos0;
}
else break;
}
}
float distance(int x1, int x2, int y1, int
y2){
float result;
float res1 = (float) x1 -(float)x2;
float res2 = (float) y1 - (float) y2;
result = sqrt(res1*res1 + res2*res2);
return result;
}
/*int abs(int value){
if(value >
0)
return value;
else return
-value;
}*/