def __init__(self):
server = ServerControl(self)
server.setDaemon(True)
server.start()
self.controller = RobotController(self)
self.controller.run()
def main():
#wvs.stop()
#wvs.release()
#TODO detect the count of forms
# 1) Create a webcam video stream
# Note: src=0 links to the USB Webcam of the computers provided for the project
webcam = True
if webcam:
#wvs =
images = get_images()
im_names = ["Analysis image"]
else:
ic = skimage.io.imread_collection("data/test1_1.png")
images = skimage.io.concatenate_images(ic)
xmin = 85
xmax = 570
ymin = 60
ymax = 425
images = np.copy(images[:, ymin:ymax, xmin:xmax, :])
im_names = ["Test image"]
# 2) first image analysis
print("Doing first image analysis")
global imageAnalysis
imageAnalysis = ImageAnalysis(images,
im_names,
result_dir=RESULT_DIR,
print_local=print_local,
save_to_file=SAVE_TO_FILE,
savefig=SAVE_FIG)
imageAnalysis.image_analysis()
figures_info = imageAnalysis.figures_info[
0] # (center, number, matching_pair, bbox)
circle_info = imageAnalysis.circle_info[0] # (index, bbox_center)
arrow_info = imageAnalysis.arrow_info[
0] # (bbox_center, orientation, direction_x)
# 3) get target list and store it
print("Getting target list")
target_list = get_target_list(figures_info, circle_info)
if len(target_list) < NUMBER_OF_FIG:
print("WARNING: not enough figures have been detected")
print("Saving gameplan")
plt.imshow(images[0])
for index in range(len(target_list) - 1):
x0 = target_list[index][1][0]
x1 = target_list[index + 1][1][0]
y0 = target_list[index][1][1]
y1 = target_list[index + 1][1][1]
plt.plot([y0, y1], [x0, x1], 'ro-')
plt.savefig("gameplan.png")
plt.close()
print("Calibrating robot")
# 4) calibrate robot
global robotController
robotController = RobotController()
robot_info_cal = get_robot_info()
robotController.calibration()
robot_info = get_robot_info()
robotController.calcFactorCalibr(
robot_info_cal[0], robot_info_cal[1], robot_info_cal[2], robot_info[0],
robot_info[1], robot_info[2]) # (x_i, y_i, theta_i, x_f, y_f, theta_f)
# Target are stored as : (key, center, is_number)
print("Executing task")
# 5) Execute task
for index, target in enumerate(target_list):
target_point = target[1]
print("Target:", target_point)
x_t = target_point[1]
y_t = target_point[0]
is_number = target[2]
if target_point[0] is "circle":
finish = True
else:
finish = False
print("New target: ", target_point)
print("Finish")
on_shape = False
while not on_shape:
robotController.GoTo(robot_info[0], robot_info[1], robot_info[2],
x_t, y_t)
robot_info = get_robot_info()
if CORR_FACTORS:
robotController.corrFactors(robot_info[0], robot_info[1],
robot_info[2])
on_shape = robotController.checkOnTheShape(robot_info[0],
robot_info[1],
is_number,
finish=finish,
dist_min=DIST_MIN)
class Robot:
robot_state = 'Q'
robot_states = {'stop':'Q','forward':'W',
'reverse':'S','right':'D','left':'A'}
motor_currents = [0, 0, 0, 0]
motor_targets = [0, 0, 0, 0]
forward_state = 0 # pos: forward | neg: back
turn_state = 0 # pos: left | neg: right
forward_target = 0
turn_target = 0
FORWARD_SPEED = 30
BACK_SPEED = -20
TURN_SPEED = 10
arm_state = 'N'
arm_states = {'stop':'N','down':'O','up':'K'}
arm_current = 0
arm_target = 0
bucket_state = 'M'
bucket_states = {'stop':'M','down':'L','up':'P'}
bucket_current = 0
bucket_target = 0
run_control = True
kill_all = True
update_rate = 0.02
## motor_delta = 1
## MAX_MOTOR = 45
def __init__(self):
server = ServerControl(self)
server.setDaemon(True)
server.start()
self.controller = RobotController(self)
self.controller.run()
def set_arm_state(self, state):
self.arm_state = state
if self.arm_state == 'N':
self.arm_target = 0
elif self.arm_state == 'K':
self.arm_target = -1
elif self.arm_state == 'O':
self.arm_target = 1
def set_bucket_state(self, state):
self.bucket_state = state
if self.bucket_state == 'M':
self.bucket_target = 0
elif self.bucket_state == 'L':
self.bucket_target = -1
elif self.bucket_state == 'P':
self.bucket_target = 1
def set_robot_state(self, state):
if self.robot_state != state:
self.robot_state = state
if self.robot_state == 'Q':
# All motors off
self.forward_target = 0
self.turn_target = 0
elif self.robot_state == 'W':
# All motors forward
print 'forw targ:',self.FORWARD_SPEED
self.forward_target = self.FORWARD_SPEED
self.turn_target = 0
elif self.robot_state == 'S':
# All motors backwards
self.forward_target = self.BACK_SPEED
self.turn_target = 0
elif self.robot_state == 'D':
# forward right
self.forward_target = self.FORWARD_SPEED
self.turn_target = -(self.TURN_SPEED)
elif self.robot_state == 'A':
# forward left
self.forward_target = self.FORWARD_SPEED
self.turn_target = self.TURN_SPEED
elif self.robot_state == 'Z':
# back right
self.forward_target = self.BACK_SPEED
self.turn_target = -(self.TURN_SPEED)
elif self.robot_state == 'C':
# back left
self.forward_target = self.BACK_SPEED
self.turn_target = self.TURN_SPEED
def all_motors_off(self):
self.robot_state = 'Q'
self.forward_target = 0
self.turn_target = 0
self.arm_state = 'N'
self.arm_target = 0
self.bucket_state = 'M'
self.bucket_target = 0
self.kill_all = True
