def update_position(self):
"""Updates the postion of the robot"""
try:
self.center, self.head, self.theta = aruco_lib.get_marker_angle(
frame, self.destination, self.MARKER_ID)
except Exception as e:
#print(e)
self.theta = 0
def send_info(self):
global frame
self.pid.set_parameters()
try:
self.center_x, self.center_y, self.head_x, self.head_y, self.theta = aruco_lib.get_marker_angle(
frame, (self.dest_x, self.dest_y), self.MARKER_ID)
except Exception as e:
self.theta = 0
#print(e)
self.packet.push(('T', self.dest_x, self.dest_y))
self.packet.push(('P', self.pid.kp, self.pid.ki, self.pid.kd))
self.packet.push(
('R', self.center_x, self.center_y, self.head_x, self.head_y))
self.packet.push(('A', self.theta + 180))
packet_data = self.packet.createPacket()
comm.tx(packet_data, self.ADDR)
cv2.line(frame, (self.dest_x, self.dest_y),
(self.center_x, self.center_y), (255, 255, 255), 1)
cv2.circle(frame, (self.dest_x, self.dest_y),
5, (0, 0, 255),
thickness=-1)
cv2.circle(frame, (self.center_x, self.center_y),
5, (0, 255, 0),
thickness=-1)
cv2.circle(frame, (self.head_x, self.head_y),
5, (0, 255, 0),
thickness=-1)
self.packet.resetData()
head_dist = self.get_distance(self.dest_x, self.dest_y)
if head_dist <= 20:
self.reached = True
def move_towards_destination(self):
"""Sends data packets containing the error angle for pid and the pid tunning parameters"""
global frame
global blocks
global robots
self.stop_bit = 0
#if collision set stop bit to 1
if self.collision:
self.stop_bit = 1
#binary image consiting of obstacles with pixel values less than 127
self.blocks_map = cv2.resize(blocks.copy(),
downscale(ARENA_SIZE),
interpolation=cv2.INTER_CUBIC)
#get the pid tuinning values from the gui trackbar
self.pid.set_parameters()
self.update_position()
#find the distance of destination from center
head_dist = self.get_distance(self.destination, "center")
#if the distance is less than 10 than stop the robot and set self.reached to true
if head_dist <= 10:
self.reached = True
self.stop_bit = 1
else:
try:
#find the path from robot center to destination
self.path = astar(self.blocks_map, downscale(self.center),
downscale(self.destination))
#select the first element in the path list as the intermidiate destination
self.intermediate_y, self.intermediate_x = upscale(
self.path[1])
except Exception as e:
#if path not found and the destination near set the destination as intermidiate destination
if head_dist < 30:
self.intermediate_x, self.intermediate_y = self.destination
try:
self.center, self.head, self.theta = aruco_lib.get_marker_angle(
frame, (self.intermediate_x, self.intermediate_y),
self.MARKER_ID)
except Exception as e:
#print(e)
self.theta = 0
#empty packets list
self.packet.resetData()
#push the pid tunning values to the packets list
self.packet.push(('P', self.pid.kp, self.pid.ki, self.pid.kd))
#push the error angle to the packets list
self.packet.push(('A', self.theta + 180))
#push the stop bit to the packets list
self.packet.push(('S', self.stop_bit))
#create the data packet
data_packet = self.packet.createPacket()
#send the data packet to the robot
comm.tx(data_packet, self.ADDR)