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 l = 1 alpha = 0 omega = 0 theta = 0.9*np.pi dt = 1.0/framerate C = 1 internal_loops = 10 dt = dt / internal_loops
for ii in range(xdatarange[0], xdatarange[1]): pts.appendleft((ii, 0)) pts2.appendleft((ii, 0)) iii = 200 aa = np.zeros((len(pts), 2)) aa[:, 1] = np.array(pts)[:, 1] aa[:, 0] = np.array(range(xdatarange[0], xdatarange[1])) cc = np.zeros((len(pts), 2)) cc[:, 1] = np.array(pts2)[:, 1] cc[:, 0] = np.array(range(xdatarange[0], xdatarange[1])) bb = np.copy(aa) dd = np.copy(cc) #screen, pos, pos2, length, scale, thickness, colour1, colour2, tolerence = [] #sbar = pgt.SliderBar(screen, (200,800), s_kp, 800, 2.00, 10, (170,170,170),(10,10,10),20) sbar = pgt.SliderBar(screen, (100, 475), s_kp, 130, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar2 = pgt.SliderBar(screen, (100, 490), s_ki, 130, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar3 = pgt.SliderBar(screen, (100, 505), s_kd, 130, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar4 = pgt.SliderBar(screen, (100, 535), a_kp, 130, 20.0, 5, (200, 200, 200), (255, 10, 10)) sbar5 = pgt.SliderBar(screen, (100, 550), a_ki, 130, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar6 = pgt.SliderBar(screen, (100, 565), a_kd, 130, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar7 = pgt.SliderBar(screen, (100, 580), noise_amplitude, 130, 2, 5, (200, 200, 200), (255, 10, 10)) sbar8 = pgt.SliderBar(screen, (100, 595), acc_g, 130, acc_g, 5, (200, 200, 200), (255, 10, 10))
for ii in range(xdatarange[0], xdatarange[1]): pts.appendleft((ii, 0)) pts2.appendleft((ii, 0)) iii = 200 aa = np.zeros((len(pts), 2)) aa[:, 1] = np.array(pts)[:, 1] aa[:, 0] = np.array(range(xdatarange[0], xdatarange[1])) cc = np.zeros((len(pts), 2)) cc[:, 1] = np.array(pts2)[:, 1] cc[:, 0] = np.array(range(xdatarange[0], xdatarange[1])) bb = np.copy(aa) dd = np.copy(cc) #screen, pos, pos2, length, scale, thickness, colour1, colour2, tolerence = [] #sbar = pgt.SliderBar(screen, (200,800), s_kp, 800, 2.00, 10, (170,170,170),(10,10,10),20) sbar = pgt.SliderBar(screen, (100, 475 + 300), s_kp, 230, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar2 = pgt.SliderBar(screen, (100, 490 + 300), s_ki, 230, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar3 = pgt.SliderBar(screen, (100, 505 + 300), s_kd, 230, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar4 = pgt.SliderBar(screen, (100, 535 + 300), a_kp, 230, 20.0, 5, (200, 200, 200), (255, 10, 10)) sbar5 = pgt.SliderBar(screen, (100, 550 + 300), a_ki, 230, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar6 = pgt.SliderBar(screen, (100, 565 + 300), a_kd, 230, 0.5, 5, (200, 200, 200), (255, 10, 10)) sbar7 = pgt.SliderBar(screen, (100, 580 + 300), noise_amplitude, 230, 2, 5, (200, 200, 200), (255, 10, 10)) sbar8 = pgt.SliderBar(screen, (100, 595 + 300), acc_g, 230, 9.81, 5, (200, 200, 200), (255, 10, 10))
pki_o = 0.4 pkd_o = 0 akp_o = 0.68 aki_o = 0.4 akd_o = 0.01 FW_o = 2 FW = FW_o FW_old = FW_o pkp_old = pkp_o pki_old = pki_o pkd_old = pkd_o akp_old = akp_o aki_old = aki_o akd_old = akd_o sbar = pgt.SliderBar(screen, (100, 455), pkp_o, 460, 5, 6, (200, 200, 200), (255, 10, 10)) sbar2 = pgt.SliderBar(screen, (100, 480), pki_o, 460, 5, 6, (200, 200, 200), (255, 10, 10)) sbar3 = pgt.SliderBar(screen, (100, 505), pkd_o, 460, 0.5, 6, (200, 200, 200), (255, 10, 10)) sbar4 = pgt.SliderBar(screen, (100, 555), akp_o, 460, 5, 6, (200, 200, 200), (255, 10, 10)) sbar5 = pgt.SliderBar(screen, (100, 580), aki_o, 460, 5, 6, (200, 200, 200), (255, 10, 10)) sbar6 = pgt.SliderBar(screen, (100, 605), akd_o, 460, 0.5, 6, (200, 200, 200), (255, 10, 10)) sbar7 = pgt.SliderBar(screen, (100, 655), FW, 460, 100, 4, (200, 200, 200), (255, 10, 10)) pos_pid = pid.pid(pkp_o, pki_o, pkd_o, [-10, 10], [0, 20], dt)
FW2_old = 2 pkp2_old = pkp2_o pki2_old = pki2_o pkd2_old = pkd2_o akp2_old = akp2_o aki2_old = aki2_o akd2_old = akd2_o pos2_pid = pid.pid(pkp2_o, pki2_o, pkd2_o, [-10, 10], [0, 20], dt) angle2_pid = pid.pid(akp2_o, aki2_o, akd2_o, [-15, 15], [-60, 60], dt) #----------------------------------------------------------------------- # Create slider bars #----------------------------------------------------------------------- barcolour = (150, 150, 150) spotcolour = (255, 10, 0) sbar = pgt.SliderBar(screen, (90, 450 - 30), pkp1_o, 180, 5, 4, barcolour, spotcolour) sbar2 = pgt.SliderBar(screen, (90, 475 - 30), pki1_o, 180, 5, 4, barcolour, spotcolour) sbar3 = pgt.SliderBar(screen, (90, 500 - 30), pkd1_o, 180, 0.5, 4, barcolour, spotcolour) sbar4 = pgt.SliderBar(screen, (90, 525 - 30), akp1_o, 180, 5, 4, barcolour, spotcolour) sbar5 = pgt.SliderBar(screen, (90, 550 - 30), aki1_o, 180, 5, 4, barcolour, spotcolour) sbar6 = pgt.SliderBar(screen, (90, 575 - 30), akd1_o, 180, 0.5, 4, barcolour, spotcolour) sbar7 = pgt.SliderBar(screen, (90, 600 - 30), FW1, 180, 100, 4, barcolour, spotcolour) sbar8 = pgt.SliderBar(screen, (400, 450 - 30), pkp2_o, 180, 5, 4, barcolour, spotcolour)
FW_old = FW_o pkp_old = pkp_o pki_old = pki_o pkd_old = pkd_o akp_old = akp_o aki_old = aki_o akd_old = akd_o pos_pid = pid.pid(pkp_o, pki_o, pkd_o, [-10, 10], [0, 20], dt) angle_pid = pid.pid(akp_o, aki_o, akd_o, [-15, 15], [-60, 60], dt) #----------------------------------------------------------------------# # Create slider bars #----------------------------------------------------------------------# barcolour = (150, 150, 150) spotcolour = (255, 10, 0) sbar = pgt.SliderBar(screen, (100, 450 - 30), pkp_o, 460, 5, 4, barcolour, spotcolour) sbar2 = pgt.SliderBar(screen, (100, 475 - 30), pki_o, 460, 5, 4, barcolour, spotcolour) sbar3 = pgt.SliderBar(screen, (100, 500 - 30), pkd_o, 460, 0.5, 4, barcolour, spotcolour) sbar4 = pgt.SliderBar(screen, (100, 525 - 30), akp_o, 460, 5, 4, barcolour, spotcolour) sbar5 = pgt.SliderBar(screen, (100, 550 - 30), aki_o, 460, 5, 4, barcolour, spotcolour) sbar6 = pgt.SliderBar(screen, (100, 575 - 30), akd_o, 460, 0.5, 4, barcolour, spotcolour) sbar7 = pgt.SliderBar(screen, (100, 600 - 30), FW, 460, 100, 4, barcolour, spotcolour) #----------------------------------------------------------------------#