def amplitude(self, *inputs):
inputs = inputs[0]
self.s_kp = inputs[0]
self.s_ki = inputs[1]
self.s_kd = inputs[2]
self.a_kp = inputs[3]
self.a_ki = inputs[4]
self.a_kd = inputs[5]
self.speed_pid = pid.pid(self.s_kp, self.s_ki, self.s_kd, [-10, 10],
[-5, 5], self.dt)
self.angle_pid = pid.pid(self.a_kp, self.a_ki, self.a_kd, [-6, 6],
[-1, 1], self.dt)
v1 = np.array([[]] * 1).T
theta = 0.99 * np.pi
acc = 0
omega = 0
velocity = 0
distance = 0
for ii in range(1000):
if theta >= np.pi / 1.845 and theta <= 1.43 * np.pi:
alpha = -np.sin(theta) * self.g / self.h
gamma = -np.cos(theta) * acc / self.h
a_acc = alpha - gamma
# integrate angular acceleration to get angular velocity
omega += a_acc * self.dt
# integrate angular velocity to get angle
theta += omega * self.dt
# integrate dt to get time
velocity += acc * self.dt
distance += velocity * self.dt
noise = np.random.rand(1) * np.pi / 180
noise = [1]
settheta = -self.speed_pid.output(
geom.v2ang(h, g, targetvelocity),
-geom.v2ang(h, g, velocity), self.dt)
acc = -self.angle_pid.output(np.pi + settheta,
(theta + noise[0]), self.dt)
v1 = np.vstack((v1, (theta - np.pi) * distance))
self.v1 = v1
self.amp = np.abs(v1).max()
#sf = 0.165
#s_kpo, s_kio, s_kdo = 0.075, 0.94, 0.022
#a_kpo, a_kio, a_kdo = 3.03, 0.0096, 0.067
#------------------------ Tuning for Earth -----------------------------
sf = 1
s_kpo, s_kio, s_kdo = 0.090, 0.256, 0.00
a_kpo, a_kio, a_kdo = 12.651, 0.072, 0.311
# s_kpo, s_kio, s_kdo = 0.026, 0.174, 0.00
# a_kpo, a_kio, a_kdo = 20.0, 0.03, 0.311
#-----------------------------------------------------------------------
sf_original = sf
s_kp, s_ki, s_kd = s_kpo, s_kio, s_kdo
a_kp, a_ki, a_kd = a_kpo, a_kio, a_kdo
speed_pid = pid.pid(s_kp, s_ki, s_kd, [-10, 10], [-5, 5], dt)
angle_pid = pid.pid(a_kp, a_ki, a_kd, [-6, 6], [-2, 2], dt)
BLACK = pygame.Color('black')
WHITE = pygame.Color('white')
GRAY = pygame.Color('gray')
RED = pygame.Color('red')
save = 0
show_arrows = 0
draw_stick_man = 1
#-----------------------------------------------------------------------
# Physical constants
#-----------------------------------------------------------------------
acc_g = 9.81
l = 0.045 # distance between the centre of gravity of the T-Bot and the axil
R = 0.024 # Radius of wheels
C = 1 # Friction
distance = 0
theta = 0.05
height_of_man = 1.8923 # Me
#height_of_man = 0.1524 # 1:12 Scale (approx. 5-6") Action Figure
height_of_man = 0.0508 # 1:48 Scale (approx. 2") Action Figure
Tbot_scalefactor = 216
height_of_TBot_body = 120E-3
Man_scalefactor = (height_of_man / (l * 2)) * Tbot_scalefactor
wheel_radius = int(R / l * Tbot_scalefactor / 2.2)
draw_stick_man = 1
tyre = 4
#-----------------------------------------------------------------------
# PID Cintrol
#-----------------------------------------------------------------------
targetvelocity = 0
speed_pid = pid.pid(0.040, 0.20, 0.00, [-10, 10], [-5, 5], dt)
angle_pid = pid.pid(12.00, 0.00, 0.20, [6, 6], [-1, 1], dt)
#-----------------------------------------------------------------------
# Drawing Geometry
#-----------------------------------------------------------------------
geom = geometry.geometry()
origin = [500, 319]
tbot_drawing_offset = [-78, -10]
tbot = np.loadtxt('T-BotSideView.dat')
tbot = np.vstack(
(tbot,
tbot[0, :])) + tbot_drawing_offset # closes the shape and adds an offset
tbot = tbot / (tbot[:, 1].max() - tbot[:, 1].min()) * Tbot_scalefactor
spokes = np.array([[0, 1], [0, 0], [0.8660254, -0.5], [0, 0],
[-0.8660254, -0.5], [0, 0]]) * (wheel_radius - tyre)
trackmarksArray = np.array([[0, origin[1] + wheel_radius],
iii = 1 loopcount = 0 pathindex = 0 timeflag = 0 pathindex = 0 rotspeed = 200 speedfactor = 0.3 turnspeedfactor = 0.3 turntime = 0.005 bendscalefactor = 10 rdeadban = 2 tolerance = 30 feedforward = 10 pos_pid = pid.pid(0.1, 0.4, 0, [-10, 10], [0, 40], turntime) angle_pid = pid.pid(0.4, 2.40, 0.01, [-15, 15], [-60, 60], turntime) #----------------------------------------------------------------------# # Set HSV Thresholds # # Artificial Lighting #----------------------------------------------------------------------# #greenLower = (36,42,228) #greenUpper = (74,255,255) #pinkLower = (143,70,113) #pinkUpper = (255,255,255) #----------------------------------------------------------------------# # Sunny #----------------------------------------------------------------------#
(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)
angle_pid = pid.pid(akp_o, aki_o, akd_o, [-15, 15], [-60, 60], dt)
pygame.display.set_caption("Tuning")
if __name__ == '__main__':
success, frame = cap.read()
if not success:
print('Failed to capture video')
sys.exit(1)
#####################################################
#----------------- Track T-Bot -------------------#
#####################################################
while cap.isOpened():
pkp1_o = 2.02 pki1_o = 0.4 pkd1_o = 0 akp1_o = 0.68 aki1_o = 0.4 akd1_o = 0.01 FW1_o = 2 FW1 = FW1_o FW1_old = 2 pkp1_old = pkp1_o pki1_old = pki1_o pkd1_old = pkd1_o akp1_old = akp1_o aki1_old = aki1_o akd1_old = akd1_o pos1_pid = pid.pid(pkp1_o, pki1_o, pkd1_o, [-10, 10], [0, 20], dt) angle1_pid = pid.pid(akp1_o, aki1_o, akd1_o, [-15, 15], [-60, 60], dt) pkp2_o = 2.02 pki2_o = 0.4 pkd2_o = 0 akp2_o = 0.68 aki2_o = 0.4 akd2_o = 0.01 FW2_o = 2 FW2 = FW1_o FW2_old = 2 pkp2_old = pkp2_o pki2_old = pki2_o pkd2_old = pkd2_o akp2_old = akp2_o
