def proportion_controller():
#This creates a new thread to update the repulsion variable
sub = threading.Thread(target=repulsion_subscriber)
sub.start()
#Setting up the command publisher
cmd_pub = rospy.Publisher("cmd_hex", RobCMD, queue_size=100)
rate = rospy.Rate(10.0)
#These are the constants used in the PI controller
ki = 0.3
integral_ang = 0
try:
while not rospy.is_shutdown():
kp = 1.5
#Accounting for all global variables
global repulsion
global obst
#print "Current repulse: %r" % repulsion
command = RobCMD()
point = get_target()
#path[0] is direction, path[1] is magnitude\
print point.point.x
print point.point.y
#Adjusts the position of the robot's destination when it needs to
#avoid another robot
if abs(calc_mag([point.point.x, point.point.y], [0, 0])) > abs(
calc_mag([obst.point.x, obst.point.y], [0, 0])):
path = build_vector([(point.point.x + repulsion[0]) * 1000,
(point.point.y + repulsion[1]) * 1000],
[0, 0])
else:
path = build_vector(
[point.point.x * 1000, point.point.y * 1000], [0, 0])
print "Path vector: %r " % path
angle = path[0]
mag = path[1]
#Calculating the speed of the robot's turn based on the angle
#to the target
proportional_ang = (kp * angle)
integral_ang += (ki * angle)
if integral_ang >= 255:
integral_ang = 255
if integral_ang <= -255:
integral_ang = -255
turn_speed = proportional_ang + integral_ang
if turn_speed >= 255:
turn_speed = 255
if turn_speed <= -255:
turn_speed = -255
#Tells the robot to turn left or right
turn_direction = _clockwise(path)
rospy.loginfo(NAME)
rospy.loginfo("Turning Speed: %r" % turn_speed)
rospy.loginfo("Turning Direction: %r" % turn_direction)
#Preparing the command to send the robot
command.code = 4
command.turn = turn_direction
command.accel = int(abs(turn_speed))
command.magnitude = 0
command.direction = 0
#Moves the robot forward if it is facing the target
if abs(angle) < 20:
command.magnitude = 100
#Stops the robot when it reaches the target, and requests
#the next target.
if abs(mag) < 50:
command.magnitude = 0
command.direction = 0
command.turn = 0
command.accel = 0
cmd_pub.publish(command)
while get_point() == point:
print "HERE"
time.sleep(.5)
cmd_pub.publish(command)
rate.sleep()
rate.sleep()
except KeyboardInterrupt:
raise
def proportion_controller(target, current):
#path[0] is direction, path[1] is magnitude
cmd_pub = rospy.Publisher("/cmd_hex", RobCMD, queue_size=100)
#rospy.init_node('PID_contr', anonymous = True)
rate = rospy.Rate(10.0)
#proportional constant
kp = 2.5
ki = 0.1
kpm = .3
#kd = 10
integral_ang = 0
integral_mag = 0
prevAngle = 0
path = build_vector(target, current)
try:
while path[0] != 0:
command = RobCMD()
point = get_target()
target[0] = point.point.x * 1000
target[1] = point.point.y * 1000
path = build_vector(target, current)
angle = path[0]
proportional_ang = (kp * angle)
integral_ang += (ki * angle)
#derivative = kd*(prevAngle - angle)
turn_speed = proportional_ang + integral_ang #derivative
#magnitude = path[1]
#proportional_mag = (kpm * magnitude)
#integral_mag += (ki * magnitude)
#mag = proportional_mag + integral_mag
print "angle: %r" % angle
if integral_ang >= 255:
integral_ang = 255
if integral_ang <= -255:
integral_ang = -255
if integral_mag >= 255:
integral_mag = 255
if integral_mag <= -255:
integral_mag = -255
if turn_speed >= 255:
turn_speed = 255
if turn_speed <= -255:
turn_speed = -255
#if mag >= 255:
# mag = 255
#if mag<= -255:
# mag= -255
#if mag >= 0:
# command.direction = 0
#if mag<= 0:
# command.direction = 1
prevAngle = angle
turn_direction = _clockwise(path)
rospy.loginfo(turn_speed)
rospy.loginfo(turn_direction)
command.code = 4
#command.magnitude = mag
command.turn = turn_direction
command.accel = int(abs(turn_speed))
command.magnitude = 0
command.direction = 0
if path[0] < 10 and path[0] > -10:
command.turn = 0
command.accel = 0
print "Aligned"
command.magnitude = 70
command.direction = 0
if path[1] < 50 and path[1] > -50:
command.magnitude = 0
command.direction = 0
cmd_pub.publish(command)
rate.sleep()
rate.sleep()
except KeyboardInterrupt:
raise
def proportion_controller():
#Initializing the command publisher
global point
cmd_pub = rospy.Publisher("cmd_hex", RobCMD, queue_size=100)
rate = rospy.Rate(10.0)
#Setting the PI constants
kp = 1.2
ki = 0.1
integral_ang = 0
t = 0
try:
while not rospy.is_shutdown():
point = get_target()
command = RobCMD()
t = t + .005
path = build_vector([point.point.x * 1000, point.point.y * 1000],
[0, 0])
print "Path vector: %r " % path
angle = path[0]
mag = path[1]
proportional_ang = (kp * angle)
integral_ang += (ki * angle)
#Setting the turn speed
if integral_ang >= 255:
integral_ang = 255
if integral_ang <= -255:
integral_ang = -255
turn_speed = proportional_ang + integral_ang
if turn_speed >= 255:
turn_speed = 255
if turn_speed <= -255:
turn_speed = -255
#Setting the turn to clockwise or counterclockwise
turn_direction = _clockwise(path)
rospy.loginfo(NAME)
rospy.loginfo("Turning Speed: %r" % turn_speed)
rospy.loginfo("Turning Direction: %r" % turn_direction)
command.code = 4
command.turn = turn_direction
command.accel = int(abs(turn_speed))
command.magnitude = 0
command.direction = 0
#The robot moves forward when it is facing the target
if abs(angle) < 40:
command.magnitude = 100
#The robot stops once it reaches the target
if abs(mag) < 50:
command.magnitude = 0
command.direction = 0
command.turn = 0
command.accel = 0
cmd_pub.publish(command)
#while get_point() == point:
#time.sleep(.5)
cmd_pub.publish(command)
rate.sleep()
rate.sleep()
except KeyboardInterrupt:
raise
