-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathSerial_Servo_Running.py
153 lines (139 loc) · 4.81 KB
/
Serial_Servo_Running.py
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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
#!/usr/bin/env python3
# encoding: utf-8
import os
import time
import threading
import sqlite3 as sql
from hwax import HWAX
import BusServoCmd as BSC
from Board import setBusServoPulse
runningAction = False
stop_action = False
stop_action_group = False
def stopAction():
global stop_action
stop_action = True
def stopActionGroup():
global stop_action_group
stop_action_group = True
__end = False
__start = True
current_status = ''
def runActionGroup(actName, times=1, with_stand=False):
global __end
global __start
global current_status
global stop_action_group
temp = times
while True:
if temp != 0:
times -= 1
try:
if (actName != 'go_forward' and actName != 'go_forward_fast' and actName != 'back' and actName != 'back_fast') or stop_action_group:
if __end:
__end = False
if current_status == 'go':
runAction('go_forward_end')
else:
runAction('back_end')
#print('end2')
if stop_action_group:
__end = False
__start = True
stop_action_group = False
#print('stop_action_group')
break
__start = True
if times < 0:
__end = False
__start = True
stop_action_group = False
break
runAction(actName)
else:
if times < 0:
#print('end1')
if with_stand:
if actName == 'go_forward' or actName == 'go_forward_fast':
runAction('go_forward_end')
else:
runAction('back_end')
break
if __start:
__start = False
__end = True
#print('start')
if actName == 'go_forward':
runAction('go_forward_start')
current_status = 'go'
elif actName == 'go_forward_fast':
runAction('go_forward_start_fast')
current_status = 'go'
elif actName == 'back':
runAction('back_start')
runAction('back')
current_status = 'back'
elif actName == 'back_fast':
runAction('back_start')
runAction('back_fast')
current_status = 'back'
else:
runAction(actName)
except BaseException as e:
print(e)
def runAction(actNum):
'''
运行动作组，无法发送stop停止信号
:param actNum: 动作组名字 ， 字符串类型
:param times: 运行次数
:return:
'''
global runningAction
global stop_action
if actNum is None:
return
hwaxNum = "/home/pi/human_code/ActionGroups/" + actNum + ".hwax"
actNum = "/home/pi/human_code/ActionGroups/" + actNum + ".d6a"
if os.path.exists(hwaxNum) is True:
if runningAction is False:
runningAction = True
BSC.portWrite()
hwax = HWAX(hwaxNum, BSC.serialHandle)
hwax.reset()
while True:
if stop_action is True:
stop_action = False
print('stop')
break
ret = hwax.next()
if ret is None:
hwax.reset()
break
hwax.close()
runningAction = False
elif os.path.exists(actNum) is True:
if runningAction is False:
runningAction = True
ag = sql.connect(actNum)
cu = ag.cursor()
cu.execute("select * from ActionGroup")
while True:
act = cu.fetchone()
if stop_action is True:
stop_action = False
print('stop')
break
if act is not None:
for i in range(0, len(act) - 2, 1):
setBusServoPulse(i + 1, act[2 + i], act[1])
time.sleep(float(act[1])/1000.0)
else: # 运行完才退出
break
runningAction = False
cu.close()
ag.close()
else:
runningAction = False
print("未能找到动作组文件")
if __name__ == '__main__':
runActionGroup('go_forward', 1)