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
示例#2
0
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
示例#3
0
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
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():
	#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
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