#----------------------------------------------------------------------- # Import Required Libraries #----------------------------------------------------------------------- import numpy as np import pygame import pygame.gfxdraw import sys, os path_above = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '..')) sys.path.append(path_above) from TBotTools import geometry, pgt from datetime import datetime #----------------------------------------------------------------------- # Setup display and objects for drawing #----------------------------------------------------------------------- geom = geometry.geometry() pygame.init() screen = pygame.display.set_mode((1000, 700)) pygame.display.set_caption("Pendulum") clock = pygame.time.Clock() framerate = 60 origin = [500,180] arrow = np.array([[1,0],[1,150],[0,152],[-1,150],[-1,0],[1,0]]).astype(float) arrow = arrow/arrow.max() scalefactor = 240 sbar = pgt.SliderBar(screen, (100,100), 1, 800, 2.00, 10, (170,170,170),(10,10,10),20) textPrint = pgt.TextPrint((255,255,255)) textPrint.setfontsize(30) #----------------------------------------------------------------------- # Constants #----------------------------------------------------------------------- g = 9.81
#----------------------------------------------------------------------# # sets the length of the trail pts = deque(maxlen=10) pts2 = deque(maxlen=10) pathindex = 0 rotspeed = 200 speedfactor = 0.3 turnspeedfactor = 0.3 turntime = 0.01 bendscalefactor = 2 rdeadban = 2 tolerance = 30 #-------------------- Define functions ------------------------------# geom = geometry.geometry(1) # scale factor to convert pixels to mm #--------------------- Setup Bluetooth --------------------------------# data = [0, 0, 0, 0] sendcount = 0 #------------------------------------------------------------------ # For Linux / Raspberry Pi #------------------------------------------------------------------ bd_addr = '98:D3:51:FD:81:AC' # use: 'hcitool scan' to scan for your T-Bot address #bd_addr = '98:D3:51:FD:82:95' # George #bd_addr = '98:D3:91:FD:46:C9' # B #bd_addr = '98:D3:32:21:3D:77' port = 1 btcom = tbt.bt_connect(bd_addr, port, 'PyBluez') # PyBluez works well for the Raspberry Pi