From a487ab4af644a92806a0d974423a34bba465f2d5 Mon Sep 17 00:00:00 2001
From: "lxbpxylps@126.com" <lxbpxylps@126.com>
Date: Mon, 15 Feb 2021 11:45:13 +0800
Subject: [PATCH] =?UTF-8?q?=E4=B8=BA=20move=20sensor=20servo=20=E5=BA=93?=
 =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=B0=83=E8=AF=95=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 TaiChi/moveTaiChi.cpp  | 49 +++++++++++++++++++++++++++++++++++++++++-
 TaiChi/moveTaiChi.h    |  5 +++++
 TaiChi/sensorTaiChi.h  |  3 +++
 TaiChi/servoTaiChi.cpp | 20 +++++++++++++++++
 TaiChi/servoTaiChi.h   |  4 ++++
 5 files changed, 80 insertions(+), 1 deletion(-)

diff --git a/TaiChi/moveTaiChi.cpp b/TaiChi/moveTaiChi.cpp
index b262143..5e1f59c 100644
--- a/TaiChi/moveTaiChi.cpp
+++ b/TaiChi/moveTaiChi.cpp
@@ -108,6 +108,13 @@ void Move::Forward(float speed_rate)
 
     current_move = FORWARD;
     current_speed_rate = speed_rate;
+
+    #ifdef DEBUG
+    //调试输出前进状态
+    Serial.print("Move Forward");
+    Serial.print(" speed_rate: ");
+    Serial.println(speed_rate);
+    #endif
 }
 
 
@@ -121,6 +128,13 @@ void Move::Backward(float speed_rate)
 
     current_move = BACKWARD;    
     current_speed_rate = speed_rate;
+
+    #ifdef DEBUG
+    //调试输出后退状态
+    Serial.print("Move Backward");
+    Serial.print(" speed_rate: ");
+    Serial.println(speed_rate);
+    #endif
 }
 
 
@@ -134,6 +148,13 @@ void Move::ForLeftward(float speed_rate, float turn_speed_rate)
 
     current_move = FORLEFTWARD;
     current_speed_rate = speed_rate;
+
+    #ifdef DEBUG
+    //调试输出向前左转状态
+    Serial.print("Move ForLeftward");
+    Serial.print(" speed_rate: ");
+    Serial.println(speed_rate);
+    #endif
 }
 
 
@@ -147,6 +168,13 @@ void Move::ForRightward(float speed_rate, float turn_speed_rate)
 
     current_move = FORRIGHTWARD;   
     current_speed_rate = speed_rate;
+
+    #ifdef DEBUG
+    //调试输出向前右转状态
+    Serial.print("Move ForRightward");
+    Serial.print(" speed_rate: ");
+    Serial.println(speed_rate);
+    #endif
 }
 
 
@@ -160,6 +188,13 @@ void Move::BackLeftward(float speed_rate, float turn_speed_rate)
 
     current_move = BACKLEFTWARD;    
     current_speed_rate = speed_rate;
+
+    #ifdef DEBUG
+    //调试输出向后左转状态
+    Serial.print("Move BackLeftward");
+    Serial.print(" speed_rate: ");
+    Serial.println(speed_rate);
+    #endif
 }
 
 
@@ -172,7 +207,14 @@ void Move::BackRightward(float speed_rate, float turn_speed_rate)
     Wheel(RIGHT_B_WHEEL, BACKWARD_ROTATION, turn_speed_rate * speed_rate);
 
     current_move = BACKRIGHTWARD;     
-    current_speed_rate = speed_rate;    
+    current_speed_rate = speed_rate;
+
+    #ifdef DEBUG
+    //调试输出向后右转状态
+    Serial.print("Move BackRightward");
+    Serial.print(" speed_rate: ");
+    Serial.println(speed_rate);
+    #endif
 }
 
 
@@ -186,6 +228,11 @@ void Move::Stop(void)
 
     current_move = STOP;
     current_speed_rate = 0;
+
+    #ifdef DEBUG
+    //调试输出制动状态
+    Serial.print("Move Stop\n");
+    #endif
 }
 
 
diff --git a/TaiChi/moveTaiChi.h b/TaiChi/moveTaiChi.h
index c61c191..8945977 100644
--- a/TaiChi/moveTaiChi.h
+++ b/TaiChi/moveTaiChi.h
@@ -1,6 +1,11 @@
 #ifndef MOVETAICHI_H
 #define MOVETAICHI_H
 
+
+//注释以关闭调试功能
+#define DEBUG
+
+
 //轮胎定义
 #define LEFT_A_WHEEL 0
 #define LEFT_B_WHEEL 1
diff --git a/TaiChi/sensorTaiChi.h b/TaiChi/sensorTaiChi.h
index ac3ce84..496bf5c 100644
--- a/TaiChi/sensorTaiChi.h
+++ b/TaiChi/sensorTaiChi.h
@@ -1,8 +1,11 @@
 #ifndef SENSORTAICHI_H
 #define SENSORTAICHI_H
 
+
+//注释以关闭调试功能
 #define DEBUG
 
+
 //灰度传感器接口定义
 #define GRAY_1_OUT A0
 #define GRAY_2_OUT A1
diff --git a/TaiChi/servoTaiChi.cpp b/TaiChi/servoTaiChi.cpp
index 848754c..a8c33c8 100644
--- a/TaiChi/servoTaiChi.cpp
+++ b/TaiChi/servoTaiChi.cpp
@@ -37,7 +37,14 @@ void Servo::RunActionGroup(uint8_t action_num, uint16_t times)
     buf[4] = action_num;          //填充要运行的动作组号
     buf[5] = GET_LOW_BYTE(times);  //取得要运行次数的低八位
     buf[6] = GET_HIGH_BYTE(times); //取得要运行次数的高八位
+
     SerialX->write(buf, 7);        //发送数据帧
+
+    #ifdef DEBUG
+    //调试输出动作组执行信息
+    Serial.print("RunServoActionGroup: ");
+    Serial.println((int)action_num);
+    #endif
 }
 
 
@@ -51,6 +58,11 @@ void Servo::StopActionGroup(void)
     buf[3] = CMD_ACTION_GROUP_STOP; //填充停止运行动作组命令
 
     SerialX->write(buf, 4); //发送数据帧
+
+    #ifdef DEBUG
+    //调试输出动作组停止信息
+    Serial.print("StopServoActionGroup\n");
+    #endif
 }
 
 
@@ -69,6 +81,14 @@ void Servo::SetActionGroupSpeed(uint8_t action_num, float speed)
     buf[6] = GET_HIGH_BYTE(speed_int);   //获得目标熟读的高八位
 
     SerialX->write(buf, 7); //发送数据帧
+
+    #ifdef DEBUG
+    //调试输出动作组速度设定信息
+    Serial.print("SetServoActionGroupSpeed: ");
+    Serial.println((int)action_num);
+    Serial.print(" Speed: ");
+    Serial.println(speed);
+    #endif
 }
 
 
diff --git a/TaiChi/servoTaiChi.h b/TaiChi/servoTaiChi.h
index d5aa08d..23b9373 100644
--- a/TaiChi/servoTaiChi.h
+++ b/TaiChi/servoTaiChi.h
@@ -5,6 +5,10 @@
 #include <Arduino.h>
 
 
+//注释以关闭调试功能
+#define DEBUG
+
+
 #define SERVO_BAUD_RATE 9600