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 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 writeData(self):
#Get the throttle value and write it to the vesc.
# Throttle duty cycle value range for vesc: -100,000 to 100,000
# Input throttle: 0 to 100
# Only write a value if we are in endurance mode
if (self.cache.getNumerical('boatConfig',0) == 0):
if (self.cache.getNumerical('throttleMode',0) == 0):
throttle = ((self.cache.getNumerical('throttle',0)/255.0) * 100.0 * 1000.0)
throttleMessage = pyvesc.SetDutyCycle(int(throttle))
self.port.write(pyvesc.encode(throttleMessage))
else:
# Goal current is the MOTOR CURRENT,
# To convert to motor current from battery current: MOTOR CURRENT = BATTERY CURRENT / DUTY CYCLE
batteryCurrent = (self.cache.getNumerical('throttleCurrentTarget',0))
dutyCycle = (self.cache.getNumerical('controllerDutyCycle',0) / 10.0)
if dutyCycle > 0:
motorCurrent = (batteryCurrent / dutyCycle) * -1.0 * 1000.0
else:
motorCurrent = 0
throttleMessage = pyvesc.SetCurrent(int(motorCurrent))
self.port.write(pyvesc.encode(throttleMessage))
else:
self.port.write(pyvesc.encode(pyvesc.SetDutyCycle(0)))
self.port.write(pyvesc.encode_request(pyvesc.GetValues))
result = list(map(float, cur_line.split(',')))
except:
pass
print(result)
InRPM = translate(result[1], 1100, 1900, -10000, 10000)
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)
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 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
# 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")
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")
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 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'])