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 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)