void obstacle_dodge()
{
while (!check_command(RETURNTOMAIN))
{
if(!Object(front2, DISTANCE_WALL))
Motors_Forward(PWM, PWM);
else
while (Object(front1, DISTANCE_WALL) || Object(front2, DISTANCE_WALL) || Object(front3, DISTANCE_WALL))
{
if (milliseconds % 2 == 0)
Rotate_Left(20);
else
Rotate_Right(20);
}
}
Motors_Stop();
}
void lateral_parking()
{
time_elapsed = 0;
distance_traveled = 0;
do //Follow right wall until spot is detected
{
Wall_Follow(right, 6);
} while (Object(right, DISTANCE_WALL));
Go_Forward(27); //Go forward until at the middle of the spot
Rotate_Right(22); //Rotate towards parking spot
Go_Forward(20); //Advance inside parking spot
Rotate_Left(24); //Rotate to be parallel to road
while (front2[2] > back[2]) //The car should be closer to the car on the back
{
Ultrasonic_Scan();
Wall_Follow(right, 3); //Follow right wall so it can adjust if not centered or rotated properly
}
Motors_Stop();
}
static int Insert(BisTree *tree, BiTreeNode **node,
const void * data, int * balanced)
{
AvlNode * avl_data = NULL;
int cmpval = 0, result = 0;
//尾节点则直接插入左子树
if (BiTree_Is_Endob(*node))
{
if (NULL == (avl_data = (AvlNode*)malloc(sizeof(AvlNode))))
return ERR;
avl_data->factor = AVL_BALANCED;
avl_data->hidden = 0;
avl_data->data = (void *)data;
return BiTree_Ins_Left(tree, *node, avl_data);
}
else
{
cmpval = tree->compare(data, ((AvlNode *)BiTree_Data(*node))->data);
/*递归调用插入左子树*/
if (cmpval < 0)
{
if (BiTree_Is_Endob(BiTree_Left(*node)))
{
if (NULL == (avl_data = (AvlNode *)malloc(sizeof(AvlNode))))
return ERR;
avl_data->factor = AVL_BALANCED;
avl_data->hidden = 0;
avl_data->data = (void *)data;
if (OK != BiTree_Ins_Left(tree, *node, avl_data))
return ERR;
*balanced = 0;
}
else
{
if (OK != (result = Insert(tree, &BiTree_Left(*node), data, balanced)))
{
return result;
}
}
/*平衡调整*/
if (!(*balanced))
{
switch (((AvlNode *)BiTree_Data(*node))->factor)
{
case AVL_LFT_HEAVY:
Rotate_Left(node);
*balanced = 1;
break;
case AVL_BALANCED:
((AvlNode *)BiTree_Data(*node))->factor = AVL_LFT_HEAVY;
break;
case AVL_RGT_HEAVY:
((AvlNode *)BiTree_Data(*node))->factor = AVL_BALANCED;
*balanced = 1;
break;
}
}
}
else if (cmpval > 0)
{
/*递归调用插入右子树*/
if (BiTree_Is_Endob(BiTree_Right(*node)))
{
if (NULL == (avl_data = (AvlNode *)malloc(sizeof(AvlNode))))
return ERR;
avl_data->factor = AVL_BALANCED;
avl_data->hidden = 0;
avl_data->data = (void *)data;
if (OK != BiTree_Ins_Right(tree, *node, avl_data))
return ERR;
*balanced = 0;
}
else
{
if (OK != (result = Insert(tree, &BiTree_Right(*node), data, balanced)))
{
return result;
}
}
/*平衡调整*/
if (!(*balanced))
{
switch (((AvlNode *)BiTree_Data(*node))->factor)
{
case AVL_LFT_HEAVY:
((AvlNode *)BiTree_Data(*node))->factor = AVL_BALANCED;
*balanced = 1;
break;
case AVL_BALANCED:
((AvlNode *)BiTree_Data(*node))->factor = AVL_RGT_HEAVY;
break;
case AVL_RGT_HEAVY:
Rotate_Right(node);
*balanced = 1;
break;
}
}
}
//查询到相同节点，则替换
else
{
if (tree->destroy != NULL)
{
/*删除上次隐藏的数据 */
tree->destroy(((AvlNode*)BiTree_Data(*node))->data);
}
((AvlNode*)BiTree_Data(*node))->data = (void *)data;
((AvlNode*)BiTree_Data(*node))->hidden = 0;
/* 树平衡未被破坏 */
*balanced = 1;
}
}
return OK;
}
void random_parking()
{
time_elapsed = 0;
distance_traveled = 0;
int spot = 0;
uint16_t distance_temp;
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,20))
Motors_Forward(PWM, PWM);
while (!Object(front2,14))
Wall_Follow(left,10);
Rotate_Right(23);
//Spot 6
distance_temp = distance_traveled;
while (Object(left,30))
{
if ((distance_traveled - distance_temp >= 33) && (distance_traveled - distance_temp <= 36) && !Object(right, 40))
{
spot = 6;
break;
}
Wall_Follow(left,14);
}
//Spot 5
if (!spot)
{
distance_temp = distance_traveled;
while(!Object(left,30))
{
if ((distance_traveled - distance_temp >= 13) && (distance_traveled - distance_temp <= 16) && !Object(right, 40))
{
spot = 5;
break;
}
Motors_Forward(PWM, PWM);
}
}
//Spot 4
if (!spot)
{
while(!Object(left,30))
Motors_Forward(PWM, PWM);
distance_temp = distance_traveled;
while(!Object(front2,15))
{
if ((distance_traveled - distance_temp >= 13) && (distance_traveled - distance_temp <= 16) && !Object(right, 40))
{
spot = 4;
break;
}
Wall_Follow(left,14);
}
}
if (!spot)
{
Rotate_Right(23);
while (!Object(right,20))
Motors_Forward(PWM,PWM);
while (Object(right,20))
Wall_Follow(right,9);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
}
//Spot 3
if (!spot)
{
distance_temp = distance_traveled;
while (Object(left,30))
{
if ((distance_traveled - distance_temp >= 30) && (distance_traveled - distance_temp <= 33) && !Object(right, 40))
{
spot = 3;
break;
}
Wall_Follow(left,14);
}
}
//Spot 2
if (!spot)
{
distance_temp = distance_traveled;
while(!Object(left,30))
{
if ((distance_traveled - distance_temp >= 10) && (distance_traveled - distance_temp <= 13) && !Object(right, 40))
{
spot = 2;
break;
}
Motors_Forward(PWM, PWM);
}
}
//Spot 1
if (!spot)
{
while(!Object(left,30))
Motors_Forward(PWM, PWM);
distance_temp = distance_traveled;
while(!Object(front2,15))
{
if ((distance_traveled - distance_temp >= 10) && (distance_traveled - distance_temp <= 13) && !Object(right, 40))
{
spot = 1;
break;
}
Wall_Follow(left,14);
}
}
if (spot)
{
Rotate_Right(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,16))
Wall_Follow(right,3);
Motors_Stop();
delay_ms(200);
LEDs_Blink(12000);
switch (spot)
{
case 1:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
break;
case 2:
Go_Back(40);
Rotate_Right(46);
break;
case 3:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(14);
Rotate_Left(23);
break;
case 4:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(14);
Rotate_Right(23);
break;
case 5:
Go_Back(40);
Rotate_Right(46);
break;
case 6:
while (!Object(back,10))
Motors_Back(PWM,PWM);
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(14);
Rotate_Left(23);
break;
}
Go_Forward(50);
Motors_Stop();
}
}
void fixed_parking()
{
time_elapsed = 0;
distance_traveled = 0;
int spot = 0, exit = 0;
uint16_t distance_temp;
while (check_command(NOMESSAGE));
spot = UART_Read();
clear_last_received();
while (check_command(NOMESSAGE));
exit = UART_Read();
clear_last_received();
switch (spot)
{
case 1:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(right,12);
Rotate_Left(23);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 35)
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(left,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
while(!Object(right,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(left,10);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(right,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(12);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
case 2:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(right,12);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 10)
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(left,3);
Motors_Stop();
LEDs_Blink(12000);
Go_Back(40);
switch (exit)
{
case 1:
Rotate_Left(46);
Go_Forward(50);
break;
case 2:
Rotate_Right(23);
while(!Object(right,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(left,10);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(right,20))
Wall_Follow(right,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(12);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
case 3:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(right,12);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 10)
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(left,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 2:
Rotate_Right(23);
while(!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(left,10);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(right,30))
Wall_Follow(right,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(12);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
case 4:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(left,12);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 13)
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(right,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(left,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Left(23);
while(!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(right,10);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 3:
Rotate_Right(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(10);
Rotate_Right(23);
Go_Forward(50);
break;
}
break;
}
case 5:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(left,12);
Rotate_Right(23);
while (Object(left,30))
Wall_Follow(left,14);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 10)
Motors_Forward(PWM,PWM);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(right,3);
Motors_Stop();
LEDs_Blink(12000);
Go_Back(40);
switch (exit)
{
case 1:
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,30))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(left,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Left(23);
while(!Object(left,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(right,10);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 3:
Rotate_Right(46);
Go_Forward(50);
break;
}
break;
}
case 6:
{
while (!Object(front2,13))
Motors_Forward(PWM,PWM);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM, PWM);
while (!Object(front2,15))
Wall_Follow(left,12);
Rotate_Right(23);
distance_temp = distance_traveled;
while (distance_traveled - distance_temp < 33)
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (!Object(front2,14))
Wall_Follow(right,3);
Motors_Stop();
LEDs_Blink(12000);
while (!Object(back,10))
Motors_Back(PWM,PWM);
switch (exit)
{
case 1:
Rotate_Right(23);
while (!Object(front2,15))
Wall_Follow(right,14);
Rotate_Left(23);
while (!Object(left,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(left,10);
while (!Object(front2,15))
Motors_Forward(PWM, PWM);
Rotate_Left(23);
while (Object(right,30))
Wall_Follow(right,14);
Go_Forward(12);
Rotate_Right(23);
Go_Forward(50);
break;
case 2:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
while(!Object(left,30))
Motors_Forward(PWM, PWM);
while(!Object(front2,15))
Wall_Follow(left,14);
Rotate_Right(23);
while (!Object(right,30))
Motors_Forward(PWM,PWM);
while (Object(left,30))
Wall_Follow(right,10);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
case 3:
Rotate_Left(23);
while (Object(left,30))
Wall_Follow(left,14);
Go_Forward(10);
Rotate_Left(23);
Go_Forward(50);
break;
}
break;
}
}
Motors_Stop();
}
