def __init__(self,nomodeset=0):
# Call class ___________________________________________________________________________________________________
if nomodeset == 1:
self.KHONG = Board('COM7',115200)
elif nomodeset == 2:
self.CAMER = Dynamixel('COM3',1000000)
elif nomodeset == 0:
self.KHONG = Board('COM7',115200)
self.CAMER = Dynamixel('COM3',1000000)
else:
raise NameError('NoMode not found (your mode is ',nomodeset)
# h**o = Get_Position.h**o
self.rtp = Real_Time_Predict()
# self.rtp.create_camera_instance(0)
self.rtp.create_HogDescriptor()
self.convert = Get_Position.World()
self.newcammtx = Get_Position.newcammtx
def __init__(self):
self.motor = Dynamixel.DynamixelMan(PORT, BAUDRATE)
time.sleep(DELAY_SAFE_DATA)
self.motor.SetMovingSpeed(MOTOR_ID, BASE_MOTOR_SPEED)
class Main:
class Const:
# Const. Position
Homei = [180, 70, 30, 90, 60, 90]
Home = [180, 90, 30, 90, 60, 90]
Front = [5, 80, 30, 90, 60, 90] ##
Right = [90, 80, 30, 90, 60, 90]
# Variable _____________________________________________________________________________________________________
CameraRight = [[-50, -70],
[90, -30], [90, -10], [90, 10], [90, 25],
[75, 22], [70, 10], [70, 0], [70, -10], [70, -20],
[60, -20], [60, -10], [55, 0], [55, 10], [55, 20], [55, 25]]
CameraBase = [[-35, -50], [-17, -60], [0, -59], [17, -60], [35, -50]]
CameraLeft = [[-70, 22], [-70, 0], [-60, 0], [-60, 10], [-60, -10], [-60, -20],
[-70, -20], [-70, -10], [-70, 0], [-70, 10], [-75, 22],
[-90, 25], [-90, 10], [-90, -10], [-90, -30]]
def __init__(self,nomodeset=0):
# Call class ___________________________________________________________________________________________________
if nomodeset == 1:
self.KHONG = Board('COM7',115200)
elif nomodeset == 2:
self.CAMER = Dynamixel('COM3',1000000)
elif nomodeset == 0:
self.KHONG = Board('COM7',115200)
self.CAMER = Dynamixel('COM3',1000000)
else:
raise NameError('NoMode not found (your mode is ',nomodeset)
# h**o = Get_Position.h**o
self.rtp = Real_Time_Predict()
# self.rtp.create_camera_instance(0)
self.rtp.create_HogDescriptor()
self.convert = Get_Position.World()
self.newcammtx = Get_Position.newcammtx
# Function _________________________________________________________________________________________________________
def camera(self,pantilt):
self.CAMER.PAN(pantilt[0])
self.CAMER.TILT(pantilt[1])
self.CAMER.WaitFinish([1,9])
def cmKhong(self,position):
self.KHONG.SetPosition(position)
time.sleep(0.1)
#KHONG.WaitFinish()
def cam_clf(self, DATA_PACK, brl, pt):
self.rtp.create_camera_instance(0)
# open camera and read model and predict get centroid of picture and 4 points of conner
cardlist, midpointlist, cornerlist, realworldlist = self.rtp.one_time()
# get_homo, put pantile's list by pan is q1 and tilt is q2 , blr is scene ('l', 'r', 'blbr')
h**o = blr.get_homo(q1_=pt[0], q2_=pt[1], blr_=brl)
# realworldlist is the function that convert centroid of picture to centroid of picture with respect to world coordinate
realworldlist = convert_pos(midpointlist, newcammtx, h**o)
# same as realworldlist but it using 4 points of conner
cornerworldlist = convert_pos(cornerlist, newcammtx, h**o, mode=0, inverse=False)
# data_pack include 1.cardlist is class og each card, 2.midpointlist is useless, 3.cornerworldlist is cornerworldlist
# 4.realworldlist is realworldlist, 5.data_pack (it will send to MATLAB later) 6.parameter that tell scene
data_pack = pack_data(cardlist, midpointlist, cornerworldlist, realworldlist, DATA_PACK, brl)
self.rtp.release_camera_instance()
print(data_pack)
return data_pack
def transform_angle(self,q_configuration):
q_homeconfig = [180,90,30,90,60,90]
q_symbol = [-1,-1,1,-1,1,-1]
q_transform = []
for i in range(0,6):
q_transform.append(q_homeconfig[i] + q_symbol[i]*q_configuration[i])
return q_transform
# Loop camera(pan,tilt),classify, position _________________________________________________________________________
def Step1FindCard(self):
T_CLF = time.time()
STATE = 1
CARD_POSITION = []
for PT in Main.Const.CameraRight:
if STATE == 1:
# 1. Rotate J1 90 degree
#self.cmKhong(Main.Const.Right)
# 2. Pan CAM (-50,-70)
#self.camera(PT)
# 3. Predict & Position
#CARD_POSITION = self.cam_clf(CARD_POSITION,'br',PT)
pass
else:
# 4. Rotate J1 90 degree
if STATE == 2:
self.cmKhong(Main.Const.Front)
time.sleep(8)
# 5. Pan CAM [95,-30],[95,-10],[95,15],[95,22],[70,22],[70,-10],[70,-30]
self.camera(PT)
# 6. Predict & Position
CARD_POSITION = self.cam_clf(CARD_POSITION,'r',PT)
STATE += 1
for PT in Main.Const.CameraBase:
self.camera(PT)
CARD_POSITION = self.cam_clf(CARD_POSITION,'b',PT)
for PT in Main.Const.CameraLeft:
if STATE != 16:
# 7. Pan CAM [-95,-30],[-95,-10],[-95,15],[-95,22],[-70,22],[-70,-10],[-70,-30]
self.camera(PT)
# 8. Predict & Position
CARD_POSITION = self.cam_clf(CARD_POSITION,'l',PT)
else:
################# Can't rotate Joint 1 to -90 degree ###################################################################
# 9. Rotate J1 90 degree
#self.cmKhong()
# 10. Pan CAM
#self.camera(PT)
# 11. Predict & Position
#CARD_POSITION = self.cam_clf(CARD_POSITION,'bl',PT)
pass
STATE += 1
# Set home position
self.CAMER.PANTILT(0)
self.cmKhong(Main.Const.Homei)
time.sleep(8)
self.cmKhong(Main.Const.Home)
ENDT_CLF = time.time() - T_CLF
cv2.destroyAllWindows()
return CARD_POSITION, ENDT_CLF
# Planning (MATLAB) ____________________________________________________________________________________________________
def Step2PathPlan(self,CardPosition):
T_PLN = time.time()
CHIN = PathPlan(CardPosition)
PATH = CHIN.EvaluateTraject()
ENDT_PLN = time.time() - T_PLN
return PATH,ENDT_PLN
# Command Khong ________________________________________________________________________________________________________
def Step3CommandKhong(self,PATH):
T_LLV = time.time()
temp = 0
#Traject
pose = [180,90,30,90,90,60]
manual = [[119, -13, 66, -78, 59, 77], [100, -5, 55, -47, 44, 46],
[76, -6, 56, 56, 47, -55], [56, -15, 69, 81, 65, -79],
[119, -19, 47, -45, 73, 36], [100, -12, 35, -17, 68, 14],
[76, -13, 35, 21, 70, -17], [58, -16, 45, 50, 74, -41]]
old_pose = [90,60,90]
for path in range(len(PATH)):
#Sub Traject
for STpath in range(len(PATH[path])):
via = PATH[path][STpath][len(PATH[path][STpath])-1]
for k in range(len(PATH[path][STpath])):
pose = [0,0,0,0,0,0]
pose[3:6] = old_pose
tj = PATH[path][STpath][k]
if k != len(PATH[path][STpath])-1:
pose[0:3] = tj[0:3]
else:
pose = via
old_pose = pose[3:6]
self.cmKhong(pose)
print('via: ',pose)
if (path+1)%2 ==0:
if (path+1)%4 == 0:
time.sleep(3)
#manual[temp][5] = via[5]
self.cmKhong(self.transform_angle(manual[temp]))
time.sleep(1.5)
self.KHONG.SetGrip(0)
temp += 1
else:
self.KHONG.SetGrip(1)
print('Finish SUB ',path)
time.sleep(2)
self.KHONG.SetGrip(0)
ENDT_LLV = time.time() - T_LLV
return ENDT_LLV
