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