servo 库新增控制单个舵机转动的功能

TaiChi/servoTaiChi.cpp

@@ -11,7 +11,7 @@
 
 Servo::Servo()
 {
-    SerialX = &Serial1; //默认使用 Mega 板 18 19 作为串口通信端口
+    SerialX = &DEFAULT_SERVO_SERIAL_NUM; //默认使用 Mega 板 18 19 作为串口通信端口
     SerialX->begin(SERVO_BAUD_RATE);
 }
 
@@ -24,6 +24,33 @@ Servo::Servo(HardwareSerial& serial_num)
 }
 
 
+//控制单个舵机转动
+//servo_id: 舵机ID, position: 目标位置, time: 转动时间
+void Servo::MoveServo(uint8_t servo_id, uint16_t position, uint16_t time)
+{
+	uint8_t buf[11];
+	buf[0] = FRAME_HEADER;                   //填充帧头
+	buf[1] = FRAME_HEADER;
+	buf[2] = 8;                              //数据长度=要控制舵机数*3+5，此处=1*3+5
+	buf[3] = CMD_SERVO_MOVE;                 //填充舵机移动指令
+	buf[4] = 1;                              //要控制的舵机个数
+	buf[5] = GET_LOW_BYTE(time);             //填充时间的低八位
+	buf[6] = GET_HIGH_BYTE(time);            //填充时间的高八位
+	buf[7] = servo_id;                        //舵机ID
+	buf[8] = GET_LOW_BYTE(position);         //填充目标位置的低八位
+	buf[9] = GET_HIGH_BYTE(position);        //填充目标位置的高八位
+
+	SerialX->write(buf, 10);
+
+    #ifdef SERVO_DEBUG
+    //调试输出动作组执行信息
+    Serial.print("#SERVO:  MoveServo: "); Serial.print((int)servo_id); 
+    Serial.print(" position: "); Serial.print((int)position); 
+    Serial.print(" time: "); Serial.println((int)time);
+    #endif
+}
+
+
 //运行指定动作组
 //action_num: 动作组序号, times: 执行次数
 //times = 0 时无限循环
@@ -100,6 +127,13 @@ void Servo::SetAllActionGroupSpeed(float speed)
 }
 
 
+//打开爪子
+void Servo::OpenClaw(void) 
+{
+    MoveServo(CLAW_SERVO_ID, CLAW_OPEN_POSITION, CLAW_OPEN_USE_TIME);
+}
+
+
 //恢复初始状态，指定速度
 void Servo::Reset(float speed)
 {

TaiChi/servoTaiChi.h

@@ -9,11 +9,26 @@
 #define SERVO_DEBUG
 
 
+//默认与舵机控制板连接串口
+//默认使用 Mega 板 18 19 作为串口通信端口
+#define DEFAULT_SERVO_SERIAL_NUM Serial1
+
+
+//与舵机控制板串口通信波特率
 #define SERVO_BAUD_RATE 9600
 
 
+//爪子舵机 id
+#define CLAW_SERVO_ID 15
+//爪子打开时舵机位置
+#define CLAW_OPEN_POSITION 500
+//爪子打开用时
+#define CLAW_OPEN_USE_TIME 1000
+
+
 //发送部分的指令
 #define FRAME_HEADER 0x55            //帧头
+#define CMD_SERVO_MOVE 0x03          //舵机移动指令
 #define CMD_ACTION_GROUP_RUN 0x06    //运行动作组指令
 #define CMD_ACTION_GROUP_STOP 0x07   //停止动作组运行指令
 #define CMD_ACTION_GROUP_SPEED 0x0B  //设置动作组运行速度指令
@@ -35,11 +50,15 @@ public:
     Servo();
     Servo(HardwareSerial &serial_num); //构造函数，指定串口通信端口
 
+    void MoveServo(uint8_t servo_id, uint16_t position, uint16_t time); //控制单个舵机转动
+
     void RunActionGroup(uint8_t action_num, uint16_t times); //运行指定动作组
     void StopActionGroup(void); //停止动作组运行
     void SetActionGroupSpeed(uint8_t action_num, float speed); //设定指定动作组的运行速度
     void SetAllActionGroupSpeed(float speed); //设置所有动作组的运行速度
 
+    void OpenClaw(void); //打开爪子
+
     void Reset(float speed = SERVO_NORMAL_SPEED); //恢复初始状态，指定速度
     void Catch(float speed = SERVO_NORMAL_SPEED); //抓取，指定速度
     void Release(float speed = SERVO_NORMAL_SPEED); //释放，指定速度