/*
Following mode：
*/
void ApplicationFunctionSet::ApplicationFunctionSet_Follow(void)
{
static uint16_t ULTRASONIC_Get = 0;
static unsigned long ULTRASONIC_time = 0;
static uint8_t Position_Servo = 1;
static uint8_t timestamp = 3;
static uint8_t OneCycle = 1;
if (Application_SmartRobotCarxxx0.Functional_Mode == Follow_mode)
{
if (Car_LeaveTheGround == false)
{
ApplicationFunctionSet_SmartRobotCarMotionControl(stop_it, 0);
return;
}
AppULTRASONIC.DeviceDriverSet_ULTRASONIC_Get(&ULTRASONIC_Get /*out*/);
if (false == function_xxx(ULTRASONIC_Get, 0, 20)) //There is no obstacle 20 cm ahead?
{
ApplicationFunctionSet_SmartRobotCarMotionControl(stop_it, 0);
static unsigned long time_Servo = 0;
static uint8_t Position_Servo_xx = 0;
if (timestamp == 3)
{
if (Position_Servo_xx != Position_Servo) //Act on servo motor：avoid loop execution
{
Position_Servo_xx = Position_Servo; //Act on servo motor：rotation angle record
if (Position_Servo == 1)
{
time_Servo = millis();
AppServo.DeviceDriverSet_Servo_control(80 /*Position_angle*/);
}
else if (Position_Servo == 2)
{
time_Servo = millis();
AppServo.DeviceDriverSet_Servo_control(20 /*Position_angle*/);
}
else if (Position_Servo == 3)
{
time_Servo = millis();
AppServo.DeviceDriverSet_Servo_control(80 /*Position_angle*/);
}
else if (Position_Servo == 4)
{
time_Servo = millis();
AppServo.DeviceDriverSet_Servo_control(150 /*Position_angle*/);
}
}
}
else
{
if (timestamp == 1)
{
timestamp = 2;
time_Servo = millis();
}
}
if (millis() - time_Servo > 1000) //Act on servo motor：stop at the current location for 2s
{
timestamp = 3;
Position_Servo += 1;
OneCycle += 1;
if (OneCycle > 4)
{
Position_Servo = 1;
OneCycle = 5;
}
}
}
else
{
OneCycle = 1;
timestamp = 1;
if ((Position_Servo == 1))
{ /*Move forward*/
ApplicationFunctionSet_SmartRobotCarMotionControl(Forward, 100);
}
else if ((Position_Servo == 2))
{ /*Turn right*/
ApplicationFunctionSet_SmartRobotCarMotionControl(Right, 150);
}
else if ((Position_Servo == 3))
{
/*Move forward*/
ApplicationFunctionSet_SmartRobotCarMotionControl(Forward, 100);
}
else if ((Position_Servo == 4))
{ /*Turn left*/
ApplicationFunctionSet_SmartRobotCarMotionControl(Left, 150);
}
}
}
else
{
ULTRASONIC_Get = 0;
ULTRASONIC_time = 0;
}
}
why is my robot twitching forward if theres no object in front in 1 sec interval? Also the robot is not sweeping its ultrasonic sensor like it should? I literally download the original code from the original form and uploaded it to fix the issue? still does the same thing with no files changed. ELEGOO Smart Robot Car Kit V4.0 Tutorial – ELEGOO Official