def init_serial(self):
self.bser = BinSerial(self.port_name, self.baud_rate)
self.read_thread = threading.Thread(
target=self.read_variables,
args=(self.bser, self.plot.time, self.plot.deque_position_input,
self.plot.deque_position_setpoint,
self.plot.deque_speed_input, self.plot.deque_speed_setpoint),
daemon=True)
self.read_thread.start()
def init_serial(self):
self.bser = BinSerial(self.serial['port'], self.serial['baud'])
self.read_thread = threading.Thread(target=self.read_serial,
args=(self.bser, self.read_format,
self.plot_data),
daemon=True)
self.read_thread.start()
self.write_thread = threading.Thread(target=self.write_serial,
args=(self.bser,
self.write_format,
self.plot_data),
daemon=True)
self.write_thread.start()
def get_step_response(self):
"""run the step response and get the measures"""
bser = BinSerial(self.port_name, self.baud_rate)
# Define the format of the structure of data sent
structFormatConfig = ['uint8', 'uint8', 'uint8', 'uint16', 'uint16']
structFormatMeasure = ['uint32', 'int32', 'float']
timestamps = np.zeros((self.nb_measure, self.nb_sample))
positions = np.zeros((self.nb_measure, self.nb_sample))
speeds = np.zeros((self.nb_measure, self.nb_sample))
# Write some data to the arduino
bser.write(structFormatConfig, [
self.pwm, self.ts, self.nb_measure, self.nb_sample, self.wait_time
])
for i in range(self.nb_measure):
self.progress_bar.setValue(i)
for j in range(self.nb_sample):
timestamps[i, j], positions[i, j], speeds[i, j]\
= bser.read(structFormatMeasure)
self.progress_bar.setValue(self.nb_measure)
self.speed = np.mean(speeds, axis=0)
class SerialInterface(QWidget):
def __init__(self):
super().__init__()
self.init_config()
self.init_ui()
self.init_serial()
def init_config(self):
# Load configuration file
with open('config.yml', 'r') as config_yml:
config = yaml.safe_load(config_yml)
# Store serial config
self.serial = config['serial']
# Initialise EasyPlot
self.easyplot = EasyPlot(self)
# Initialise the subplots
for subplot in config['subplots']:
self.easyplot.add_subplot(subplot['pos'], subplot['title'],
subplot['min'], subplot['max'])
# Initialise the plots for reading
self.read_format = []
self.plot_data = []
for plot in config['data']['read']:
self.read_format.append(plot['type'])
self.plot_data.append(
self.easyplot.add_plot(plot['pos'], plot['label']))
# Initialise for writing
self.write_format = []
for widget in config['data']['write']:
self.write_format.append(widget['type'])
def init_ui(self):
self.setWindowTitle('SerialInterface')
main_layout = QHBoxLayout()
main_layout.addWidget(self.easyplot)
self.setLayout(main_layout)
def init_serial(self):
self.bser = BinSerial(self.serial['port'], self.serial['baud'])
self.read_thread = threading.Thread(target=self.read_serial,
args=(self.bser, self.read_format,
self.plot_data),
daemon=True)
self.read_thread.start()
self.write_thread = threading.Thread(target=self.write_serial,
args=(self.bser,
self.write_format,
self.plot_data),
daemon=True)
self.write_thread.start()
def read_serial(self, bser, read_format, plot_data):
i = 0
while (True):
i += 1
data = bser.read(read_format)
for j in range(len(read_format)):
plot_data[j][0].append(i)
plot_data[j][1].append(data[j])
def write_serial(self, bser, write_format, plot_data):
"""run the step response and get the measures"""
# Write some data to the arduino
while (True):
self.bser.write(write_format, [float(input("write: "))])
#!/usr/bin/python3
# -*- coding: utf-8 -*-
from binserial import BinSerial
if __name__ == "__main__":
bser = BinSerial("/dev/ttyACM0", 115200)
while True:
print(bser.read(['float'] * 3))
#!/usr/bin/python3
# -*- coding: utf-8 -*-
from binserial import BinSerial
if __name__ == "__main__":
bser = BinSerial("/dev/ttyACM0", 9600)
while True:
# bser.write(['uint32'], [int(input('time: '))])
bser.write(['int16'], [int(input('pwm: '))])
print(bser.read(['int32'] * 2))
class PidInterface(QWidget):
def __init__(self):
super().__init__()
self.init_parameters()
self.init_ui()
# self.init_serial()
def init_serial(self):
self.bser = BinSerial(self.port_name, self.baud_rate)
self.read_thread = threading.Thread(
target=self.read_variables,
args=(self.bser, self.plot.time, self.plot.deque_position_input,
self.plot.deque_position_setpoint,
self.plot.deque_speed_input, self.plot.deque_speed_setpoint),
daemon=True)
self.read_thread.start()
def init_parameters(self):
with open('config.yml', 'r') as config_yml:
config = yaml.safe_load(config_yml)
self.port_name = config['port_name']
self.baud_rate = config['baud_rate']
self.kp = config['kp']
self.ki = config['ki']
self.kd = config['kd']
self.sample_time = config['sample_time']
self.max_setpoint = config['max_setpoint']
self.mode = config['mode']
self.anti_windup = config['anti_windup']
def set_sample_time(self, new_sample_time):
self.sample_time = new_sample_time
def set_kp(self, new_kp):
self.kp = new_kp
def set_ki(self, new_ki):
self.ki = new_ki
def set_kd(self, new_kd):
self.kd = new_kd
def set_setpoint(self, new_setpoint):
self.setpoint = new_setpoint
self.sent_parameters()
def set_mode(self, new_mode):
self.mode = new_mode
def set_anti_windup(self, new_anti_windup):
self.anti_windup = new_anti_windup
def init_ui(self):
self.setWindowTitle('PidInterface')
# Parameters
self.sample_time_spin = QSpinBox()
self.sample_time_spin.setMinimum(0)
self.sample_time_spin.setMaximum(1000)
self.sample_time_spin.setSuffix(' ms')
self.sample_time_spin.setValue(self.sample_time)
self.sample_time_spin.valueChanged.connect(self.set_sample_time)
self.kp_spin = QDoubleSpinBox()
self.kp_spin.setMinimum(0)
self.kp_spin.setMaximum(float('inf'))
self.kp_spin.setValue(self.kp)
self.kp_spin.valueChanged.connect(self.set_kp)
self.ki_spin = QDoubleSpinBox()
self.ki_spin.setMinimum(0)
self.ki_spin.setMaximum(float('inf'))
self.ki_spin.setValue(self.ki)
self.ki_spin.valueChanged.connect(self.set_ki)
self.kd_spin = QDoubleSpinBox()
self.kd_spin.setMinimum(0)
self.kd_spin.setMaximum(float('inf'))
self.kd_spin.setValue(self.kd)
self.kd_spin.valueChanged.connect(self.set_kd)
self.setpoint_slider = QSlider(QtCore.Qt.Horizontal)
self.setpoint_slider.setMinimum(0)
self.setpoint_slider.setMaximum(self.max_setpoint)
self.setpoint_slider.setValue(0)
self.setpoint_slider.sliderMoved.connect(self.set_setpoint)
self.mode_check = QCheckBox()
self.mode_check.setChecked(self.mode)
self.mode_check.toggled.connect(self.set_mode)
self.anti_windup_check = QCheckBox()
self.anti_windup_check.setChecked(self.anti_windup)
self.anti_windup_check.toggled.connect(self.set_anti_windup)
parameters_layout = QFormLayout()
parameters_layout.addRow('sample_time', self.sample_time_spin)
parameters_layout.addRow('kp', self.kp_spin)
parameters_layout.addRow('ki', self.ki_spin)
parameters_layout.addRow('kd', self.kd_spin)
parameters_layout.addRow('setpoint', self.setpoint_slider)
parameters_layout.addRow('mode', self.mode_check)
parameters_layout.addRow('anti_windup', self.anti_windup_check)
parameters_group = QGroupBox('Parameters')
parameters_group.setLayout(parameters_layout)
apply_button = QPushButton('Apply')
apply_button.clicked.connect(self.sent_parameters)
pid_layout = QVBoxLayout()
pid_layout.addWidget(parameters_group)
pid_layout.addWidget(apply_button)
pid_layout.addStretch()
# Display
self.plot = BeautifulPlot(self)
# Main
main_layout = QHBoxLayout()
main_layout.addLayout(pid_layout)
main_layout.addWidget(self.plot)
self.setLayout(main_layout)
def sent_parameters(self):
"""run the step response and get the measures"""
# Write some data to the arduino
self.bser.write(['uint32'] + ['float'] * 4 + ['bool'] * 2, [
self.sample_time, self.kp, self.ki, self.kd, self.setpoint,
self.mode, self.anti_windup
])
def read_variables(self, bser, time, deque_position_input,
deque_position_setpoint, deque_speed_input,
deque_speed_setpoint):
i = 0
while (True):
i += 1
position_input, position_setpoint, position_output, position_integral, speed_input, speed_setpoint, speed_output, speed_integral = bser.read(
['float'] * 8)
time.append(i)
deque_position_input.append(position_input)
deque_position_setpoint.append(position_setpoint)
deque_speed_input.append(speed_input)
deque_speed_setpoint.append(speed_setpoint)
+ alpha*config['wait_time']/1000*d_thetaG
phi = alpha*phi + (1-alpha)*phiA\
+ alpha*config['wait_time']/1000*d_phiG
return theta, phi
if __name__ == '__main__':
with open('config.yml', 'r') as f:
config = yaml.load(f.read())
# Define the format of the structure of data sent.
struct_format_measure = ['uint32'] + ['int16'] * 10
# Initialize connection with the arduino.
bser = BinSerial(config['port_name'], config['baud_rate'])
# Read test connection of the sensor.
test_adxl, test_mpu = bser.read(['bool'] * 2)
if test_adxl or test_mpu:
# Calibrate
offsets, scales = calibrate(bser, config['nb_measure_calibration'],
config['do_calibration'])
# Send the wait time if at least one sensor is online.
# If it is not sent the arduino do not start the measures.
bser.write(['uint32'], [config['wait_time']])
# Coefficient the sensor fusion
alpha = config['time_constant']\
#!/usr/bin/python3
# -*- coding: utf-8 -*-
from binserial import BinSerial
import threading
def print_odometry():
while True:
print(bser.read(['float']*3+['bool']))
if __name__ == "__main__":
# Initialize serial
bser = BinSerial("/dev/ttyACM0", 9600)
# Prepare odometry thread
odometry_thread = threading.Thread(target=print_odometry, daemon=True)
# Sent initial position to the robot (True)
bser.write(['float']*3+['bool'], [0, 0, 0, True])
# Start odometry thread
odometry_thread.start()
while True:
# Sent setpoint position to the robot (False)
bser.write(['float']*3+['bool'], [float(input('x: ')), 0, 0, False])