Four-legged-robot/Serial_Servo_Running.py at main · Rongkangqi/Four-legged-robot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
#!/usr/bin/env python3
# encoding: utf-8
import time
import os
import sqlite3 as sql
import SerialServoCmd as ssc
import config_serial_servo
import threading


runningAction = False
stopRunning = False


def serial_setServo(s_id, pos, s_time):
    if pos > 1000:
        pos = 1000
    elif pos < 0:
        pos = 0
    else:
        pass
    if s_time > 30000:
        s_time = 30000
    elif s_time < 10:
        s_time = 10
    ssc.serial_serro_wirte_cmd(s_id, ssc.LOBOT_SERVO_MOVE_TIME_WRITE, pos, s_time)


def setDeviation(servoId, d):
    '''
    配置舵机偏差
    :param servoId:
    :param d:
    :return:
    '''
    global runningAction
    if servoId < 1 or servoId > 13:
        return
    if d < -200 or d > 200:
        return
    if runningAction is False:
        config_serial_servo.serial_servo_set_deviation(servoId, d)


def stop_action_group():
    global stopRunning
    stopRunning = True


def running_action_group(actNum, times):
    '''
    运行动作组，无法发送stop停止信号
    :param actNum: 动作组名字 ， 字符串类型
    :param times:  运行次数
    :return:
    '''
    global runningAction
    global stopRunning
    actNum = "/home/pi/AlienbotPi/ActionGroups/" + actNum + ".d6a"
    while times:    # 运行次数
        if os.path.exists(actNum) is True:
            ag = sql.connect(actNum)
            cu = ag.cursor()
            cu.execute("select * from ActionGroup")
            if runningAction is False:
                runningAction = True
                while True:
                    act = cu.fetchone()
                    if stopRunning is True:
                        print('stop')
                        stopRunning = False
                        runningAction = False
                        cu.close()
                        ag.close()
                        break
                    if act is not None:
                        for i in range(0, len(act)-2, 1):
                                serial_setServo(i+1, act[2 + i], act[1])
                        time.sleep(float(act[1])/1000.0)
                    else:
                        runningAction = False
                        cu.close()
                        ag.close()
                        break
        else:
            runningAction = False
            print("未能找到动作组文件")
        times -= 1


def thread_runActing(actnum, times):
    '''
    线程运行动作，可以发送stop停止
    :param actnum:
    :param times:
    :return:
    '''
    try:
        threading.Thread(target=running_action_group, args=(actnum, times)).start()
    except Exception as e:
        print(e)


def stop_servo():
    print("停止")
    for i in range(8):
        config_serial_servo.serial_servo_stop(i+1)