class usb_vcp_tmcl_interface(tmcl_module_interface, tmcl_host_interface):
def __init__(self, port=0, data_rate=None, host_id=2, module_id=1, debug=False):
del data_rate
tmcl_module_interface.__init__(self, host_id, module_id, debug)
tmcl_host_interface.__init__(self, host_id, module_id, debug)
self.__vcp = USB_VCP(port)
self.__vcp.init()
self.__vcp.setinterrupt(-1)
def __enter__(self):
return self
def __exit__(self, exitType, value, traceback):
del exitType, value, traceback
self.close()
def close(self):
self.__vcp.setinterrupt(3)
self.__vcp.close()
return 0
def data_available(self, hostID, moduleID):
del hostID, moduleID
return self.__vcp.any()
def _send(self, hostID, moduleID, data):
del hostID, moduleID
self.__vcp.write(data)
def _recv(self, hostID, moduleID):
del hostID, moduleID
read = bytearray(0)
while(len(read) < 9):
read += self.__vcp.read(9)
if(not(read)):
read = bytearray(0)
return read
def printInfo(self):
pass
def enableDebug(self, enable):
self._debug = enable
@staticmethod
def supportsTMCL():
return True
@staticmethod
def supportsCANopen():
return False
@staticmethod
def available_ports():
return set([0])
def run_usb_vcp_test():
serial_port = USB_VCP()
nmea_line = None
#serial_port.setinterrupt(-1)
while True:
# retrieving data
if serial_port.any():
nmea_line = str(serial_port.readline(), 'utf-8')
if nmea_line:
delay(1)
nmea_line = None
delay(50)
class USB_Port:
def __init__(self):
self.usb_serial = USB_VCP()
self.recv_buf = bytearray(1)
# Disable Control-C on the USB serial port in case one comes in the
# data.
self.usb_serial.setinterrupt(-1)
def read_byte(self):
"""Reads a byte from the usb serial device."""
if self.usb_serial.any():
bytes_read = self.usb_serial.recv(self.recv_buf)
if bytes_read > 0:
return self.recv_buf[0]
def write(self, data):
"""Writes an entire packet to the serial port."""
self.usb_serial.write(data)
class USB_Port:
"""Implements a port which can be used to receive bioloid device commands
from a host.
"""
def __init__(self):
self.usb_serial = USB_VCP()
self.baud = 0
self.recv_buf = bytearray(1)
# Disable Control-C on the USB serail port in case one comes in the
# data.
self.usb_serial.setinterrupt(-1)
def any(self):
"""Returns a truthy value if characters are available to be read."""
return self.usb_serial.any()
def read_byte(self):
"""Reads a byte from the usb serial device.
This function will return None if no character was read within the
designated timeout.
The max Return Delay time is 254 x 2 usec = 508 usec (the
default is 500 usec). This represents the minimum time between
receiving a packet and sending a response.
"""
bytes_read = self.usb_serial.recv(self.recv_buf, timeout=2)
if bytes_read > 0:
return self.recv_buf[0]
def set_baud(self, baud):
"""Sets the baud rate. Note that for USB Serial, this is essentially
a no-op. We store the baud rate that was set, so that
"""
self.baud = baud
def write_packet(self, packet_data):
"""Writes an entire packet to the serial port."""
self.usb_serial.write(packet_data)
class USB_Port:
"""Implements a port which can be used to receive bioloid device commands
from a host.
"""
def __init__(self):
self.usb_serial = USB_VCP()
self.baud = 0
self.recv_buf = bytearray(1)
# Disable Control-C on the USB serail port in case one comes in the
# data.
self.usb_serial.setinterrupt(-1)
def any(self):
"""Returns a truthy value if characters are available to be read."""
return self.usb_serial.any()
def read_byte(self):
"""Reads a byte from the usb serial device.
This function will return None if no character was read within the
designated timeout.
The max Return Delay time is 254 x 2 usec = 508 usec (the
default is 500 usec). This represents the minimum time between
receiving a packet and sending a response.
"""
bytes_read = self.usb_serial.recv(self.recv_buf, timeout=2)
if bytes_read > 0:
return self.recv_buf[0]
def set_baud(self, baud):
"""Sets the baud rate. Note that for USB Serial, this is essentially
a no-op. We store the baud rate that was set, so that
"""
self.baud = baud
def write_packet(self, packet_data):
"""Writes an entire packet to the serial port."""
self.usb_serial.write(packet_data)
class Controller(object):
def __init__(self):
self.vs = USB_VCP()
self.direction = 0
self.speed = 0
self.select_dir = 0
self.select_speed = 0
#self.vs.setinterrupt(-1)
def dir_menu(self):
print('please input your instructions')
print('1. set the direction ')
print('2. set the speed ')
print('3. send the instruction ')
def get_instrcution(self):
print('please input your instructions ')
print('1. set the direction ')
print('2. set the speed ')
print('3. send the instruction ')
while True:
if self.vs.any():
pyb.delay(2000)
self.instrcution = self.vs.readline()
if (self.select_dir == 1):
if (self.instrcution == b'1'):
self.select_dir = 0
print('forward')
self.dir_menu()
elif (self.instrcution == b'2'):
self.select_dir = 0
print('back')
self.dir_menu()
elif (self.instrcution == b'3'):
self.select_dir = 0
print('leftforward')
self.dir_menu()
elif (self.instrcution == b'4'):
self.select_dir = 0
print('leftback')
self.dir_menu()
elif (self.instrcution == b'5'):
self.select_dir = 0
print('rightforward')
self.dir_menu()
elif (self.instrcution == b'6'):
self.select_dir = 0
print('rightback')
self.dir_menu()
elif (self.instrcution == b'7'):
self.select_dir = 0
print('stop')
self.dir_menu()
elif (self.instrcution == b'8'):
self.select_dir = 0
print('MotorA-F')
self.dir_menu()
elif (self.instrcution == b'9'):
self.select_dir = 0
print('MotorA-R')
self.dir_menu()
elif (self.instrcution == b'10'):
self.select_dir = 0
print('MotorB-F')
self.dir_menu()
elif (self.instrcution == b'11'):
self.select_dir = 0
print('MotorB-R')
self.dir_menu()
elif (self.instrcution == b'12'):
self.select_dir = 0
print('RUN')
self.dir_menu()
else:
print('please set the diretion')
elif (self.select_speed == 1):
self.select_speed == 0
print('speed is ', int(self.instrcution))
print('please input your instructions')
print('1. set the direction ')
print('2. set the speed ')
print('3. send the instruction ')
elif (self.instrcution == b'1'):
self.select_dir = 1
print('please set the diretion ')
print('1. forward')
print('2. back')
print('3. leftforward')
print('4. leftback')
print('5. rightforward')
print('6. rightback')
print('7. stop')
print('8. MotorA-F')
print('9. MotorA-R')
print('10.MotorB-F')
print('11.MotorB-R')
print('12.RUN')
elif (self.instrcution == b'2'):
self.select_speed = 1
print('set the speed ')
elif (self.instrcution == b'3'):
print('send the instruction ')
else:
print('test', self.instrcution)
print('please input your instructions')
print('1. set the direction ')
print('2. set the speed ')
print('3. send the instruction ')
import time
from pyb import (USB_VCP, Servo)
SLEEP = 0.1
UP = -90
DOWN = 0
SPEED = 1000
servo = Servo(1)
servo.angle(30)
p = USB_VCP()
while True:
if p.any():
command = p.readline().strip()
if command == b'up':
servo.angle(UP, SPEED)
p.write(b'up-ok\n')
elif command == b'down':
servo.angle(DOWN, SPEED)
p.write(b'down-ok\n')
else:
p.write(command + b'\n')
p.write(b'err\n')
time.sleep(SLEEP)
scalex = 127 / (sensor.height() - xbottom - xtop) # scale outputs to 127
scaley = 127 / (sensor.width() - ybottom - ytop)
scalez = (127 / (sensor.width() * sensor.height())
) * 10 #factor gives fraction of area that will max out integer
yhalf = sensor.width() / 2
buf = 10
scaley_half = 64 / (yhalf - 2 * buf)
mode = 0
debug = 0 # 0 for debug 1 for run
while (True):
#time.sleep(100)
if (usb.any()):
#halfmode = int.from_bytes(usb.read(), 'little')
mode = int(usb.read())
if not debug:
#timeprint(0)
img = sensor.snapshot() # Take a picture and return the image.
#img.difference(bg)
#stat = img.get_statistics()
#print(stat.max())
#img.binary([(30,255)])
blob = img.find_blobs([(100, 255)], area_threshold=2)
def run_uav_test(i2c_bus=2):
global SIGNALS
global SIGNAL_USR
SIGNALS[0] = 0
serial_port = USB_VCP()
nmea_line = None
# serial_port.setinterrupt(-1)
disp = display.create_spi_display(SSD1322, 256, 64)
i2c = I2C(i2c_bus, freq=400000)
devices = i2c.scan()
lsm303 = LSM303D(i2c)
switch = Switch()
speed_pid = PID(target=500, kp=.4, ki=.2, kd=.1)
uav['pid'] = speed_pid
timestamp = None
w = 0
screen_renderers = [
render_gps_screen, render_hud_screen, render_imu_screen
]
renderer_idx = 0
render_screen = screen_renderers[renderer_idx]
switch.callback(switch_cb)
while True:
# retrieving data
accel, mag = lsm303.read()
if serial_port.any():
nmea_line = str(serial_port.readline(), 'utf-8')
# processing data
x, y, z = accel
x, z, y = mag
uav['imu']['north'] = atan2(y, x) * 180.0 / pi
if nmea_line:
update_gps_data(nmea_line)
nmea_line = None
# sending orders
if renderer_idx % 2:
if timestamp:
pid_value = speed_pid.update(
uav['speed'],
elapsed_millis(timestamp) / 1000.0)
adjust_throttle(serial_port, pid_value)
timestamp = millis()
if SIGNALS[0] & SIGNAL_USR:
renderer_idx = renderer_idx + 1
render_screen = screen_renderers[renderer_idx %
len(screen_renderers)]
SIGNALS[0] = SIGNALS[0] & ~SIGNAL_USR
render_screen(disp.framebuf, uav)
disp.send_buffer()
delay(50)
