机器人网络控制指令

最终的舵机控制是通过网络，这一部分就是开启一个服务器，通过一个线程去监听服务，然后通过数据处理类来处理接收的命令。
关于这一部分在LeServer中

一、网络处理类

指令格式参考
指令格式

指令格式

1.I001 运行动作

I001-500-16-1-500-2-388-3-500-4-594-5-500-6-575-7-800-8-725-9-500-10-612-11-500-12-406-13-500-14-425-15-200-16-275

执行这个命令，机器人会立正

2.I002 停止动作组

I002

3.I003 运行动作组

I003-1-1

运行前进动作，执行一次

所有的动作组

TonyPi动作组文件命名		
0	立正	
1	前进	
2	后退	
3	左转	
4	右转	
7	俯卧撑	
8	仰卧起坐	
9	挥手	
10	鞠躬	
11	左侧滑	左小迈步
12	右侧滑	右小迈步
14	下蹲	
15	开怀大笑	
30	右弓步	
31	左弓步	
32	右勾拳	
33	左勾拳	
34	左右直拳	
35	下蹲拳	
36	咏春拳	
37	捶胸	
50	左侧踢	
51	右侧踢	
52	左脚射门	
53	右脚射门	
60	拍照
101	前跌倒起立	
102	后跌倒起立	
go	前进	
go_fast	快速前进	
hand5	左右摇摆	
left	左侧滑	
left_move	左小迈步	
left_move_one_step	左迈一步		
pick	抱住	
pickTostand	松手	
right	右侧滑	
right_move	右小迈步	
right_move_one_step	右迈一步	
shot_left	左脚射门	
shot_right	右脚射门	
stand	站立	
stand_	半蹲
stand_gtolr	向左转	
stand_lrtog	站立	
stand_slow	站立(缓慢)	
stoop	向前俯身	

4.I004 查询动作组

I004

返回所有动作组
上位机接收的

这个乱码是一个中文，没有打印出来，可参照下面的
树莓派打印的

5.I007 设置任意舵机位置

可以直接执行I001指令，改一下指令即可
只修改一个舵机

I007-500-1-14-200

这里14号舵机是右手的那个舵机，值越大，越往外，越小，越往里

修改两个舵机

I007-500-2-6-200-14-800

修改两个舵机，6号舵机是左手那个舵机，值越大，越往里，越小，越往外 14号舵机正好相反。

其他指令可根据命令执行填写。

二、程序代码

# 控制服务
#!/usr/bin/python3
# encoding: utf-8

import socketserver  # 导入网络服务
import threading     # 导入线程
import time          # 导入时间
import PWMServo      # 导入PWM驱动舵机
import re            # 导入正则表达式
from config import * # 导入配置文件
from LeCmd import LeError # 导入异常错误类
import LeCmd # 导入控制命令
import sys #  系统
import Serial_Servo_Running # 串口舵机运行


# socketserver 数据处理类  继承BaseRequestHandler
class ServoServer(socketserver.BaseRequestHandler):
    Flag = True  #  循环标志
    
    # 数据处理函数
    def handle(self):
        print("已连接")
        conn = self.request # 获取请求连接对象
        recv = b''
        recv_data = "" 
        while self.Flag:
            try:
                recv = conn.recv(1024) # 接收1024个字节
                #print('recv:',recv)
                recv_data = recv.decode() # 编码
                #print('recv_data', recv_data)
                #print('***********')
                if not recv_data:  # 没有接受到数据
                    self.Flag = False  # 结束循环
                    print("没有接收到数据")   # 输出没有接收到字符
                    break
                # cmd_i00x
                rdata = recv_data.split("\r\n")  # # 按换行符对接收的数据进行分割  
                # ^ 是限定开头的意思，就是说如果后面的字符是在开头出现，就匹配，不再开头就不匹配
                # （I[0-9]{3}）这是说I后面有3个数字
                # ^(I[0-9]{3}) 是说在开头是I后面加3个数字               
                rex = re.compile(r'^(I[0-9]{3}).*')  # 判断收到的指令是否符合规则
                data = rdata[0]  # 分离出的第一个字符串
                match = data
                match = rex.match(match) # 看是否有上面匹配的字符串 有获得匹配结果，没有返回none
                if match:  # 有匹配结果
                # match.start() 获取匹配字符串在原字符串的起始和结束位置 
                    if not 0 == match.start() or not len(data) == match.end(): # 匹配的是原字符串的子集
                        print("错误指令 1")
                    else:
                        print ('data', data) # 输出data
                        data = data.split('-')  # 对匹配的字符串再按-分割
                        cmd = data[0][1:5] # 取分割后的第一个字串的第2345个字符
                        del data[0] # 删除data[0]
                        par = [] # 创建一个列表
                        try:
                            cmd = int(cmd) # 转换cmd为整形 但cmd可能有非数字
                            # print ('cmd', cmd)
                            if 3 <= cmd <= 9:
                                # print(rdata)
                                LeCmd.cmd_list[cmd](conn, data)
                            else:
                                for p in data:
                                    par.append(int(p))
                                # print ('par', par)
                                LeCmd.cmd_list[cmd](par)
                        except LeError as err:
                            print(err.msg) # 打印错误信息
                            print(err.data)  # 打印错误数据
                else:  # 没有匹配结果
                    #print(data)
                    print("错误指令 2")
                if not self.Flag: # Flag = false
                    print("break1")
                    break
            except Exception as e:
                print(e) # 输入异常信息
                break

    def finish(self):
        print("已断开")


class LeServer(socketserver.ThreadingTCPServer):
    allow_reuse_address = True

if __name__ == "__main__":
    PWMServo.setServo(1, 1500, 500) # 设置PWm舵机复位
    PWMServo.setServo(2, 1500, 500)
    # 时间  舵机数目  id1 pos1 id2 pos2
    LeCmd.cmd_i001([500, 16, 1, 500, 2, 388, 3, 500, 4, 594, 5, 500, 6, 575, 7, 800, 8, 725, 9, 500, 10, 612, 11, 500,
                    12, 406, 13, 500, 14, 425, 15, 200, 16, 275])    # 立正
    time.sleep(0.5)
    server = LeServer(("", 1075), ServoServer) # 开启服务器
    try:
        server_thread = threading.Thread(target=server.serve_forever) # 线程监听
        server_thread.start()
        while True:
            time.sleep(0.1)
    except:
        server.shutdown()
        server.server_close()

三、添加为开机自启

1.建服务文件

sudo nano /etc/systemd/system/LeServer.service

2.添加内容

[Unit]
Description = run LeServer
After = rc-local.service

[Service]
Type = simple
User = root
Group = root
WorkingDirectory = /home/pi/Desktop/Serial_SteerEngine
ExecStart = /usr/bin/sudo python3 LeServer.py
Restart = always

[Install]
WantedBy = multi-user.target

3.设为开机自启

sudo systemctl enable LeServer.service

4.重启