-
Notifications
You must be signed in to change notification settings - Fork 0
/
config-app.py
executable file
·118 lines (103 loc) · 3.41 KB
/
config-app.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
#!/usr/bin/python
# -*- coding: utf-8 -*-
import serial
import sys
import array
cfgs = {
"left-kp": [0, 2, 256.0, 0.0, 256.0, lambda _: True],
"left-ki": [1, 2, 256.0, 0.0, 256.0, lambda _: True],
"left-kd": [2, 2, 256.0, 0.0, 256.0, lambda _: True],
"right-kp": [3, 2, 256.0, 0.0, 256.0, lambda _: True],
"right-ki": [4, 2, 256.0, 0.0, 256.0, lambda _: True],
"right-kd": [5, 2, 256.0, 0.0, 256.0, lambda _: True],
"enc-frames": [6, 1, 1.0, 0.0, 32.0, lambda x: int(x) == x],
"recv-samples": [7, 1, 1.0, 0.0, 31.0, lambda x: int(x) == x and int(x) % 2 == 1],
"left-reverse": [8, 1, 1.0, 0.0, 1.0, lambda x: int(x) == x],
"right-reverse": [9, 1, 1.0, 0.0, 1.0, lambda x: int(x) == x],
"esc-reverse": [10, 1, 1.0, 0.0, 1.0, lambda x: int(x) == x],
"esc-calibration-mode": [11, 1, 1.0, 0.0, 1.0, lambda x: int(x) == x]
}
write_offset = 0x30
ack = 0xac
error_offset = 0xe0
errors = [
"RAW: Comando inválido!",
"RAW: Variável inválida!",
"RAW: Variável de leitura inválida!",
"RAW: Parâmetros inválidos!",
"RAW: Buffer muito grande!",
]
def comm(ser, cmd):
cmd.insert(0, len(cmd))
ser.write(array.array('B', cmd).tostring())
req = ser.read()
if req == '':
raise serial.SerialException("timeout occured!")
sz = ord(req)
return map(ord, ser.read(sz))
def bytestoint(arr, sz):
arr.reverse()
num = 0
for v in arr[:sz]:
num = (num * 256) + v
return num
def inttobytes(num, sz):
arr = []
while sz > 0:
arr.append(num % 256)
num = int(num/256)
sz -= 1
return arr
#---------------------------------------------------------------------------------------------------------------
port = sys.argv[1]
baud = 19200
if len(sys.argv) < 2:
print "Uso:", sys.argv[0], "<port>"
sys.exit(-1)
try:
with serial.Serial(port, baud, timeout=2.0) as ser:
ser.write('\x55')
while ser.read() != '\xac':
ser.write('\x55')
print "RoboIME Sanhaço - plataforma de configuração."
while True:
cmd = map(lambda x: x.lower(), filter(bool, raw_input("Entre o comando > ").split(' ')))
if len(cmd) >= 2 and cmd[0] == "read":
if cfgs.has_key(cmd[1]):
cfg = cfgs[cmd[1]]
rep = comm(ser, [cfg[0]])
if len(rep) >= 1 + cfg[1] and rep[0] == ack:
param = bytestoint(rep[1:], cfg[1]) / cfg[2]
print "Parâmetro", cmd[1], ':', param
else:
print "Erro na leitura!"
else:
print "Parâmetro inválido!"
elif len(cmd) >= 3 and cmd[0] == "write":
if cfgs.has_key(cmd[1]):
cfg = cfgs[cmd[1]]
param = float(cmd[2])
if param >= cfg[3] and param <= cfg[4] and cfg[5](param):
rep = comm(ser, [write_offset | cfg[0]]
+ inttobytes(int(param * cfg[2]), cfg[1]))
if len(rep) >= 1 and rep[0] == ack:
print "Escrita efetuada com sucesso!"
else:
print "Erro na escrita!"
else:
print "Parâmetro fora da faixa!"
else:
print "Parâmetro inválido!"
elif len(cmd) >= 1 and cmd[0] == "finish":
print "Finalizando modo de configuração! Reiniciando uC!"
comm(ser, [0xff])
break
elif len(cmd) >= 1 and cmd[0] == "raw":
resp = comm(ser, map(lambda x: int(x, 16), cmd[1:]))
line = ' '.join(map(lambda x: format(x, 'x'), resp))
print "Resposta ao comando puro:", line
else:
print "Comando inválido!"
except serial.SerialException as e:
print "An error occured:", e
sys.exit(-1)