HallBot Interactive C code
#use "use-sonar.ic"
#use "explego.icb"
int sonar1 = 100000;
int done = 0;
#define desired_dist 8000.0
#define REAR_MOTOR 3
#define STEER_MOTOR 0
#define STEER_SPEED 10
#define BUMP 2
#define STEP 20
#define FL 2540
#define F 2040
#define FR 1540
#define RF 1040
#define R 540
#define RB 40
#define FRONT_THRESHOLD 6000.0
#define Kp .7
// proportional gain
#define Kd 0.0
// derivative gain
void ctr()
{
int pos = (lego0_counts > 0);
while(lego0_counts != 0)
{
if(pos)
motor(STEER_MOTOR, -STEER_SPEED);
else
motor(STEER_MOTOR, +STEER_SPEED);
printf("center Angle: %d\n", lego0_counts);
}
if(pos)
motor(STEER_MOTOR, +STEER_SPEED);
else
motor(STEER_MOTOR, -STEER_SPEED);
sleep(0.07);
off(STEER_MOTOR);
// if(digital(BUMP))
// lego0_counts = 0;
}
void l(int num)
{
while(lego0_counts < num)
{
motor(STEER_MOTOR, STEER_SPEED);
printf("left Angle: %d\n", lego0_counts);
}
motor(STEER_MOTOR, -STEER_SPEED);
sleep(0.07);
off(STEER_MOTOR);
}
void r(int num)
{
while(lego0_counts > -num)
{
motor(STEER_MOTOR, -STEER_SPEED);
printf("right Angle: %d\n", lego0_counts);
}
motor(STEER_MOTOR, STEER_SPEED);
sleep(0.07);
off(STEER_MOTOR);
}
void main()
{
int pid1;
int pid2;
int pid3;
printf("Not started\n");
while(!start_button());
/*
#if 1
test_steering();
#else
*/
pid1 = start_process(moveServo());
// pid2 = start_process(detectSonar());
pid3 = start_process(wallFollow());
while(!stop_button());
done = 1;
alloff();
kill_process(pid1);
// kill_process(pid2);
kill_process(pid3);
// #endif
}
void test_steering()
{
int pid = start_process(wallFollow());
printf("Left\n");
turnLeft();
sleep(2.0);
printf("Right\n");
turnRight();
sleep(2.0);
printf("Right\n");
turnRight();
sleep(2.0);
printf("Left\n");
turnLeft();
sleep(2.0);
printf("Center\n");
center();
sleep(2.0);
/*printf("Right\n");
turnRight();
sleep(2.0);
printf("Center\n");
center();
sleep(2.0);
printf("Center");
center();
sleep(2.0);
*/
//kill_process(pid);
}
void moveServo()
{
int region[] = {RB, R, RF, FR, F, FL};
float currMinRange[] = {desired_dist, desired_dist, desired_dist, 20000.0, 20000.0};
float error[] = {desired_dist, desired_dist, desired_dist, 20000.0, 20000.0};
float last_distance;
init_expbd_servos(1);
sonar_init();
while (!done) {
float distance;
float last_distance;
int i, r;
servo0 = RB;
sleep(0.7);
beep();
// Iterate through each sonar region
for (r = 0; r < 5; ++r)
{
float min_side_error;
float min_front_dist;
int pos;
int count = 0;
// Save the last reading for comparison before writing
// a new one.
last_distance = currMinRange[r];
currMinRange[r] = 0.0;
// Iterate through all valid positions in the sonar region
for (pos = region[r]; pos < region[r + 1]; pos+=STEP)
{
// Move sonar to new position
servo0 = pos;
// Check the sonar and update the minimum value.
distance = (float)sonar_sample();
// if (distance != -1L){// && distance < currMinRange[r]) {
count += 1;
currMinRange[r] += distance;
// }
}
//printf("currmin = %d\n", (int)(currMinRange[r]-32000L));
//sleep(.5);
currMinRange[r] = currMinRange[r]*(1.0/(float)count);
// printf("minrange = %f, count = %d\n", (float)currMinRange[r],(int)count);
// sleep(.5);
/* NASTY HACK: in long hallways, we would read really low
* values that we shouldn't see, ever. Our hypothesis is
* that this is caused by echoes that take longer
* than the period of the sonar. Our solution is to
* change any suspiciously close value to be "really far."
*/
if (currMinRange[r] < 2000.0)
currMinRange[r] = 20000.0;
// Proportional Derivative Control
//printf("Range: %d Loc: %d\n", (int)currMinRange[r], r);
// Use PD control on side sensor regions
if (r < 4) {
float p_error = (currMinRange[r] - desired_dist) * Kp;
float d_error = (currMinRange[r] - last_distance)* Kd;
error[r] = p_error + d_error;
}
if (currMinRange[4] > currMinRange[3])
min_front_dist = currMinRange[4];
else min_front_dist = currMinRange[3];
// Compute minimum error over the side region
// First, set error to really big numbers, because
// we'll be finding lower numbers and want
// to keep the smallest.
min_side_error = 20000.0;
for(i = 0 ; i < 4; ++i)
if (error[i] < min_side_error)
min_side_error = error[i];
// printf("front = %d ", min_front_dist);
// printf("side = %d\n", (min_side_error + desired_dist));
// Act on errors:
// If we're about to hit something, turn left.
// Otherwise, align with right wall at desired dist.
if (currMinRange[4] < FRONT_THRESHOLD)
{printf("front =%f side_err=%f front-left\n",min_front_dist, (min_side_error)); turnLeft();}
// turnLeft();
else if (min_side_error > 1000.0)
{printf("front =%f side_err=%f right\n", min_front_dist, (min_side_error)); turnRight();}
// turnRight();
else if (min_side_error < -500.0)
{printf("front =%f side_err=%f left\n", min_front_dist, (min_side_error)); turnLeft();}
// turnLeft();
else
{printf("front =%f side_err=%f center\n", min_front_dist, (min_side_error)); center();}
// center();
}
}
}
/*
* STEERING CONTROL
*
* The turnRight, turnLeft, and Center functions set a global variable
* to indicate to the wallFollow function which direction to turn the
* front wheels.
*/
int STEER_DIRECTION = 0;
void turnRight()
{
STEER_DIRECTION = 1;
}
void center()
{
STEER_DIRECTION = 0;
}
void turnLeft()
{
STEER_DIRECTION = -1;
}
// This function follows walls on the right
void wallFollow()
{
int prev_direction = 0;
// The robot should start out with its steering centered.
// We give a brief audio warning if this doesn't seem to be the case.
if(!digital(BUMP)) beep();
// Start moving!
motor(3,60);
// Turn vehicle based on global steering direction.
// This variable is set by the turning commands above.
while(!done){
// turn the robot if it needs turning
float turn_time = .5;
if (STEER_DIRECTION != prev_direction)
{
// Center wheels
if (STEER_DIRECTION == 0)
{
ctr();
if (prev_direction == 1)
l(1);
else r(1);
ctr();
/*
// Bring motor back from previous direction.
motor(STEER_MOTOR, -prev_direction * 4);
while (!digital(BUMP));
beep();
// This will overshoot a bit.
motor(STEER_MOTOR, -prev_direction * 5);
sleep(turn_time + .5);
motor(STEER_MOTOR, 0);
printf("centering now");
beep();
// Return to center.
motor(STEER_MOTOR, prev_direction * 4);
while (!digital(BUMP));
beep();
/*
// Correct overshoot by moving wheels until
// the bump sensor detects centering
while(!digital(BUMP))
motor(STEER_MOTOR, -prev_direction * 2);
*/
/*
// Done turning
motor(STEER_MOTOR, 0);
printf("CENTER");
*/ }
// Turn left or right
else
{
// If the motor wasn't centered, we have to recenter it
// before we turn the other direction
if (prev_direction != 0)
{
ctr();
/*
motor(STEER_MOTOR, STEER_DIRECTION * 4);
while (!digital(BUMP))
{
sleep(.05);
beep();
}
// sleep(0.05);
*/ }
if(STEER_DIRECTION == 1)
r(2);
else
l(2);
ctr();
/*
motor(STEER_MOTOR, STEER_DIRECTION * 5);
sleep(turn_time);
// Done turning
motor(STEER_MOTOR, 0);
*/ }
prev_direction = STEER_DIRECTION;
}
}
// All done.
alloff();
}