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 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 send_duties(self): ''' Tell each motor controller to turn on motors''' if "d_armBase" in self.devices: with serial.Serial(self.devices["d_armBase"], baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT) as ser: ser.write(pyvesc.encode(SetDutyCycle(int(100000*self.speeds[0])))) if "d_armShoulder" in self.devices: with serial.Serial(self.devices["d_armShoulder"], baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT) as ser: ser.write(pyvesc.encode(SetDutyCycle(int(100000*self.speeds[1])))) if "d_armElbow" in self.devices: with serial.Serial(self.devices["d_armElbow"], baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT) as ser: ser.write(pyvesc.encode(SetDutyCycle(int(100000*self.speeds[2])))) if "d_armWristRot" in self.devices: with serial.Serial(self.devices["d_armWristRot"], baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT) as ser: ser.write(pyvesc.encode(SetDutyCycle(int(100000*self.speeds[4])))) if "d_armGripperOpen" in self.devices: with serial.Serial(self.devices["d_armGripperOpen"], baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT) as ser: ser.write(pyvesc.encode(SetDutyCycle(int(100000*self.speeds[5]))))
def messageTrigger(self, message):
if message.key in device_keys:
self.log("Received device: {} at {}".format(message.key, message.data), "DEBUG")
self.devices[message.key] = message.data
with serial.Serial(message.data, baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT) as ser:
# Turn on encoder readings for this VESC
ser.write(pyvesc.encode(
SetRotorPositionMode(
SetRotorPositionMode.DISP_POS_MODE_ENCODER )))
def _movement_ctrl_th_tf(self):
"""Target function of movement control thread. Responsive for navigation engines work (forward/backward)"""
try:
while self._keep_thread_alive:
if self._allow_movement:
if time.time() - self._start_time > self._moving_time:
self._ser.write(pyvesc.encode(pyvesc.SetRPM(0)))
self._last_stop_time = time.time()
self._allow_movement = False
continue
if time.time() > self._next_alive_time:
self._next_alive_time = time.time() + self._alive_freq
self._ser.write(pyvesc.encode(pyvesc.SetAlive))
time.sleep(self._check_freq)
except serial.SerialException as ex:
print(ex)
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))
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!")
async def _send_task(self):
"""Write a packet (or bytes) to the serial device."""
if not self._writer:
raise RuntimeError("Serial writer not initialized yet")
debug("Send task running")
while True:
packet = await self._send_queue.get()
debug("Sending packet {}".format(packet))
if self.encoding == 'raw':
await self._writer.write(packet)
elif self.encoding == 'utf8':
await self._writer.write(packet.encode())
elif self.encoding == 'json':
await self._writer.write(json.dumps(packet).encode())
elif self.encoding == 'vesc':
# I don't know why I can't await the _writer in
# this context, but can in others...
self._writer.write(pyvesc.encode(packet))
else:
raise RuntimeError('Packet format type not supported')
def apply_rpm(self, rpm):
if self._rpm != rpm: # TODO: bug to fix: if rpm was set by set_rpm - it won't be applied on vesc
self._rpm = rpm
self._ser.write(pyvesc.encode(pyvesc.SetRPM(self._rpm)))
# I
if __name__ == '__main__':
# 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=115200,
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
# plot inits
plt.style.use('ggplot')
size = 10
x_vec = np.linspace(0, 1, size + 1)[0:-1]
y_vec = np.ones(len(x_vec))
line1 = []
# misc inits
def stop_moving(self):
self._allow_movement = False
self._last_stop_time = time.time()
self._ser.write(pyvesc.encode(pyvesc.SetRPM(0)))
def stop_current(self):
self._ser.write(pyvesc.encode(pyvesc.SetCurrent(0)))
def set_current(self, current_ref):
self.ser.write(pyvesc.encode(pyvesc.SetCurrent(int(current_ref))))
def start_moving(self):
self._start_time = self._next_alive_time = time.time()
self._ser.write(pyvesc.encode(pyvesc.SetRPM(self._rpm)))
self._allow_movement = True
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 disconnect(self):
self._ser.write(pyvesc.encode(pyvesc.SetRPM(0)))
self._keep_thread_alive = False
self._ser.close()
#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)
print(vescMessage.temp_mos3)
throt_msg = pyvesc.SetDutyCycle(10000)
conn.write(pyvesc.encode(throt_msg))
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'])
def changeDuty(num):
ser.write(pyvesc.encode(pyvesc.SetDutyCycle(num)))
time.sleep(SLEEP_TIME)
def write(self, vesc_message):
'''Send a pyvesc VESC message to the device'''
b = pyvesc.encode(vesc_message)
self.ser.write(b)
def current_packet(value) -> bytes:
message = pyvesc.SetCurrent(value)
packet = pyvesc.encode(message)
return packet
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 changeBrake(num):
ser.write(pyvesc.encode(pyvesc.SetCurrentBrake(num)))
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 dutyPackage(voltage) -> bytes:
slowDutyCycleF = pyvesc.SetDutyCycle(
voltage) # prints value of my_msg.duty_cycle
sendSDCF = pyvesc.encode(slowDutyCycleF)
return sendSDCF
