def see(self):
newlist = []
personal_marker_list = []
facelist = ["0","1","2","3","4","5"]
robot_position = vector(self.box.getPosition())
rel_robot_axis = norm(self.box.GetFeature("box").axis)
world_robot_axis = odelib.rotateVector(self.box.getRotation(),rel_robot_axis)
#check arena markers
for m in arena_marker_list:
robot_to_marker = m.pos - robot_position
rot_y = self.roty_between(world_robot_axis,robot_to_marker)
rot_z = self.rotz_between(world_robot_axis,robot_to_marker)
if rot_y < 0.53 and rot_y > -0.53:
if rot_z < 0.53 and rot_z > -0.53:
distance = mag(robot_to_marker)
marker = self.Markertuple(distance,m.code,m.marker_type,self.Bearingtuple(0,np.degrees(rot_y),np.degrees(rot_z)),self.Worldtuple(robot_to_marker.x,robot_to_marker.y,robot_to_marker.z))
personal_marker_list.append(marker)
#check token markers
for t in token_list:
for f in facelist:
face = t.markers[int(f)]
faceaxis = norm(odelib.rotateVector(t.box.getRotation(),face.axis))
faceposition = vector(t.box.getPosition()) + faceaxis*0.05 #Replace 0.05 with marker size/2
robot_to_marker = faceposition - robot_position
rot_y = self.roty_between(world_robot_axis,robot_to_marker)
rot_z = self.rotz_between(world_robot_axis,robot_to_marker)
if diff_angle(faceaxis, world_robot_axis) < pi/2:
if rot_y < 0.53 and rot_y > -0.53:
if rot_z < 0.53 and rot_z > -0.53:
distance = mag(robot_to_marker)
if distance>0.7:
marker = self.Markertuple(distance,face.code,face.marker_type,self.Bearingtuple(0,np.degrees(rot_y),np.degrees(rot_z)),self.Worldtuple(robot_to_marker.x,robot_to_marker.y,robot_to_marker.z))
personal_marker_list.append(marker)
return personal_marker_list
def update(self):
#Calculates turning effect of each motor and uses them to make a turn
averagespeed = float((self.motors[0].speed + self.motors[1].speed)/2)/100
moment0 = float(self.motors[0].speed)/100
moment1 = float(-self.motors[1].speed)/100
self.totalmoment = 2*(moment0 + moment1)
self.box.setAngularVel((0,self.totalmoment,0))
vel = odelib.rotateVector(self.box.getRotation(),(averagespeed,0,0))
self.box.setLinearVel((vel[0],vel[1],vel[2]))
self.box.UpdateDisplay()
def update(self):
#Calculates turning effect of each motor and uses them to make a turn
averagespeed = float((self.motors[0].speed + self.motors[1].speed)/2)/100
moment0 = float(self.motors[0].speed)/100
moment1 = float(-self.motors[1].speed)/100
self.totalmoment = 2*(moment0 + moment1)
self.box.setAngularVel((0,self.totalmoment,0))
vel = odelib.rotateVector(self.box.getRotation(),(averagespeed,0,0))
self.box.setLinearVel((vel[0],vel[1],vel[2]))
self.box.UpdateDisplay()
def see(self):
newlist = []
personal_marker_list = []
facelist = ["0","1","2","3","4","5"]
robot_position = vector(self.box.getPosition())
rel_robot_axis = norm(self.box.GetFeature("box").axis)
world_robot_axis = odelib.rotateVector(self.box.getRotation(),rel_robot_axis)
#check arena markers
for m in arena_marker_list:
robot_to_marker = m.pos - robot_position
rot_y = self.roty_between(world_robot_axis,robot_to_marker)
rot_z = self.rotz_between(world_robot_axis,robot_to_marker)
if rot_y < 0.53 and rot_y > -0.53:
if rot_z < 0.53 and rot_z > -0.53:
distance = mag(robot_to_marker)
marker = self.Markertuple(distance,m.code,m.marker_type,self.Bearingtuple(0,degrees(rot_y),degrees(rot_z)),self.Worldtuple(robot_to_marker.x,robot_to_marker.y,robot_to_marker.z))
if distance > 0.7:
personal_marker_list.append(marker)
#check token markers
for t in token_list:
for f in facelist:
face = t.markers[int(f)]
faceaxis = norm(odelib.rotateVector(t.box.getRotation(),face.axis))
faceposition = vector(t.box.getPosition()) + faceaxis*0.05 #Replace 0.05 with marker size/2
robot_to_marker = faceposition - robot_position
rot_y = self.roty_between(world_robot_axis,robot_to_marker)
rot_z = self.rotz_between(world_robot_axis,robot_to_marker)
if diff_angle(faceaxis, world_robot_axis) < pi/2:
if rot_y < 0.53 and rot_y > -0.53:
if rot_z < 0.53 and rot_z > -0.53:
distance = mag(robot_to_marker)
marker = self.Markertuple(distance,face.code,face.marker_type,self.Bearingtuple(0,degrees(rot_y),degrees(rot_z)),self.Worldtuple(robot_to_marker.x,robot_to_marker.y,robot_to_marker.z))
if distance > 0.5:
personal_marker_list.append(marker)
return personal_marker_list
