📄 car.c
字号:
g_usTargetRight = SPEED(0);
//
// Done with this mode.
//
break;
}
//
// If the car has gotten too far from both walls, or there is a
// wall to the front, then switch back into the approaching mode.
//
if(((g_usLeftSensor > INCHES(12, 0)) &&
(g_usRightSensor > INCHES(12, 0))) ||
(g_usFrontSensor != INFINITY))
{
//
// Drive straight ahead.
//
g_usTargetLeft = SPEED(50);
g_usTargetRight = SPEED(50);
//
// Go to the approaching mode.
//
g_eMode = MODE_APPROACHING;
//
// Done with this mode.
//
break;
}
//
// See if the left or right wall is closer and compute the new
// motor speed based on the distance to the wall.
//
if(g_usLeftSensor < g_usRightSensor)
{
lDiff = ((INCHES(9, 0) - g_usLeftSensor) * 5) / INCHES(1, 0);
}
else
{
lDiff = ((g_usRightSensor - INCHES(9, 0)) * 5) / INCHES(1, 0);
}
//
// Set the motor speed as computed.
//
g_usTargetLeft = SPEED(50 + lDiff);
g_usTargetRight = SPEED(50 - lDiff);
//
// Done with this mode.
//
break;
}
//
// Transition to turning in place to the left.
//
case MODE_TURN_LEFT:
{
//
// Turn on the tail lights.
//
TailLightsOn();
//
// See if both motors have stopped.
//
if((g_usMotorLeft < (SPEED(1) / 2)) &&
(g_usMotorRight < (SPEED(1) / 2)))
{
//
// Set the mode to turning.
//
g_eMode = MODE_TURNING;
//
// Reverse the left motor.
//
MotorLeftDir(0);
//
// Set the motor speeds to 70%.
//
g_usTargetLeft = SPEED(70);
g_usTargetRight = SPEED(70);
}
//
// Done with this mode.
//
break;
}
//
// Transition to turning in place to the right.
//
case MODE_TURN_RIGHT:
{
//
// Turn on the tail lights.
//
TailLightsOn();
//
// See if both motors have stopped.
//
if((g_usMotorLeft < (SPEED(1) / 2)) &&
(g_usMotorRight < (SPEED(1) / 2)))
{
//
// Set the mode to turning.
//
g_eMode = MODE_TURNING;
//
// Reverse the right motor.
//
MotorRightDir(0);
//
// Set the motor speeds to 70%.
//
g_usTargetLeft = SPEED(70);
g_usTargetRight = SPEED(70);
}
//
// Done with this mode.
//
break;
}
//
// The car is turning in place.
//
case MODE_TURNING:
{
//
// If there are no walls too close, then stop turning.
//
if((g_usFrontSensor > INCHES(16, 0)) &&
(((g_usLeftSensor > INCHES(12, 0)) &&
(g_usRightSensor > INCHES(7, 500))) ||
((g_usLeftSensor > INCHES(7, 500)) &&
(g_usRightSensor > INCHES(12, 0)))))
{
//
// Set the mode to forward.
//
g_eMode = MODE_FORWARD;
//
// Set the motor speeds to 0%.
//
g_usTargetLeft = SPEED(0);
g_usTargetRight = SPEED(0);
}
//
// Done with this mode.
//
break;
}
//
// Transition to going forward.
//
case MODE_FORWARD:
{
//
// See if both motors have stopped.
//
if((g_usMotorLeft < (SPEED(1) / 2)) &&
(g_usMotorRight < (SPEED(1) / 2)))
{
//
// Set the mode to approaching.
//
g_eMode = MODE_APPROACHING;
//
// Turn off the tail lights.
//
TailLightsOff();
//
// Set the motors forward.
//
MotorLeftDir(1);
MotorRightDir(1);
//
// Set the motor speeds to 50%.
//
g_usTargetLeft = SPEED(50);
g_usTargetRight = SPEED(50);
}
//
// Done with this mode.
//
break;
}
//
// Transition to a random turn in place to the left.
//
case MODE_RANDOM_LEFT:
{
//
// Turn on the tail lights.
//
TailLightsOn();
//
// See if both motors have stopped.
//
if((g_usMotorLeft < (SPEED(1) / 2)) &&
(g_usMotorRight < (SPEED(1) / 2)))
{
//
// Set the mode to a random turn.
//
g_eMode = MODE_RANDOM_TURN;
//
// Reverse the left motor.
//
MotorLeftDir(0);
//
// Set the motor speeds to 70%.
//
g_usTargetLeft = SPEED(70);
g_usTargetRight = SPEED(70);
}
//
// Done with this mode.
//
break;
}
//
// Transition to a random turn in place to the right.
//
case MODE_RANDOM_RIGHT:
{
//
// Turn on the tail lights.
//
TailLightsOn();
//
// See if both motors have stopped.
//
if((g_usMotorLeft < (SPEED(1) / 2)) &&
(g_usMotorRight < (SPEED(1) / 2)))
{
//
// Set the mode to a random turn.
//
g_eMode = MODE_RANDOM_TURN;
//
// Reverse the right motor.
//
MotorRightDir(0);
//
// Set the motor speeds to 70%.
//
g_usTargetLeft = SPEED(70);
g_usTargetRight = SPEED(70);
}
//
// Done with this mode.
//
break;
}
//
// The car is making a random turn in place.
//
case MODE_RANDOM_TURN:
{
//
// Decrement the count if it has not reached zero.
//
if(g_usCount != 0)
{
g_usCount--;
}
//
// If the car has turned for the proper amount of time and there
// are no walls too close, then stop turning.
//
if((g_usCount == 0) &&
(g_usFrontSensor > INCHES(16, 0)) &&
(((g_usLeftSensor > INCHES(12, 0)) &&
(g_usRightSensor > INCHES(12, 0))) ||
((g_usLeftSensor > INCHES(12, 0)) &&
(g_usRightSensor > INCHES(12, 0)))))
{
//
// Set the mode to forward.
//
g_eMode = MODE_RANDOM_FORWARD;
//
// Set the motor speeds to 0%.
//
g_usTargetLeft = SPEED(0);
g_usTargetRight = SPEED(0);
}
//
// Done with this mode.
//
break;
}
//
// Transition to going forward.
//
case MODE_RANDOM_FORWARD:
{
//
// See if both motors have stopped.
//
if((g_usMotorLeft < (SPEED(1) / 2)) &&
(g_usMotorRight < (SPEED(1) / 2)))
{
//
// Set the mode to in the open.
//
g_eMode = MODE_IN_OPEN;
g_usLeftSensor = INFINITY;
g_sLeftDelta = 0;
g_usRightSensor = INFINITY;
g_sRightDelta = 0;
g_usFrontSensor = INFINITY;
g_sFrontDelta = 0;
//
// Pick the amount of time until the next random turn.
//
g_usCount = RANDOM_TIME;
//
// Turn off the tail lights.
//
TailLightsOff();
//
// Set the motors forward.
//
MotorLeftDir(1);
MotorRightDir(1);
//
// Set the motor speeds to 50%.
//
g_usTargetLeft = SPEED(50);
g_usTargetRight = SPEED(50);
}
//
// Done with this mode.
//
break;
}
//
// The car is in diagnostic mode 3.
//
case MODE_DIAG3:
{
//
// If the delay has reached 50ms, reverse the direction of the left
// motor.
//
if(g_ucDelay == ((50 * SYSTICK_CLOCK) / 1000))
{
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -