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