Blame view
software/servo/client/servo_client
9.63 KB
14b996da3
First commit
|
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 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 |
#!/usr/bin/python3
"""
author Benoit Dubois
copyright FEMTO ENGINEERING, 2021
license GPL v3.0+
brief Basic GUI to deal with "servo2toX" of STM32 board.
Need to connect USB cable to CN5 to open a serial link with board
(usually /dev/ttyACM0).
"""
# Ctrl-c closes the application
import signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
import os
import sys
from struct import pack
import argparse
import configparser
import tkinter as tk
import serial
import threading
APPLICATION = "stm_client"
CONFIG_DIR = os.path.join(os.path.expanduser("~"), ".config/", APPLICATION)
PARAMETERS = (
{'mnemo': "kp", 'default': 0, 'min': -32768, 'max': 32768, 'format': None},
{'mnemo': "ki", 'default': 0, 'min': -32768, 'max': 32768, 'format': None},
{'mnemo': "sp", 'default': 0, 'min': 0, 'max': 4095, 'format': None},
{'mnemo': "osp", 'default': 0, 'min': -40000, 'max': 40000, 'format': None},
{'mnemo': "imax", 'default': 80000, 'min': 0, 'max': 800000, 'format': None},
{'mnemo': "omax", 'default': 40000, 'min': 0, 'max': 40000, 'format': None},
{'mnemo': "oscale", 'default': 8, 'min': 0, 'max': 20, 'format': None},
{'mnemo': "fs", 'default': 1, 'min': 1, 'max': 1000, 'format': None}
)
#==============================================================================
class ClientStmGui(tk.Frame):
"""Basic GUI to deal with "double PID" servo STM32 board.
"""
def __init__(self, master=None):
super().__init__(master)
self.cctrl = tk.IntVar()
self._build_ui()
def _build_ui(self):
lbl = (dict(), dict())
self.sbox = (dict(), dict())
self.box_val = (dict(), dict())
self.btn = (dict(), dict())
for ctrl in range(2):
for param in PARAMETERS:
lbl[ctrl][param['mnemo']] = tk.Label(text=param['mnemo'])
self.box_val[ctrl][param['mnemo']] = tk.IntVar()
self.sbox[ctrl][param['mnemo']] = tk.Spinbox(from_=param['min'],
to=param['max'],
textvariable=self.box_val[ctrl][param['mnemo']],
format=param['format'])
self.btn[ctrl][param['mnemo']] = tk.Button(text="Set")
# Layout
tk.Label(text='Controller 1').grid(row=0, column=1)
tk.Label(text='Controller 2').grid(row=0, column=4)
for ctrl in range(2):
for idx, param in enumerate(PARAMETERS):
lbl[ctrl][param['mnemo']].grid(row=idx+len(PARAMETERS)+1,
column=ctrl*3+0)
self.sbox[ctrl][param['mnemo']].grid(row=idx+len(PARAMETERS)+1,
column=ctrl*3+1)
self.btn[ctrl][param['mnemo']].grid(row=idx+len(PARAMETERS)+1,
column=ctrl*3+2)
# Global look
self.config(bd=3, relief='groove')
#==============================================================================
class ClientStm(object):
def __init__(self, port, config_file, ofile, ui_type='2to1', master=None):
"""
:param ui_type: handle behavior of client with respect to final
application (value: '2to1', '2to2' or '2to2s').
"""
self.scom = serial.Serial(port=port,
baudrate=115200,
timeout=1,
xonxoff=False,
rtscts=False,
dsrdtr=True)
self.ui = ClientStmGui(master)
self.ui.destroy.configure(self.del_())
self.ui_type = ui_type
self.config_file = config_file
self.config_to_device()
self.config_to_ui(0)
self.config_to_ui(1)
if ofile is None:
self.fd = sys.stdout
else:
self.fd = open(ofile, 'a')
self._app_run = threading.Event()
self.read_handler = threading.Thread(target=self.read_handling,
args=(self.scom,
self.fd,
self._app_run))
self._app_run.set()
self.read_handler.start()
self.ui.mainloop()
def del_(self):
"""Intercepter self.ui.destroy()
"""
self._app_run.clear()
print("flag cleared")
self.read_handler.join()
try:
self.fd.close()
except:
pass
@property
def ui_type(self):
return self._ui_type
@ui_type.setter
def ui_type(self, ui_type):
self._ui_type = ui_type
for ctrl in range(2):
for param in PARAMETERS:
self.ui.btn[ctrl][param['mnemo']].configure(
command=lambda x=ctrl, y=param['mnemo']:
self.btn_clicked(x, y))
if ui_type == '2to2':
pass
elif ui_type == '2to2s':
# Sync spinbox 'fs'
self.ui.sbox[0]['fs'].configure(command=lambda:
self.ui.box_val[1]['fs'].set(self.ui.box_val[0]['fs'].get()))
self.ui.sbox[1]['fs'].configure(command=lambda:
self.ui.box_val[0]['fs'].set(self.ui.box_val[1]['fs'].get()))
else: # -> ui_type = '2to1'
pass
def btn_clicked(self, ctrl, mnemo):
try:
value = self.ui.box_val[ctrl][mnemo].get()
except Exception as ex:
print("Value error: {}".format(ex))
return
self.send_cmd(mnemo, ctrl, value)
self.save_setting("servo"+str(ctrl), mnemo, value)
def config_to_device(self):
"""Get value of parameters from configuration file and
set them to device.
"""
for ctrl in range(2):
config = self.get_servo_config(ctrl)
for param in PARAMETERS:
mnemo = param['mnemo']
value = config[param['mnemo']]
self.send_cmd(mnemo, ctrl, value)
def config_to_ui(self, cctrl):
"""Get value of parameters of the controller from configuration
file and set them to UI.
"""
config = self.get_servo_config(cctrl)
for param in PARAMETERS:
self.ui.box_val[cctrl][param['mnemo']].set(config[param['mnemo']])
def get_servo_config(self, cctrl):
configp = configparser.ConfigParser()
configp.read(self.config_file)
servo_cfg = dict()
for param in PARAMETERS:
servo_cfg[param['mnemo']] = configp.getint('servo'+str(cctrl),
param['mnemo'])
return servo_cfg
def save_setting(self, section, parameter, value):
"""Write a specific setting to the configuration file.
"""
configp = configparser.ConfigParser()
configp.read(self.config_file)
configp.set(section, parameter, str(value))
with open(self.config_file, 'w') as fd:
configp.write(fd)
def send_cmd(self, mnemo, ctrl, value):
msg = "{} {} {}".format(mnemo, ctrl, value)
self.send_data(msg)
def send_data(self, msg):
header = pack('<L', len(msg))
self.scom.write(header + msg.encode('utf-8'))
def read_handling(self, scom, fd, app_run):
while app_run.is_set() is True:
try:
message = scom.read(2048).decode('utf8')
fd.write(message)
except KeyboardInterrupt:
print("KeyboardInterrupt")
app_run.clear()
#==============================================================================
def reset_settings(settings_file):
"""Resets the "settings" file with default values.
:param settings_file: file containing setting data value (str)
:returns: None
"""
config = configparser.ConfigParser()
for ctrl in range(2):
config.add_section('servo'+str(ctrl))
for param in PARAMETERS:
config.set('servo'+str(ctrl),
param['mnemo'],
str(param['default']))
# Write modification to setting file
with open(settings_file, 'w') as fd:
fd.truncate(0) # Reset file contents
config.write(fd)
#==============================================================================
def handle_args():
parser = argparse.ArgumentParser(
prog="client_stm",
usage="%(prog)s [options]",
description="Client for \"double PID\" on STM32",
)
parser.add_argument(
"-p", "--port",
dest="port",
action="store",
help="Set the port to connect to",
default="/dev/ttyACM0"
)
parser.add_argument(
"-o", "--ofile",
dest="ofile",
action="store",
help="Set a file to write logged data, otherwise write to stdout ",
)
parser.add_argument(
"-t", "--type",
dest="ui_type",
action="store",
help="Set the behavior of the UI ('2to1', '2to2' or '2to2s')",
default="2to1"
)
return parser.parse_args(sys.argv[1:])
#==============================================================================
def main():
args = handle_args()
ini_file = os.path.join(CONFIG_DIR, APPLICATION + args.ui_type + ".ini")
if not os.path.isfile(ini_file):
if not os.path.exist(CONFIG_DIR):
os.makedirs(CONFIG_DIR)
reset_settings(ini_file)
root = tk.Tk()
root.resizable(0, 0)
root.title("Double servo client ({})".format(args.ui_type))
app = ClientStm(args.port, ini_file, args.ofile, args.ui_type, root)
#==============================================================================
main()
|