def parse_vesc(self):
buf = bytes([x.data for x in self.data])
vmsg, consumed = pyvesc.decode(buf)
if not vmsg:
return None
if self.parent.options['json']:
fields = {}
for f in vmsg.fields:
name = f[0]
value = vmsg.__getattribute__(name)
if type(value) == bytes:
value = value.decode()
fields[name] = value
msg = json.dumps({
'command': type(vmsg).__name__,
'direction': self.prefix,
'fields': fields
})
else:
arg = ', '.join([
f[0] + "=" + str(vmsg.__getattribute__(f[0]))
for f in vmsg.fields
])
msg = '{} {}'.format(type(vmsg).__name__, arg)
self.puti(len(self.data) - 4, 'command', msg)
return msg
def run_command(serialport, command):
with serial.Serial(serialport, baudrate=115200, timeout=0.2) as ser:
# Encode the command as per vesc spec and write to serial port
ser.write(pyvesc.encode(SetTerminalCmd(command)))
# read all bytes available from serial port
bytedata = read_all(ser)
# VESC data comes in frames, we need to decode each frame and append
# together.
fullresponse = ''
consumed = 0
fl = True
while consumed < len(bytedata):
(response, consumed) = pyvesc.decode(bytedata)
# strip the bytes already decoded from the serial data
bytedata = bytedata[consumed:]
if fl:
fl = False
fullresponse = response.message
else:
fullresponse = fullresponse + '\n' + response.message
return fullresponse
def verify_encode_decode(self, msg):
import pyvesc
encoded = pyvesc.encode(msg)
decoded, consumed = pyvesc.decode(encoded)
self.assertEqual(consumed, len(encoded))
for field in msg._field_names:
self.assertEqual(getattr(msg, field), getattr(decoded, field))
def get_values_example():
ERPM = 3000
with serial.Serial(serialport, baudrate=115200, timeout=0.05) as ser:
try:
# Optional: Turn on rotor position reading if an encoder is installed
ser.write(
pyvesc.encode(
SetRotorPositionMode(SetRotorPositionMode.DISP_POS_OFF)))
while True:
# Set the ERPM of the VESC motor
# Note: if you want to set the real RPM you can set a scalar
# manually in setters.py
# 12 poles and 19:1 gearbox would have a scalar of 1/228
# ERPM = ERPM - 1
ser.write(pyvesc.encode(SetRPM(ERPM)))
# Request the current measurement from the vesc
ser.write(pyvesc.encode_request(GetValues))
time.sleep(0.05)
# Check if there is enough data back for a measurement
if ser.in_waiting > 71:
(response,
consumed) = pyvesc.decode(ser.read(ser.in_waiting))
# Print out the values
if response:
print_VESC_values(response)
print(ERPM)
except KeyboardInterrupt:
# Turn Off the VESC
ser.write(pyvesc.encode(SetCurrent(0)))
def read(self):
'''Read a pyvesc VESC message from the device'''
# from Roveberrypy
to_int = lambda b: int.from_bytes(b, byteorder='big')
head = self.ser.read()
# magic VESC header must be 2 or 3
if not to_int(head) == 2 or to_int(head) == 3:
return None
length = self.ser.read(to_int(head) - 1)
packet = head + length + self.ser.read(to_int(length) + 3)
return pyvesc.decode(packet)[0]
async def _receive_task(self):
"""Recieve packets and notify the upstream Device"""
if not self._reader:
raise RuntimeError("Serial reader not initialized yet")
debug("Serial Receive task running")
while True:
try:
_packet = {}
if self.encoding == 'json':
pkt = ''
curleystack = 0
squarestack = 0
done_reading = False
while not done_reading:
b = await self._reader.read(1)
b = b.decode()
if b == '{':
curleystack += 1
elif b == '}':
curleystack -= 1
elif b == '[':
squarestack += 1
elif b == ']':
squarestack -= 1
pkt += b
if curleystack == 0 and squarestack == 0:
done_reading = True
_packet = json.loads(pkt)
elif self.encoding == 'vesc':
# Taken from Roveberrypy
def to_int(b):
return int.from_bytes(b, byteorder='big')
header = await self._reader.read(1)
# magic VESC header must be 2 or 3
if not to_int(header) == 2 or to_int(header) == 3:
continue # raise error maybe?
length = await self._reader.read(to_int(header) - 1)
packet = await self._reader.read(to_int(length) + 4)
msg, _ = pyvesc.decode(header + length + packet)
_packet = {'event': msg.__class__.__name__, 'data': msg}
else:
raise RuntimeError('Encoding is not supported')
_packet['port'] = self.name
debug("Got packet {}".format(_packet))
await self._packet_queue.put(_packet)
except serial.serialutil.SerialException:
print('serial disconnect')
self._reader = None
while self._reader == None:
await self._init_serial()
def poll_encoder(self, device):
''' Polls each VESC for its encoder position.'''
with serial.Serial(self.devices[device], baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT) as ser:
ser.write(pyvesc.encode_request(GetRotorPosition))
# while ser.in_waiting < 9:
# # Wait for response. TODO: Maybe don't wait forever...
# pass
buffer = ser.read(10) # size of a RotorPosition message
try:
(response, consumed) = pyvesc.decode(buffer)
if response.__class__ == GetRotorPosition:
return response.rotor_pos
except:
self.log("Failed to read rotor position {}".format(device), "ERROR")
return None
def get_sensors_data(self, report_field_names):
self._ser.write(pyvesc.encode_request(pyvesc.GetValues))
in_buf = b''
while self._ser.in_waiting > 0:
in_buf += self._ser.read(self._ser.in_waiting)
if len(in_buf) == 0:
return None
response, consumed = pyvesc.decode(in_buf)
if consumed == 0:
return None
if isinstance(response, pyvesc.GetValues):
report_row = {}
for field_name in report_field_names:
report_row[field_name] = getattr(response, field_name)
return report_row
return None
def get_values_example():
print("running")
with serial.Serial(serialport, baudrate=115200, timeout=0.05) as ser:
try:
# Optional: Turn on rotor position reading if an encoder is installed
ser.write(
pyvesc.encode(
SetRotorPositionMode(
SetRotorPositionMode.DISP_POS_MODE_ENCODER)))
while True:
# Set the ERPM of the VESC motor
# Note: if you want to set the real RPM you can set a scalar
# manually in setters.py
# 12 poles and 19:1 gearbox would have a scalar of 1/228
ser.write(pyvesc.encode(SetDutyCycle(10000)))
# Request the current measurement from the vesc
ser.write(pyvesc.encode_request(GetValues))
# Check if there is enough data back for a measurement
if ser.in_waiting > 61:
print(pyvesc.decode(ser.read(61)))
# print("hola");
# Print out the values
try:
# print("trato")
print(response.tachometer)
except:
# ToDo: Figure out how to isolate rotor position and other sensor data
# in the incoming datastream
#try:
# print(response.rotor_pos)
#except:
# pass
pass
time.sleep(0.1)
except KeyboardInterrupt:
# Turn Off the VESC
ser.write(pyvesc.encode(SetCurrent(0)))
print("putamadres")
def simple_example():
# lets make a SetDuty message
my_msg = pyvesc.SetDutyCycle(255)
# now lets encode it to make get a byte string back
packet = pyvesc.encode(my_msg)
# now lets create a buffer with some garbage in it and put our packet in the middle
buffer = b'\x23\x82\x02' + packet + b'\x38\x23\x12\x01'
# now lets parse our message which is hidden in the buffer
msg, consumed = pyvesc.decode(buffer)
# update the buffer
buffer = buffer[consumed:]
# check that the message we parsed is equivalent to my_msg
assert my_msg.duty_cycle == msg.duty_cycle
print("Success!")
def update(self):
if self.open:
try:
if (time.time() - self.lastWrite > self.writeRate):
self.lastWrite = time.time()
self.writeData()
if self.port.in_waiting >= 71:
# Check for new vesc message in the buffer
(vescMessage, consumed) = pyvesc.decode(self.port.read(70))
if vescMessage:
if (self.cache.getNumerical("boatConfig",0) == 0):
self.cache.set("motorTemp",float(vescMessage.temp_motor_filtered))
self.cache.set("controllerTemp",float(vescMessage.temp_fet_filtered))
self.cache.set("controllerInCurrent",float(vescMessage.avg_input_current))
self.cache.set("controllerOutCurrent",float(vescMessage.avg_motor_current))
self.cache.set("controllerDutyCycle",float(vescMessage.duty_cycle))
self.cache.set("controllerRpm",float(vescMessage.rpm))
self.cache.set("controllerInVoltage",float(vescMessage.v_in))
#self.cache.set("vescFault",int(vescMessage.mc_fault_code))
except Exception as e:
print(e)
self.close()
def test_vesc_write():
# Same as test_serial_write but with vesc data
master, slave = pty.openpty()
device = Device('test', 'tester')
device.create_serial(os.ttyname(slave), encoding='vesc')
TEST_MSG = 'Hello world'
TEST_DATA = ExampleSendMessage(TEST_MSG)
@device.task
async def write_ser():
await device.ports[os.ttyname(slave)].write(TEST_DATA)
print("done write")
device.start()
time.sleep(0.2)
data = os.read(master, 100)
print(data)
msg, _ = pyvesc.decode(data)
assert(msg.string == TEST_MSG)
device.stop()
def get_values_example():
print("running")
input_buffer=b""
output=0
t=0
old_tach=0
Connected = False #global variable for the state of the connection
client = mqtt.Client("Rigth_motor") # Create instance of client with client ID “digi_mqtt_test”
client.username_pw_set("enano","enano")
client.on_connect = on_connect # Define callback function for successful connection
client.on_message = on_message # Define callback function for receipt of a message
# client.connect("m2m.eclipse.org", 1883, 60) # Connect to (broker, port, keepalive-time)
client.connect('localhost', 1883)
with serial.Serial(serialport, baudrate=115200, timeout=0.05) as ser:
try:
client.loop_start()
# Optional: Turn on rotor position reading if an encoder is installed
ser.write(pyvesc.encode(SetRotorPositionMode(SetRotorPositionMode.DISP_POS_MODE_ENCODER)))
while True:
# Set the ERPM of the VESC motor
# Note: if you want to set the real RPM you can set a scalar
# manually in setters.py
# 12 poles and 19:1 gearbox would have a scalar of 1/228
# t = time.time()
# Start networking daemon
#print(time.time()-t)
ser.write(pyvesc.encode(SetDutyCycle(int(output))))
# Request the current measurement from the vesc
ser.write(pyvesc.encode_request(GetValues))
# Check if there is enough data back for a measurement
if ser.in_waiting >0:
input_buffer+=ser.read(ser.in_waiting)
#print(input_buffer)
if len(input_buffer) > 61:
data=input_buffer
[response,consumed]=pyvesc.decode(data)
input_buffer=input_buffer[consumed:]
# print("hola");
# Print out the values
try:
#if (float(response.tachometer/65536)!=old_tach):
# elapsed=time.time()-t
# t = time.time()
# speed=1/(90*elapsed)*60
# old_tach=round(float(response.tachometer/65536),3)
# print(response.tachometer/65535)
global motor_tach
global motor_speed
motor_speed=round(float(response.rpm/65536/14),3)
motor_tach=round(float(response.tachometer/65536),3)
try:
real_speed=round(float(response.rpm/65536/14),0)
temp_out=0 #pid(real_speed)
if (pid.setpoint==0.0 and force_break==0):
output=0
pid.auto_mode=False
else:
pid.auto_mode=True
output = pid(real_speed)
print("temp_out:"+str(temp_out)+" output:"+str(output)+" setpoint:"+str(pid.setpoint)+" speed:"+str(real_speed))
except Exception as e:
print("In error" +e)
pass
except Exception as e:
print("Out error" + e)
pass
#time.sleep(0.01)
client.loop_stop()
except KeyboardInterrupt:
# Turn Off the VESC
ser.write(pyvesc.encode(SetCurrent(0)))
print("putamadres")
def get_values_example():
print("running")
input_buffer = b""
output = 0
t = 0
old_tach = 0
with serial.Serial(serialport, baudrate=115200, timeout=0.05) as ser:
try:
# Optional: Turn on rotor position reading if an encoder is installed
ser.write(
pyvesc.encode(
SetRotorPositionMode(
SetRotorPositionMode.DISP_POS_MODE_ENCODER)))
while True:
# Set the ERPM of the VESC motor
# Note: if you want to set the real RPM you can set a scalar
# manually in setters.py
# 12 poles and 19:1 gearbox would have a scalar of 1/228
ser.write(pyvesc.encode(SetDutyCycle(int(output))))
# Request the current measurement from the vesc
ser.write(pyvesc.encode_request(GetValues))
# Check if there is enough data back for a measurement
if ser.in_waiting > 0:
input_buffer += ser.read(ser.in_waiting)
if len(input_buffer) > 61:
data = input_buffer
[response, consumed] = pyvesc.decode(data)
input_buffer = input_buffer[consumed:]
# print("hola");
# Print out the values
if (float(response.tachometer / 65536) != old_tach):
elapsed = time.time() - t
t = time.time()
speed = 1 / (90 * elapsed) * 60
old_tach = float(response.tachometer / 65536)
#print(speed)
try:
print(float(response.rpm) / 65536 / 14)
output = pid(float(response.rpm / 65536 / 14))
# print(response.tachometer/65536)
#print(output)
#print(elapsed)
except:
# ToDo: Figure out how to isolate rotor position and other sensor data
# in the incoming datastream
#try:
# print(response.rotor_pos)
#except:
# pass
pass
#time.sleep(0.01)
except KeyboardInterrupt:
# Turn Off the VESC
ser.write(pyvesc.encode(SetCurrent(0)))
print("putamadres")
def get_values_example():
print("running")
input_buffer=b""
output=0
t=0
old_tach=0
with serial.Serial(serialport, baudrate=115200, timeout=0.05) as ser:
try:
# Optional: Turn on rotor position reading if an encoder is installed
ser.write(pyvesc.encode(SetRotorPositionMode(SetRotorPositionMode.DISP_POS_MODE_ENCODER)))
while True:
# Set the ERPM of the VESC motor
# Note: if you want to set the real RPM you can set a scalar
# manually in setters.py
# 12 poles and 19:1 gearbox would have a scalar of 1/228
# t = time.time()
# Start networking daemon
#print(time.time()-t)
ser.write(pyvesc.encode(SetDutyCycle(int(output))))
# Request the current measurement from the vesc
ser.write(pyvesc.encode_request(GetValues))
# Check if there is enough data back for a measurement
if ser.in_waiting >0:
input_buffer+=ser.read(ser.in_waiting)
#print(input_buffer)
if len(input_buffer) > 61:
data=input_buffer
[response,consumed]=pyvesc.decode(data)
input_buffer=input_buffer[consumed:]
# print("hola");
# Print out the values
try:
#if (float(response.tachometer/65536)!=old_tach):
# elapsed=time.time()-t
# t = time.time()
# speed=1/(90*elapsed)*60
# old_tach=round(float(response.tachometer/65536),3)
# print(response.tachometer/65535)
global motor_tach
global motor_speed
# motor_speed=round(float(response.rpm/65536/14),3)
motor_tach=round(float(response.tachometer/65536),3)
try:
real_speed=round(float(response.rpm/65536/14),0)
temp_out=0 #pid(real_speed)
if (pid.setpoint==0.0 and force_break==0):
output=0
pid.auto_mode=False
else:
pid.auto_mode=True
output = pid(real_speed)
print("temp_out:"+str(temp_out)+" output:"+str(output)+" setpoint:"+str(pid.setpoint)+" speed:"+str(real_speed))
except Exception as e:
print("In error" + str(e))
pass
except Exception as e:
print("Out error" + str(e))
pass
#time.sleep(0.01)
except KeyboardInterrupt:
# Turn Off the VESC
ser.write(pyvesc.encode(SetCurrent(0)))
print("putamadres")
send2PCA(pwm, result)
cur_line = data_seg.split('\r')[1]
#### Read/Write data from/to VESC ####
# Set the ERPM of the VESC motor
# Note: ERPM/poles = real RPM, in this case poles = 2
ser_vesc.write(pyvesc.encode(SetRPM(InRPM)))
# ser_vesc.write(pyvesc.encode(SetCurrent(2)))
# Request the current measurement from the vesc
ser_vesc.write(pyvesc.encode_request(GetValues))
# time.sleep(0.01)
# Check if there is enough data back for a measurement
if ser_vesc.in_waiting > 71:
(response,
consumed) = pyvesc.decode(ser_vesc.read(ser_vesc.in_waiting))
# Print out the values
if response:
pmotor, tau = print_VESC_values(response)
#### Update plot ####
y_vec[-1] = tau
line1 = live_plotter(x_vec, y_vec, line1)
y_vec = np.append(y_vec[1:], 0.0)
toc = time.time() - tic
print(toc)
if time.time() - start > 20:
break
import pyvesc
import serial
import time
conn=serial.Serial(port="/dev/ttyACM0",baudrate=115200)
#request data
conn.write(pyvesc.encode_request(pyvesc.GetValues))
while conn.inWaiting() < 70:
pass
_buffer= conn.read(70)
(vescMessage,consumed) = pyvesc.decode(_buffer)
if(vescMessage):
print("VESC Data:")
print(vescMessage.temp_fet_filtered)
print(vescMessage.temp_motor_filtered)
print(vescMessage.avg_motor_current)
print(vescMessage.avg_input_current)
print(vescMessage.duty_cycle)
print(vescMessage.rpm)
print(vescMessage.v_in)
print(vescMessage.amp_hours)
print(vescMessage.watt_hours)
print(vescMessage.tachometer)
print(vescMessage.tachometer_abs)
print(vescMessage.mc_fault_code)
print(vescMessage.temp_mos1)
print(vescMessage.temp_mos2)
def log_vesc(base_name="vesc_log", port="COM1", log_interval=100):
# Prepare the log directory path
log_dir = os.path.join(os.getcwd(), 'logs')
# Create the log directory and don't throw an exception if it exists already
os.makedirs(log_dir, exist_ok=True)
# Check if the log directory exists now
if os.path.isdir(log_dir) == False:
print("Error: Unable to create log directory. Aborting.")
return
# Generate filename based on existing files in log dir
new_log = 1
reg_pattern = '_([0-9]+).csv'
no_logs = True
# Create a list of items in log directory
for root, dirs, files in os.walk(log_dir):
# For each file
for filename in files:
# Is the file an existing log?
if filename.find(base_name) != -1:
# no_logs remains True only when no matches were found
no_logs = False
# Extract the log number from the filename
reg_out = re.search(reg_pattern, filename)
old_log = int(reg_out.group(1))
if old_log > new_log:
new_log = old_log
if no_logs:
log_filename = base_name + "_1.csv"
else:
new_log += 1
log_filename = base_name + "_" + str(new_log) + ".csv"
# Add the full path to the filename
log_filename = os.path.join(log_dir, log_filename)
print("New log will be " + log_filename)
try:
# Open serial port
s1 = serial.Serial(port, baudrate=115200)
# Open log file for writing
log_file = open(log_filename, 'w')
input_buffer = bytearray()
# Start polling timer
last_time = millis()
start_time = last_time
first_msg = True
while True:
loop_time = millis()
# if polling timer expires reset the timer and poll the VESC
if loop_time - last_time >= log_interval:
last_time = loop_time
# pyvesc creates frame to request sensor data
send_packet = pyvesc.encode_request(GetValues)
s1.write(send_packet)
sent_time = millis()
# Check if serial buffer has data
if s1.in_waiting > 0:
input_buffer += s1.read(s1.in_waiting) # Grab the bytes waiting in the serial input
# Enough data for vesc frame?
if len(input_buffer) > 61:
recv_msg, consumed = pyvesc.decode(input_buffer)
input_buffer = input_buffer[consumed:] # trim consumed bytes
# Sort the sensor names
sensor_keys = sorted(recv_msg.__dict__.keys())
if first_msg == True:
first_msg = False
csv_names = "time (s), "
# Use the received sensor names for the CSV file's column names
for sensor in sensor_keys:
csv_names += sensor +", "
print("Logging values: " + csv_names + "\n")
log_file.write(csv_names + "\n")
# Start the line with the time column value
log_line = str((loop_time - start_time) / 1000) + ','
# Fill out the time string
sec, ms = divmod(loop_time - start_time, 1000)
time_str = time.strftime("%Hh %Mm %S.", time.gmtime(sec))
time_str += "{:03d}s".format(ms)
# Add each sensor value to the log line
for sensor in sensor_keys:
log_line += str(recv_msg.__dict__[sensor]) + ','
# Finish the line and write it
log_line += '\n'
log_file.write(log_line)
log_file.flush()
print("Sensor data received at " + time_str)
except (OSError, serial.SerialException, ValueError) as ex:
print("Error: " + str(ex))
# Flush file buffer and close file
log_file.flush()
log_file.close()
# Close serial port
s1.close()
return
def getdata(q):
# ==============INITS==================
# serial port inits
start = time.time()
cur_line = ""
ser_ard = serial.Serial(port='/dev/ttyUSB0', baudrate=2000000, dsrdtr=True)
# VESC comm inits
ser_vesc = serial.Serial(port='/dev/ttyACM0',
baudrate=2000000,
timeout=0.05)
# Optional: Turn on rotor position reading if an encoder is installed
ser_vesc.write(
pyvesc.encode(SetRotorPositionMode(SetRotorPositionMode.DISP_POS_OFF)))
# i2c inits
pwm = Adafruit_PCA9685.PCA9685(busnum=1)
pwm.set_pwm_freq(60)
pwm.set_pwm(0, 0, 375)
InRPM = 0
# misc inits
pmotor = 0
tau = 0
Result_Ard = [1500] * 4 + [0] * 9
ControlSwitch = 1900
Ctrl_Radio = [0, 0, 0]
# Open Result txt to log data
now = datetime.datetime.now()
currentDateTime = now.strftime("%Y%m%d-%H%M%S")
f = open("./Results/" + currentDateTime + ".txt", "w+")
# Log headers
# time.time(),tau,response.rpm,steering,InRPM*10.0,Ctrl_Radio[1],pmotor
f.write(
"Rel_time,Abs_time,tau,rpm,steering PWM,DutyCycle,Radio_Throttle_PWM,P_motor,\n"
)
# 1104, 1506, 1906 for VESC
ser_vesc.write(pyvesc.encode(SetRPM(1000)))
# ==============Central Loop==================
while time.time() - start < 60:
tic = time.time()
##### Read Data #####
# Read data chuck from Arduino
ser_var = ser_ard.read(ser_ard.inWaiting())
data_seg = ser_var.decode('utf-8')
if data_seg.find('\r') == -1:
# If chuck is incomplete, keep reading
cur_line += data_seg
else:
# If chuck is complete, parse chuck
cur_line += data_seg.split('\r')[0]
try:
Result_Ard = list(map(float, cur_line.split(',')))
except:
pass
cur_line = data_seg.split('\r')[1]
# Request the current state from the vesc
ser_vesc.write(pyvesc.encode_request(GetValues))
# Check if there is enough data back for a measurement
if ser_vesc.in_waiting > 71:
(response,
consumed) = pyvesc.decode(ser_vesc.read(ser_vesc.in_waiting))
# Print out the values
if response:
pmotor, tau, vmotor = print_VESC_values(response)
#### Update plot ####
cur_t = time.time() - start
q.put([
cur_t, tau, response.rpm, response.avg_motor_current,
vmotor
])
#### Write to log ####
steering = Ctrl_Radio[0]
f.write("%f,%f,%f,%f,%f,%f,%f,%f\n" %
(cur_t, time.time(), tau, response.rpm, steering,
InRPM * 10.0, Ctrl_Radio[1], pmotor))
##### Understand Data #####
Ctrl_Radio = Result_Ard[:3]
ControlSwitch = Result_Ard[3]
IMUdata = Result_Ard[4:]
InRPM = translate(Ctrl_Radio[1], 1100, 1900, -10000, 10000)
if ControlSwitch < 1200:
Ctrl_Radio[0] = (time.time() - start) / 20 * 200.0 * math.sin(
(time.time() - start) * 2) + 1435
dummyRPM = (time.time() - start) / 20 * 2000.0 * math.sin(
(time.time() - start) * 10) + 4000
##### Send Commands #####
# Set the ERPM of the VESC motor. | Note: ERPM = poles*real_RPM, in this case poles = 2
send2PCA(pwm, Ctrl_Radio)
# ser_vesc.write(pyvesc.encode(SetDutyCycle(10000.0)))
# ser_vesc.write(pyvesc.encode(SetCurrent(2)))
if ControlSwitch < 1200:
ser_vesc.write(pyvesc.encode(SetRPM(dummyRPM)))
else:
ser_vesc.write(pyvesc.encode(SetDutyCycle(InRPM * 10.0)))
# Show time usage for each cycle
toc = time.time() - tic
print(toc)
# ==============Cleanups==================
# Close VESC
# ser_vesc.write(pyvesc.encode(SetCurrent(0)))
# Close serial ports
ser_ard.close()
ser_vesc.close()
# Close file
f.close()
print("serial ports closed")
# Close parallel queue
q.put(['Q', 'Q', 'Q', 'Q', 'Q'])
