为 move sensor servo 库增加调试功能

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
 }
 
 
@@ -173,6 +208,13 @@ void Move::BackRightward(float speed_rate, float turn_speed_rate)
 
     current_move = BACKRIGHTWARD;     
     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
 }
 
 

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

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

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
 }
 
 

TaiChi/servoTaiChi.h

@@ -5,6 +5,10 @@
 #include <Arduino.h>
 
 
+//注释以关闭调试功能
+#define DEBUG
+
+
 #define SERVO_BAUD_RATE 9600