def pid_tuner():
pub = rospy.Publisher('robot1Com', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
do_action()
message = piddata()
for i in vars(T):
if i in vars(message):
setattr(message, i, eval('T.{}'.format(i)))
message.cmdType = 'pid'
rospy.loginfo(message.cmdType)
pub.publish(message)
rate.sleep()
def pid_tuner():
pub = rospy.Publisher('robot1Com', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
do_action()
message = piddata()
for i in vars(T):
if i in vars(message):
setattr(message, i, eval('T.{}'.format(i)))
message.cmdType = 'pid'
rospy.loginfo(message.cmdType)
pub.publish(message)
rate.sleep()
def do_action():
message = piddata()
action = get_action()
# Display commands (help)
if action == 'HELP':
print("Change tuning variable with 1,2,3")
print("Tune kp with < >")
print("Tune kd with k l")
print("Tune ki with i o")
print("Press l to show current gains")
print("Press s to change tuning sensitivity")
print("Press Space to change commanded position")
print("Press h to bring up the commands (this menu)")
print("Press q to quit")
elif action == 'QUIT':
quit()
# Switch tuning variable
elif action == 'X':
T.tuningVariable = 'x'
print("changed tuning variable to x")
elif action == 'Y':
T.tuningVariable = 'y'
print("changed tuning variable to y")
elif action == 'THETA':
T.tuningVariable = 'theta'
print("changed tuning variable to theta")
# change tuning paramaters for current tuning variable
elif action == 'KP_DOWN':
if T.tuningVariable == 'x':
T.kp_x = T.kp_x - T.tuningSensitivity
print("kp_x: " + str(T.kp_x))
elif T.tuningVariable == 'y':
T.kp_y = T.kp_y - T.tuningSensitivity
print("kp_y: " + str(T.kp_y))
elif T.tuningVariable == 'theta':
T.kp_theta = T.kp_theta - T.tuningSensitivity
print("kp_theta: " + str(T.kp_theta))
elif action == 'KP_UP':
if T.tuningVariable == 'x':
T.kp_x = T.kp_x + T.tuningSensitivity
print("kp_x: " + str(T.kp_x))
elif T.tuningVariable == 'y':
T.kp_y = T.kp_y + T.tuningSensitivity
print("kp_y: " + str(T.kp_y))
elif T.tuningVariable == 'theta':
T.kp_theta = T.kp_theta + T.tuningSensitivity
print("kp_theta: " + str(T.kp_theta))
elif action == 'KD_DOWN':
if T.tuningVariable == 'x':
T.kd_x = T.kd_x - T.tuningSensitivity
print("kd_x: " + str(T.kd_x))
elif T.tuningVariable == 'y':
T.kd_y = T.kd_y - T.tuningSensitivity
print("kd_y: " + str(T.kd_y))
elif T.tuningVariable == 'theta':
T.kd_theta = T.kd_theta - T.tuningSensitivity
print("kd_theta: " + str(T.kd_theta))
elif action == 'KD_UP':
if T.tuningVariable == 'x':
T.kd_x = T.kd_x + T.tuningSensitivity
print("kd_x: " + str(T.kd_x))
elif T.tuningVariable == 'y':
T.kd_y = T.kd_y + T.tuningSensitivity
print("kd_y: " + str(T.kd_y))
elif T.tuningVariable == 'theta':
T.kd_theta = T.kd_theta + T.tuningSensitivity
print("kd_theta: " + str(T.kd_theta))
elif action == 'KI_DOWN':
if T.tuningVariable == 'x':
T.ki_x = T.ki_x - T.tuningSensitivity
print("ki_x: " + str(T.ki_x))
elif T.tuningVariable == 'y':
T.ki_y = T.ki_y - T.tuningSensitivity
print("ki_y: " + str(T.ki_y))
elif T.tuningVariable == 'theta':
T.ki_theta = T.ki_theta - T.tuningSensitivity
print("ki_theta: " + str(T.ki_theta))
elif action == 'KI_UP':
if T.tuningVariable == 'x':
T.ki_x = T.ki_x + T.tuningSensitivity
print("ki_x: " + str(T.ki_x))
elif T.tuningVariable == 'y':
T.ki_y = T.ki_y + T.tuningSensitivity
print("ki_y: " + str(T.ki_y))
elif T.tuningVariable == 'theta':
T.ki_theta = T.ki_theta + T.tuningSensitivity
print("ki_theta: " + str(T.ki_theta))
elif action == 'LIST':
print("kp_x: " + str(T.kp_x))
print("kd_x: " + str(T.kd_x))
print("ki_x: " + str(T.ki_x))
print("\n")
print("kp_y: " + str(T.kp_y))
print("kd_y: " + str(T.kd_y))
print("ki_y: " + str(T.ki_y))
print("\n")
print("kp_theta: " + str(T.kp_theta))
print("kd_theta: " + str(T.kd_theta))
print("ki_theta: " + str(T.ki_theta))
elif action == 'SENSITIVITY':
T.tuningSensitivity = input("Tuning Sensitivity: ")
def do_action():
message = piddata()
action = get_action()
# Display commands (help)
if action == 'HELP':
print("Change tuning variable with 1,2,3")
print("Tune kp with < >")
print("Tune kd with k l")
print("Tune ki with i o")
print("Press l to show current gains")
print("Press s to change tuning sensitivity")
print("Press Space to change commanded position")
print("Press h to bring up the commands (this menu)")
print("Press q to quit")
elif action == 'QUIT':
quit()
# Switch tuning variable
elif action == 'X':
T.tuningVariable = 'x'
print("changed tuning variable to x")
elif action == 'Y':
T.tuningVariable = 'y'
print("changed tuning variable to y")
elif action == 'THETA':
T.tuningVariable = 'theta'
print("changed tuning variable to theta")
# change tuning paramaters for current tuning variable
elif action == 'KP_DOWN':
if T.tuningVariable == 'x':
T.kp_x = T.kp_x - T.tuningSensitivity
print("kp_x: " + str(T.kp_x))
elif T.tuningVariable == 'y':
T.kp_y = T.kp_y - T.tuningSensitivity
print("kp_y: " + str(T.kp_y))
elif T.tuningVariable == 'theta':
T.kp_theta = T.kp_theta - T.tuningSensitivity
print("kp_theta: " + str(T.kp_theta))
elif action == 'KP_UP':
if T.tuningVariable == 'x':
T.kp_x = T.kp_x + T.tuningSensitivity
print("kp_x: " + str(T.kp_x))
elif T.tuningVariable == 'y':
T.kp_y = T.kp_y + T.tuningSensitivity
print("kp_y: " + str(T.kp_y))
elif T.tuningVariable == 'theta':
T.kp_theta = T.kp_theta + T.tuningSensitivity
print("kp_theta: " + str(T.kp_theta))
elif action == 'KD_DOWN':
if T.tuningVariable == 'x':
T.kd_x = T.kd_x - T.tuningSensitivity
print("kd_x: " + str(T.kd_x))
elif T.tuningVariable == 'y':
T.kd_y = T.kd_y - T.tuningSensitivity
print("kd_y: " + str(T.kd_y))
elif T.tuningVariable == 'theta':
T.kd_theta = T.kd_theta - T.tuningSensitivity
print("kd_theta: " + str(T.kd_theta))
elif action == 'KD_UP':
if T.tuningVariable == 'x':
T.kd_x = T.kd_x + T.tuningSensitivity
print("kd_x: " + str(T.kd_x))
elif T.tuningVariable == 'y':
T.kd_y = T.kd_y + T.tuningSensitivity
print("kd_y: " + str(T.kd_y))
elif T.tuningVariable == 'theta':
T.kd_theta = T.kd_theta + T.tuningSensitivity
print("kd_theta: " + str(T.kd_theta))
elif action == 'KI_DOWN':
if T.tuningVariable == 'x':
T.ki_x = T.ki_x - T.tuningSensitivity
print("ki_x: " + str(T.ki_x))
elif T.tuningVariable == 'y':
T.ki_y = T.ki_y - T.tuningSensitivity
print("ki_y: " + str(T.ki_y))
elif T.tuningVariable == 'theta':
T.ki_theta = T.ki_theta - T.tuningSensitivity
print("ki_theta: " + str(T.ki_theta))
elif action == 'KI_UP':
if T.tuningVariable == 'x':
T.ki_x = T.ki_x + T.tuningSensitivity
print("ki_x: " + str(T.ki_x))
elif T.tuningVariable == 'y':
T.ki_y = T.ki_y + T.tuningSensitivity
print("ki_y: " + str(T.ki_y))
elif T.tuningVariable == 'theta':
T.ki_theta = T.ki_theta + T.tuningSensitivity
print("ki_theta: " + str(T.ki_theta))
elif action == 'LIST':
print("kp_x: " + str(T.kp_x))
print("kd_x: " + str(T.kd_x))
print("ki_x: " + str(T.ki_x))
print("\n")
print("kp_y: " + str(T.kp_y))
print("kd_y: " + str(T.kd_y))
print("ki_y: " + str(T.ki_y))
print("\n")
print("kp_theta: " + str(T.kp_theta))
print("kd_theta: " + str(T.kd_theta))
print("ki_theta: " + str(T.ki_theta))
elif action == 'SENSITIVITY':
T.tuningSensitivity = input("Tuning Sensitivity: ")
