From 2efd7a390bc734092b325129055594fa64ea6358 Mon Sep 17 00:00:00 2001
From: "lxbpxylps@126.com" <lxbpxylps@126.com>
Date: Mon, 15 Feb 2021 10:58:46 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E9=83=A8=E5=88=86=E9=94=99?=
 =?UTF-8?q?=E8=AF=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 TaiChi/TaiChi.ino | 24 ++++++++++--------------
 1 file changed, 10 insertions(+), 14 deletions(-)

diff --git a/TaiChi/TaiChi.ino b/TaiChi/TaiChi.ino
index 24f12dd..3c0dd77 100644
--- a/TaiChi/TaiChi.ino
+++ b/TaiChi/TaiChi.ino
@@ -79,7 +79,7 @@ uint8_t next_position = FRONT_NEXT;
 
 
 //****************************************自定函数****************************************
-//计算方向
+//计算方向，同时更改完成转向后相对下一点的朝向
 uint8_t CalcDirection(void);
 
 #define FRONT_END 0
@@ -116,11 +116,9 @@ void setup()
 void loop()
 {
     //情况一：刚完整经过普通点，下一个点为普通点
-    if (route[passed_flag][TYPE] == NORMAL_POINT && route[passed_flag + 1][TYPE] == NORMAL_POINT)   
+    if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == NORMAL_POINT)   
     {
-        uint8_t direction = CalcDirection();
-
-        if (direction == FORWARD || direction == FORLEFTWARD || direction == FORRIGHTWARD)
+        if (next_position == FRONT_NEXT)
         {
             //沿线直行，到前端传感器接触下一条线为止
             LineForward(FRONT_END);
@@ -132,10 +130,10 @@ void loop()
         }
 
         //继续直行或后退或转向
-        TurnDirection(direction);
+        TurnDirection(CalcDirection());
     }
     //情况二：刚完整经过普通点，下一个点为抓取点
-    else if (route[passed_flag][TYPE] == NORMAL_POINT && route[passed_flag + 1][TYPE] == CATCH_POINT)
+    else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == CATCH_POINT)
     {
         //沿线直行，在抓取位置停止
         LineForward(CATCH_END);
@@ -154,7 +152,7 @@ void loop()
         route[passed_flag][TYPE] = NORMAL_POINT;
     }
     //情况三：刚完整经过普通点，下一个点为释放点
-    else if (route[passed_flag][TYPE] == NORMAL_POINT && route[passed_flag + 1][TYPE] == RELEASE_POINT)
+    else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == RELEASE_POINT)
     {
         //沿线直行，在释放位置停止
         LineForward(RELEASE_END);
@@ -167,10 +165,8 @@ void loop()
         next_position = BACK_NEXT;
     }
     //情况四：刚完整经过释放点，下一个点为普通点
-    else if (route[passed_flag][TYPE] == RELEASE_POINT && route[passed_flag + 1][TYPE] == NORMAL_POINT)
+    else if (route[passed_flag][TYPE] == RELEASE_POINT && route[next_flag][TYPE] == NORMAL_POINT)
     {
-        uint8_t direction = CalcDirection();
-        
         //沿线后退，到后端传感器接触下一条线为止
         LineBackward(BACK_END);
 
@@ -179,7 +175,7 @@ void loop()
         delay(RESET_DELAY_TIME); //复原留时
 
         //继续后退或转向
-        TurnDirection(direction);
+        TurnDirection(CalcDirection());
     }
     else move.Stop(); //DEBUG
 
@@ -195,7 +191,7 @@ void loop()
 }
 
 
-//计算方向
+//计算方向，同时更改完成转向后相对下一点的朝向
 uint8_t CalcDirection(void)
 {
     //计算第三点与第一点的相对坐标 rx0, ry0
@@ -287,7 +283,7 @@ uint8_t CalcDirection(void)
 void LineForward(uint8_t end_position, double speed_rate)
 {
     //记录开始时间
-    int begin_time = micros();
+    unsigned long begin_time = micros();
 
     while(1)
     {