📄 car.c
字号:
//
g_sFrontDelta = (((3 * g_sFrontDelta) +
(g_usFrontSensor - lDiff)) / 4);
g_usFrontSensor = ((3 * g_usFrontSensor) + lDiff) / 4;
}
//
// Check for a random direction change if in a running mode that is not
// turning.
//
if((g_eMode == MODE_IN_OPEN) || (g_eMode == MODE_APPROACHING) ||
(g_eMode == MODE_FOLLOWING))
{
//
// Decrement the count if it has not reached zero.
//
if(g_usCount != 0)
{
g_usCount--;
}
//
// See if a random direction change should be made.
//
if((g_usCount == 0) &&
(g_usFrontSensor > INCHES(16, 0)) &&
(g_usLeftSensor > INCHES(12, 0)) &&
(g_usRightSensor > INCHES(12, 0)))
{
//
// Get a random number.
//
lDiff = RandomNumber();
//
// Choose whether to turn left or right.
//
if((g_usLeftSensor < INCHES(24, 0)) &&
(g_usLeftSensor < g_usRightSensor))
{
g_eMode = MODE_RANDOM_RIGHT;
}
else if((g_usRightSensor < INCHES(24, 0)) &&
(g_usRightSensor < g_usLeftSensor))
{
g_eMode = MODE_RANDOM_LEFT;
}
else
{
g_eMode = ((lDiff < 0) ? MODE_RANDOM_LEFT :
MODE_RANDOM_RIGHT);
}
//
// Select a random amount of time to turn.
//
g_usCount = ((lDiff >> 27) & 15) + 10;
//
// Stop the motors.
//
g_usTargetLeft = SPEED(0);
g_usTargetRight = SPEED(0);
//
// Nothing further needs to be done now.
//
return;
}
}
}
//
// Determine what to do based on the current mode.
//
switch(g_eMode)
{
//
// The car is in the open with no walls within its range of interest.
//
case MODE_IN_OPEN:
{
//
// If there is a sensor reading, then transition to the approaching
// state.
//
if((g_usFrontSensor != INFINITY) || (g_usLeftSensor != INFINITY) ||
(g_usRightSensor != INFINITY))
{
g_eMode = MODE_APPROACHING;
}
//
// Done with this mode.
//
break;
}
//
// The car is approaching a wall.
//
case MODE_APPROACHING:
{
//
// If the car as gotten too close to a wall then stop and turn in
// place to get away from it.
//
if((g_usFrontSensor < INCHES(10, 0)) ||
(g_usLeftSensor < INCHES(7, 0)) ||
(g_usRightSensor < INCHES(7, 0)))
{
//
// Turn either to the left or right based on whichever wall is
// closer.
//
if(g_usLeftSensor < g_usRightSensor)
{
g_eMode = MODE_TURN_RIGHT;
}
else
{
g_eMode = MODE_TURN_LEFT;
}
//
// Stop the motors.
//
g_usTargetLeft = SPEED(0);
g_usTargetRight = SPEED(0);
//
// Done with this mode.
//
break;
}
//
// Turn if there is a wall within 20 inches (50.8 cm).
//
if((g_usLeftSensor < INCHES(20, 0)) ||
(g_usRightSensor < INCHES(20, 0)) ||
(g_usFrontSensor != INFINITY))
{
//
// See if the left or right wall is closer.
//
if(g_usLeftSensor < g_usRightSensor)
{
//
// If the left wall is getting further away at a reasonable
// rate (i.e. between .2 and .5 inches per 1/10th of a
// second) then switch into following mode.
//
if((g_sLeftDelta < (0 - INCHES(0, 200))) &&
(g_sLeftDelta > (0 - INCHES(0, 500))) &&
(g_usLeftSensor < INCHES(12, 0)) &&
(g_usFrontSensor == INFINITY))
{
g_usTargetLeft = SPEED(50);
g_usTargetRight = SPEED(50);
g_eMode = MODE_FOLLOWING;
break;
}
//
// See if the left wall is too far away to care yet.
//
if(g_usLeftSensor > INCHES(20, 0))
{
//
// Don't turn just yet based on the left wall.
//
lDiff = 0;
}
else
{
//
// The rate to turn away from the wall is inversely
// proportional to the distance to the wall.
//
if(g_usRightSensor < INCHES(20, 0))
{
lDiff = g_usRightSensor - g_usLeftSensor;
}
else
{
lDiff = INCHES(20, 0) - g_usLeftSensor;
}
//
// If approaching the wall, increase the turn rate
// proportional to the rate of approach.
//
if(g_sLeftDelta > 0)
{
lDiff += g_sLeftDelta * 4;
}
}
//
// See if there is a wall in front as well.
//
if(g_usFrontSensor != INFINITY)
{
//
// Increase the turn rate by an amount inversely
// proportional to the distance to the wall in front.
//
lDiff += ((INCHES(26, 0) - g_usFrontSensor) * 3) / 2;
//
// If approaching the wall, increase the turn rate
// proportional to the rate of approach.
//
if(g_sFrontDelta > 0)
{
lDiff += g_sFrontDelta * 4;
}
}
//
// Scale the turn rate to something between 0% and 50%.
//
lDiff = ((lDiff * INCHES(1, 250)) /
(INCHES(1, 0) * INCHES(1, 0)));
if(lDiff > MOTOR_LIMIT)
{
lDiff = MOTOR_LIMIT;
}
//
// Compute the new left and right motor speeds.
//
lLeft = lDiff + 50;
lRight = 50 - lDiff;
}
else
{
//
// If the right wall is getting further away at a
// reasonable rate (i.e. between .2 and .5 inches per
// 1/10th of a second) then switch into following mode.
//
if((g_sRightDelta < (0 - INCHES(0, 200))) &&
(g_sRightDelta > (0 - INCHES(0, 500))) &&
(g_usRightSensor < INCHES(12, 0)) &&
(g_usFrontSensor == INFINITY))
{
g_usTargetLeft = SPEED(50);
g_usTargetRight = SPEED(50);
g_eMode = MODE_FOLLOWING;
break;
}
//
// See if the right wall is too far away to care yet.
//
if(g_usRightSensor > INCHES(20, 0))
{
//
// Don't turn just yet based on the right wall.
//
lDiff = 0;
}
else
{
//
// The rate to turn away from the wall is inversely
// proportional to the distance to the wall.
//
if(g_usLeftSensor < INCHES(20, 0))
{
lDiff = g_usLeftSensor - g_usRightSensor;
}
else
{
lDiff = INCHES(20, 0) - g_usRightSensor;
}
//
// If approaching the wall, increase the turn rate
// proportional to the rate of approach.
//
if(g_sRightDelta > 0)
{
lDiff += g_sRightDelta * 4;
}
}
//
// See if there is a wall in front as well.
//
if(g_usFrontSensor != INFINITY)
{
//
// Increase the turn rate by an amount inversely
// proportional to the distance to the wall in front.
//
lDiff += ((INCHES(26, 0) - g_usFrontSensor) * 3) / 2;
//
// If approaching the wall, increase the turn rate
// proportional to the rate of approach.
//
if(g_sFrontDelta > 0)
{
lDiff += g_sFrontDelta * 4;
}
}
//
// Scale the turn rate to something between 0% and 50%.
//
lDiff = ((lDiff * INCHES(1, 250)) /
(INCHES(1, 0) * INCHES(1, 0)));
if(lDiff > MOTOR_LIMIT)
{
lDiff = MOTOR_LIMIT;
}
//
// Compute the new left and right motor speeds.
//
lLeft = 50 - lDiff;
lRight = lDiff + 50;
}
}
else
{
//
// The walls aren't close enough to care about yet, so drive
// straight ahead.
//
lLeft = 50;
lRight = 50;
//
// If there are no walls in sight, switch back into the in the
// open mode.
//
if((g_usLeftSensor == INFINITY) &&
(g_usRightSensor == INFINITY) &&
(g_usFrontSensor == INFINITY))
{
g_eMode = MODE_IN_OPEN;
}
}
//
// Set the motor speed as computed.
//
g_usTargetLeft = SPEED(lLeft);
g_usTargetRight = SPEED(lRight);
//
// Done with this mode.
//
break;
}
//
// The car is following a wall.
//
case MODE_FOLLOWING:
{
//
// If the car as gotten too close to a wall then stop and turn in
// place to get away from it.
//
if((g_usFrontSensor < INCHES(10, 0)) ||
(g_usLeftSensor < INCHES(7, 0)) ||
(g_usRightSensor < INCHES(7, 0)))
{
//
// Turn either to the left or right based on whichever wall is
// closer.
//
if(g_usLeftSensor < g_usRightSensor)
{
g_eMode = MODE_TURN_RIGHT;
}
else
{
g_eMode = MODE_TURN_LEFT;
}
//
// Stop the motors.
//
g_usTargetLeft = SPEED(0);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -