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 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