add a dance demo

hotwheelspkg/scripts/demo.py

+from manager import ServoManager
+
+#!/usr/bin/env python
+from cgitb import reset
+import time
+import math
+import sys
+sys.path.append("/home/yyf/ros03/src/hotwheelspkg/scripts")#导入模块，搜索当前目录、已安装内置模块、第三方模块
+from tracemalloc import start
+from uservo import serial, UartServoManager
+from FileManager import *
+import rospy
+from std_msgs.msg import String
+import numpy as np
+#from calculate import calc_angleup, calc_angledown, calc_posup, calc_posdown
+
+# UART
+SERVO_PORT_NAME =  '/dev/ttyAMA0'		# 舵机串口号
+SERVO_BAUDRATE = 115200			# 舵机的波特率
+# 初始化串口
+uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,
+				parity=serial.PARITY_NONE, stopbits=1,
+				bytesize=8,timeout=0)
+uservo = UartServoManager(uart)
+filefolder = dict()
+
+def response(msg):
+    cmd = msg.data
+    exec(cmd)
+
+# define servo angles matrix
+servo_angles = np.array([
+    [115,98,172,147,8,59,139,106,-10,81,56,78,-35,-167,77],
+    [115,140,172,147,20,68,123,106,-10,80,56,78,-35,-167,77],
+    [115,106,171,147,20,68,135,88,3,81,48,86,-33,-167,77],
+    [115,146,178,113,60,43,164,95,3,81,48,87,-33,-167,77],
+    [115,169,164,26,108,42,49,104,-7,80,51,86,-33,-168,76],
+    [115,169,164,147,108,-7,-3,104,-7,80,51,86,-33,-168,76],
+    [115,146,178,147,16,53,165,121,-19,80,44,97,-34,-168,76],
+    [115,154,112,126,17,58,10,121,-24,80,44,95,-36,-167,76],
+    [115,154,167,112,-28,62,151,126,-23,80,47,97,-42,-165,76],
+    [115,198,118,71,0,62,10,126,-23,80,47,97,-42,-165,76],
+    [115,154,167,126,-24,62,150,129,-32,80,47,93,-42,-165,76],
+    [115,120,161,169,-6,38,150,129,-33,80,47,93,-42,-165,76],
+    [115,189,136,126,106,38,74,132,-45,81,85,43,-36,-165,77]
+])
+
+# main程序
+if __name__ == "__main__":
+
+    manager = ServoManager()
+
+    for i in range(len(servo_angles)):
+        for j in range(len(servo_angles[i])):
+            manager.SettingSingleAngle(j+1,servo_angles[i][j],0.1)
+            time.sleep(0.05)
+    #    print(i+1)
+    #    time.sleep(1)
+    # manager.SettingSingleAngle(4,100,1) 
+    # for j in range(len(servo_angles[10])):
+    #     manager.SettingSingleAngle(j+1,servo_angles[11][j],1)  
+    #     time.sleep(0.1)
+    manager.ShowServoState()

hotwheelspkg/scripts/manager.py

@@ -200,7 +200,7 @@ class ServoManager:
         uservo.set_damping(servo_index)
     def DampAll(self):
         print(uservo.servos)
-        for servo_index in range(1, self.servo_num -7)                                                                                                                        :
+        for servo_index in range(1, self.servo_num -1)                                                                                                                        :
             uservo.set_damping(servo_index)
             time.sleep(0.1)