def __init__(self, *args, **kwargs):
self.sonar_map = kwargs.pop('sonar_map')
line_map_sprite = kwargs.pop('line_map_sprite')
self.static_objects = kwargs.pop('static_objects')
batch = kwargs.pop('batch')
window_width = kwargs.pop('window_width')
window_height = kwargs.pop('window_height')
robot_group = pyglet.graphics.OrderedGroup(1)
super(Pi2Go, self).__init__(src.resources.pi2go_image, 0, 0, batch, robot_group, window_width=window_width,
window_height=window_height)
# drawing batch
self.batch = batch
self.group = robot_group
self.robot_name = "PI2GO"
self.radius = max(self.image.width, self.image.height) / 2.0
x_light_offset = self.image.width/2
y_light_offset = self.image.height/2
self.sonar_sensor = FixedTransformDistanceSensor(self, self.sonar_map, SONAR_OFFSET_X, 0, 0, SONAR_MIN_RANGE,
SONAR_MAX_RANGE, SONAR_BEAM_ANGLE)
self.ir_left_sensor = FixedTransformDistanceSensor(self, self.sonar_map, IR_OFFSET_X, IR_OFFSET_Y,
IR_SENSOR_ANGLE, IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.ir_middle_sensor = FixedTransformDistanceSensor(self, self.sonar_map, IR_OFFSET_X_MIDDLE, 0,
0, IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.ir_right_sensor = FixedTransformDistanceSensor(self, self.sonar_map, IR_OFFSET_X, -IR_OFFSET_Y,
-IR_SENSOR_ANGLE, IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.light_frontleft_sensor = FixedLightSensor(self, x_light_offset, y_light_offset-10, "FrontLeft", drawing_colour=(255,0,0,255))
self.light_frontright_sensor = FixedLightSensor(self, x_light_offset, -y_light_offset+10, "FrontRight", drawing_colour=(0,255,0,255))
self.light_backleft_sensor = FixedLightSensor(self, -x_light_offset, y_light_offset-10, "BackLeft", drawing_colour=(0,0,255,255))
self.light_backright_sensor = FixedLightSensor(self, -x_light_offset, -y_light_offset+10, "BackRight", drawing_colour=(255,255,255,255))
self.light_sensors = []
self.light_sensors.append(self.light_frontleft_sensor)
self.light_sensors.append(self.light_frontright_sensor)
self.light_sensors.append(self.light_backleft_sensor)
self.light_sensors.append(self.light_backright_sensor)
# add the LEDs - radius of the light is 10, a gap of 3 is added between neighbouring leds
# left side leds
self.left_led1 = FixedLED(self, 1, y_light_offset-22, "left_led1")
self.left_led2 = FixedLED(self, 12, y_light_offset-22, "left_led2")
# right side leds
self.right_led1 = FixedLED(self, 1, -y_light_offset+22, "right_led1")
self.right_led2 = FixedLED(self, 12, -y_light_offset+22, "right_led2")
# front side leds
self.front_led1 = FixedLED(self, x_light_offset-17, -15, "front_led1")
self.front_led2 = FixedLED(self, x_light_offset-17, +15, "front_led2")
# back side leds
self.back_led1 = FixedLED(self, -x_light_offset+10, -10, "back_led1")
self.back_led2 = FixedLED(self, -x_light_offset+10, +10, "back_led2")
self.leds = []
self.leds.append(self.left_led1)
self.leds.append(self.left_led2)
self.leds.append(self.right_led1)
self.leds.append(self.right_led2)
self.leds.append(self.front_led1)
self.leds.append(self.front_led2)
self.leds.append(self.back_led1)
self.leds.append(self.back_led2)
self.line_sensor_map = LineSensorMap(line_map_sprite)
self.left_line_sensor = FixedLineSensor(self, self.line_sensor_map, LINE_OFFSET_X, LINE_OFFSET_Y)
self.right_line_sensor = FixedLineSensor(self, self.line_sensor_map, LINE_OFFSET_X, -LINE_OFFSET_Y)
self.mouse_move_state = False
self.mouse_position = [0, 0]
self.vx = 0.0
self.vth = 0.0
self.publish_continue = True
self.receive_continue = True
self.led_init_anim_on = True
self.led_init_anim_count = 0
self.leds_prev_values = []
self.control_switch_on = False
self.turn_off_leds()
self.event_handlers = [self, self.on_mouse_release, self.on_mouse_drag]
self.publish_thread = threading.Thread(target=self.publish_state_udp)
# self.publish_thread.setDaemon(True)
self.publish_thread.start()
self.cmd_thread = threading.Thread(target=self.recv_commands)
# self.cmd_thread.setDaemon(True)
self.cmd_thread.start()
class Pi2Go(basicsprite.BasicSprite):
def __init__(self, *args, **kwargs):
self.sonar_map = kwargs.pop('sonar_map')
line_map_sprite = kwargs.pop('line_map_sprite')
self.static_objects = kwargs.pop('static_objects')
batch = kwargs.pop('batch')
window_width = kwargs.pop('window_width')
window_height = kwargs.pop('window_height')
robot_group = pyglet.graphics.OrderedGroup(1)
super(Pi2Go, self).__init__(src.resources.pi2go_image, 0, 0, batch, robot_group, window_width=window_width,
window_height=window_height)
# drawing batch
self.batch = batch
self.group = robot_group
self.robot_name = "PI2GO"
self.radius = max(self.image.width, self.image.height) / 2.0
x_light_offset = self.image.width/2
y_light_offset = self.image.height/2
self.sonar_sensor = FixedTransformDistanceSensor(self, self.sonar_map, SONAR_OFFSET_X, 0, 0, SONAR_MIN_RANGE,
SONAR_MAX_RANGE, SONAR_BEAM_ANGLE)
self.ir_left_sensor = FixedTransformDistanceSensor(self, self.sonar_map, IR_OFFSET_X, IR_OFFSET_Y,
IR_SENSOR_ANGLE, IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.ir_middle_sensor = FixedTransformDistanceSensor(self, self.sonar_map, IR_OFFSET_X_MIDDLE, 0,
0, IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.ir_right_sensor = FixedTransformDistanceSensor(self, self.sonar_map, IR_OFFSET_X, -IR_OFFSET_Y,
-IR_SENSOR_ANGLE, IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.light_frontleft_sensor = FixedLightSensor(self, x_light_offset, y_light_offset-10, "FrontLeft", drawing_colour=(255,0,0,255))
self.light_frontright_sensor = FixedLightSensor(self, x_light_offset, -y_light_offset+10, "FrontRight", drawing_colour=(0,255,0,255))
self.light_backleft_sensor = FixedLightSensor(self, -x_light_offset, y_light_offset-10, "BackLeft", drawing_colour=(0,0,255,255))
self.light_backright_sensor = FixedLightSensor(self, -x_light_offset, -y_light_offset+10, "BackRight", drawing_colour=(255,255,255,255))
self.light_sensors = []
self.light_sensors.append(self.light_frontleft_sensor)
self.light_sensors.append(self.light_frontright_sensor)
self.light_sensors.append(self.light_backleft_sensor)
self.light_sensors.append(self.light_backright_sensor)
# add the LEDs - radius of the light is 10, a gap of 3 is added between neighbouring leds
# left side leds
self.left_led1 = FixedLED(self, 1, y_light_offset-22, "left_led1")
self.left_led2 = FixedLED(self, 12, y_light_offset-22, "left_led2")
# right side leds
self.right_led1 = FixedLED(self, 1, -y_light_offset+22, "right_led1")
self.right_led2 = FixedLED(self, 12, -y_light_offset+22, "right_led2")
# front side leds
self.front_led1 = FixedLED(self, x_light_offset-17, -15, "front_led1")
self.front_led2 = FixedLED(self, x_light_offset-17, +15, "front_led2")
# back side leds
self.back_led1 = FixedLED(self, -x_light_offset+10, -10, "back_led1")
self.back_led2 = FixedLED(self, -x_light_offset+10, +10, "back_led2")
self.leds = []
self.leds.append(self.left_led1)
self.leds.append(self.left_led2)
self.leds.append(self.right_led1)
self.leds.append(self.right_led2)
self.leds.append(self.front_led1)
self.leds.append(self.front_led2)
self.leds.append(self.back_led1)
self.leds.append(self.back_led2)
self.line_sensor_map = LineSensorMap(line_map_sprite)
self.left_line_sensor = FixedLineSensor(self, self.line_sensor_map, LINE_OFFSET_X, LINE_OFFSET_Y)
self.right_line_sensor = FixedLineSensor(self, self.line_sensor_map, LINE_OFFSET_X, -LINE_OFFSET_Y)
self.mouse_move_state = False
self.mouse_position = [0, 0]
self.vx = 0.0
self.vth = 0.0
self.publish_continue = True
self.receive_continue = True
self.led_init_anim_on = True
self.led_init_anim_count = 0
self.leds_prev_values = []
self.control_switch_on = False
self.turn_off_leds()
self.event_handlers = [self, self.on_mouse_release, self.on_mouse_drag]
self.publish_thread = threading.Thread(target=self.publish_state_udp)
# self.publish_thread.setDaemon(True)
self.publish_thread.start()
self.cmd_thread = threading.Thread(target=self.recv_commands)
# self.cmd_thread.setDaemon(True)
self.cmd_thread.start()
# pyglet.clock.schedule_interval(self.update_sensors, 1.0 / 30)
def start_robot(self):
self.publish_continue = True
self.receive_continue = True
# reset the movement values
self.velocity_x = 0.0
self.velocity_y = 0.0
self.vx = 0.0
self.vth = 0.0
# this method is called when the robot control switch is switched ON
self.control_switch_on = True
# release the brakes on movement
# pyglet.clock.unschedule(self.stop_robot_movement)
def stop_robot(self):
# cause the publish and command threads to stop
self.publish_continue = False
self.receive_continue = False
self.control_switch_on = False
self.turn_off_leds()
# stop movement
# pyglet.clock.unschedule(self.stop_robot_movement)
time.sleep(0.3)
# # stop robot movement using a background thread - the target method is called continually when the robot control switch is OFF
#stop_movement_thread = threading.Thread(target=pyglet.clock.schedule_interval, args=(self.stop_robot_movement, 1.0 / 30))
#stop_movement_thread.setDaemon(True)
#stop_movement_thread.start()
# pyglet.clock.schedule_interval(self.stop_robot_movement, 1.0 / 30)
def stop_robot_movement(self, dt):
# stop movement
self.velocity_x = 0.0
self.velocity_y = 0.0
self.vx = 0.0
self.vth = 0.0
def light_leds(self):
# first refresh the lights and then light them up
for led in self.leds:
if led.fill_rep is not None:
led.fill_rep.delete()
led.outline_rep.delete()
led.shine()
def turn_off_leds(self):
# first refresh the lights and then light them up
for led in self.leds:
if led.fill_rep is not None:
led.fill_rep.delete()
led.outline_rep.delete()
led.shine()
led.turn_off()
def perform_led_init_animation(self):
self.led_init_anim_on = True
self.led_init_anim_count = 0
self.leds_prev_values = []
# pyglet.clock.schedule_interval(self.led_init_animation, 1.0)
def led_init_animation(self, dt):
if self.led_init_anim_on == True:
self.leds_prev_values = []
for led in self.leds:
# save off the original values of light for each led
self.leds_prev_values.append([led.red_value, led.green_value, led.blue_value])
# animate the leds with random values of light
led.red_value = int(random.uniform(0, theled.MAX_VALUE))
led.green_value = int(random.uniform(0, theled.MAX_VALUE))
led.blue_value = int(random.uniform(0, theled.MAX_VALUE))
self.light_leds()
elif self.led_init_anim_on == False:
for id, led in enumerate(self.leds):
# reinstate the value before flash
led.red_value = self.leds_prev_values[id][0]
led.green_value = self.leds_prev_values[id][1]
led.blue_value = self.leds_prev_values[id][2]
self.light_leds()
# toggle the on state for the next clock interval schedule
self.led_init_anim_on = True if self.led_init_anim_on == False else False
# update the count for the flashes
self.led_init_anim_count += 1
if self.led_init_anim_count > LED_INIT_FLASH_COUNT:
# stop the schedules
self.led_init_anim_count = 0
pyglet.clock.unschedule(self.led_init_animation)
self.turn_off_leds()
def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers):
"""Allows the robot to be dragged around using the mouse."""
if self.mouse_move_state:
self.x = x
self.y = y
self.velocity_x = 0
self.velocity_y = 0
#self.sonar_sensor.update(1)
#def on_mouse_release(self, x, y, button, modifiers):
# super(Pi2Go, self).on_mouse_release(x, y, button, modifiers)
# update the LEDs
# self.perform_led_init_animation()
# self.light_leds()
def recv_commands(self):
"""Thread function which handles incomming commands for an external python script via a UDP socket.
Commands are strings and take the form: <<LINEAR_VELOCITY;ANGULAR_VELOCITY;
FRONT_LED1_RED_VALUE;FRONT_LED1_GREEN_VALUE;FRONT_LED1_BLUE_VALUE;
FRONT_LED2_RED_VALUE;FRONT_LED2_GREEN_VALUE;FRONT_LED2_BLUE_VALUE;
LEFT_LED1_RED_VALUE;LEFT_LED1_GREEN_VALUE;LEFT_LED1_BLUE_VALUE;
LEFT_LED2_RED_VALUE;LEFT_LED2_GREEN_VALUE;LEFT_LED2_BLUE_VALUE;
BACK_LED1_RED_VALUE;BACK_LED1_GREEN_VALUE;BACK_LED1_BLUE_VALUE;
BACK_LED2_RED_VALUE;BACK_LED2_GREEN_VALUE;BACK_LED2_BLUE_VALUE;
RIGHT_LED1_RED_VALUE;RIGHT_LED1_GREEN_VALUE;RIGHT_LED1_BLUE_VALUE;
RIGHT_LED2_RED_VALUE;RIGHT_LED2_GREEN_VALUE;RIGHT_LED2_BLUE_VALUE>>
"""
self.sock_recv = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
self.sock_recv.bind((UDP_IP, UDP_COMMAND_PORT))
self.sock_recv.settimeout(1)
# double loop is a hack - changing while True to while self.receive_continue while debugging
while self.receive_continue is True:
while self.receive_continue is True:
try:
data_e, addr = self.sock_recv.recvfrom(1024) # buffer size is 1024 bytes
data = data_e.decode()
if data.startswith("<<") and data.endswith(">>"):
data = data.replace("<<", "")
data = data.replace(">>", "")
values_list = data.split(";")
# print (data);
if len(values_list) == 26:
self.vx = float(values_list[0])
self.vth = float(values_list[1])
if self.vx == 0 and self.vth != 0:
self.is_rotating = True
else: self.is_rotating = False
self.front_led1.red_value = values_list[2]
self.front_led1.green_value = values_list[3]
self.front_led1.blue_value = values_list[4]
self.front_led2.red_value = values_list[5]
self.front_led2.green_value = values_list[6]
self.front_led2.blue_value = values_list[7]
self.right_led1.red_value = values_list[8]
self.right_led1.green_value = values_list[9]
self.right_led1.blue_value = values_list[10]
self.right_led2.red_value = values_list[11]
self.right_led2.green_value = values_list[12]
self.right_led2.blue_value = values_list[13]
self.back_led1.red_value = values_list[14]
self.back_led1.green_value = values_list[15]
self.back_led1.blue_value = values_list[16]
self.back_led2.red_value = values_list[17]
self.back_led2.green_value = values_list[18]
self.back_led2.blue_value = values_list[19]
self.left_led1.red_value = values_list[20]
self.left_led1.green_value = values_list[21]
self.left_led1.blue_value = values_list[22]
self.left_led2.red_value = values_list[23]
self.left_led2.green_value = values_list[24]
self.left_led2.blue_value = values_list[25]
except Exception:
pass
time.sleep(READ_INTERVAL)
# we need to call stop_robot() again since the values for vx and vth and velocities may
# already have been reinstated by an update from the client just before the inner while loop
# above stopped (due to self.receive_continue being set to false)
#self.stop_robot()
print("closing receive socket\n")
self.sock_recv.close()
def publish_state_udp(self):
"""Thread function which publishes the state of the robot to an external python script via UDP socket.
State strings take the form:
<<ROBOT_NAME;SONAR_RANGE;LEFT_LINE;RIGHT_LINE;LEFT_IR;RIGHT_IR;
FRONT_LEFT_LIGHTSENSOR; FRONT_RIGHT_LIGHTSENSOR;
BACK_RIGHT_LIGHTSENSOR; BACK_LEFT_LIGHTSENSOR;
FRONT_LED1_RED_VALUE;FRONT_LED1_GREEN_VALUE;FRONT_LED1_BLUE_VALUE;
FRONT_LED2_RED_VALUE;FRONT_LED2_GREEN_VALUE;FRONT_LED2_BLUE_VALUE;
BACK_LED1_RED_VALUE;BACK_LED1_GREEN_VALUE;BACK_LED1_BLUE_VALUE;
BACK_LED2_RED_VALUE;BACK_LED2_GREEN_VALUE;BACK_LED2_BLUE_VALUE;
LEFT_LED1_RED_VALUE;LEFT_LED1_GREEN_VALUE;LEFT_LED1_BLUE_VALUE;
LEFT_LED2_RED_VALUE;LEFT_LED2_GREEN_VALUE;LEFT_LED2_BLUE_VALUE;
RIGHT_LED1_RED_VALUE;RIGHT_LED1_GREEN_VALUE;RIGHT_LED1_BLUE_VALUE;
RIGHT_LED2_RED_VALUE;RIGHT_LED2_GREEN_VALUE;RIGHT_LED2_BLUE_VALUE; CONTROL_SWITCH>>
"""
self.sock_publish = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
while self.publish_continue is True:
while self.publish_continue is True:
ir_left = self.ir_left_sensor.get_fixed_triggered(IR_MAX_RANGE)
ir_mid = self.ir_middle_sensor.get_fixed_triggered(IR_MAX_RANGE)
ir_right = self.ir_right_sensor.get_fixed_triggered(IR_MAX_RANGE)
line_left = self.left_line_sensor.get_triggered()
line_right = self.right_line_sensor.get_triggered()
light_fl = int(self.light_frontleft_sensor.value)
light_fr = int(self.light_frontright_sensor.value)
light_bl = int(self.light_backleft_sensor.value)
light_br = int(self.light_backright_sensor.value)
message = "<<%s;%f;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d>>" % (
self.robot_name, self.sonar_sensor.get_distance(),
line_left, line_right, ir_left, ir_mid, ir_right,
light_fl, light_fr, light_br, light_bl,
int(self.front_led1.red_value),
int(self.front_led1.green_value),
int(self.front_led1.blue_value),
int(self.front_led2.red_value),
int(self.front_led2.green_value),
int(self.front_led2.blue_value),
int(self.back_led1.red_value),
int(self.back_led1.green_value),
int(self.back_led1.blue_value),
int(self.back_led2.red_value),
int(self.back_led2.green_value),
int(self.back_led2.blue_value),
int(self.right_led1.red_value),
int(self.right_led1.green_value),
int(self.right_led1.blue_value),
int(self.right_led2.red_value),
int(self.right_led2.green_value),
int(self.right_led2.blue_value),
int(self.left_led1.red_value),
int(self.left_led1.green_value),
int(self.left_led1.blue_value),
int(self.left_led2.red_value),
int(self.left_led2.green_value),
int(self.left_led2.blue_value),
int(self.control_switch_on))
try:
self.sock_publish.sendto(message.encode('utf-8'), (UDP_IP, UDP_DATA_PORT))
updated_switch_finally = False
time.sleep(PUBLISH_INTERVAL)
except Exception:
pass
# send again, once, to update the control switch
if updated_switch_finally is False:
message = "<<%s;%f;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d>>" % (
self.robot_name, self.sonar_sensor.get_distance(),
line_left, line_right, ir_left, ir_mid, ir_right,
light_fl, light_fr, light_br, light_bl,
int(self.front_led1.red_value),
int(self.front_led1.green_value),
int(self.front_led1.blue_value),
int(self.front_led2.red_value),
int(self.front_led2.green_value),
int(self.front_led2.blue_value),
int(self.back_led1.red_value),
int(self.back_led1.green_value),
int(self.back_led1.blue_value),
int(self.back_led2.red_value),
int(self.back_led2.green_value),
int(self.back_led2.blue_value),
int(self.right_led1.red_value),
int(self.right_led1.green_value),
int(self.right_led1.blue_value),
int(self.right_led2.red_value),
int(self.right_led2.green_value),
int(self.right_led2.blue_value),
int(self.left_led1.red_value),
int(self.left_led1.green_value),
int(self.left_led1.blue_value),
int(self.left_led2.red_value),
int(self.left_led2.green_value),
int(self.left_led2.blue_value),
int(self.control_switch_on))
try:
print(message)
self.sock_publish.sendto(message.encode('utf-8'), (UDP_IP, UDP_DATA_PORT))
updated_switch_finally = True
time.sleep(PUBLISH_INTERVAL)
except Exception:
pass
print("closing publish socket\n")
self.sock_publish.close()
def update_sensors(self):
"""Take a new reading for each sensor."""
self.sonar_sensor.update_sensor()
self.ir_left_sensor.update_sensor()
self.ir_middle_sensor.update_sensor()
self.ir_right_sensor.update_sensor()
self.left_line_sensor.update_sensor()
self.right_line_sensor.update_sensor()
def update_light_sensors(self, simulator):
""" Updates the light sensors """
# compute the angular distance of each light sensor to the light source.
# based on the angular distance, use a gaussian to determine the sensor
# value for the light source.
sensor_angles = []
for ls in self.light_sensors:
sensor_angles.append((ls.name, ls.update_sensor(simulator))) # ls.update_sensor() will update each sensor's value
# ls.make_circle()
return sensor_angles
def reset_light_sensors(self):
for ls in self.light_sensors:
ls.reset_beam_cone_stddev()
ls.value = 0
def update(self, dt, simulator):
"""Update the state of the robot. This updates the velocity of the robot based on the current velocity commands
self.vx and self.vth. Also updates the position of the sonar sensor sprite accordingly. This function will not
update the robots state if the robot is currently being moved by the user (via mouse drag). """
# Do all the normal physics stuff
super(Pi2Go, self).update(dt)
if not self.mouse_move_state:
angle_radians = -math.radians(self.rotation)
self.velocity_x = self.vx * math.cos(angle_radians)
self.velocity_y = self.vx * math.sin(angle_radians)
self.rotation -= self.vth * dt
self.update_sensors()
# src.util.circle(self.left_line_sensor.sensor_x, self.left_line_sensor.sensor_y, 10, 10)
# src.util.circle(30, 30, 30)
# self.update_sensors
self.update_light_sensors(simulator)
self.light_leds()
# self.left_line_sensor.make_circle()
# self.right_line_sensor.make_circle()
# self.sonar_sensor.make_circle()
# self.ir_left_sensor.make_circle()
# self.ir_right_sensor.make_circle()
# self.ir_middle_sensor.make_circle()
# Let the light ray track the robot when it moves normally - NO!
# if simulator.light_source is not None and not simulator.is_ray_being_dragged \
# and not simulator.ray_was_dragged:
# if simulator.light_source.x < self.x: px = self.x - self.image.width/2
# elif simulator.light_source.x == self.x: px = self.x
# else: px = self.x + self.image.width/2
# if simulator.light_source.y < self.y: py = self.y - self.image.height/2
# elif simulator.light_source.y == self.y: py = self.y
# else: py = self.y + self.image.height/2
# simulator.light_follow_mouse(px, py)
def robot_collides_with(self, other_object):
"""Collision checking between the robot and another object. This function uses ver simple radius based collision
detection."""
# Calculate distance between object centers that would be a collision,
# assuming square resources
collision_distance = self.radius + other_object.image.width / 2.0
# Get distance using position tuples
actual_distance = src.util.distance(self.position, other_object.position)
return actual_distance <= collision_distance
def get_shining_light(self):
"""Checks if there is a light source in the world and returns it
"""
if self.static_objects is not None:
for obj in self.static_objects:
if obj.object_type.startswith("light"):
return obj
return None
def delete(self):
"""Deletes the robot sprite."""
super(Pi2Go, self).delete()
def draw_robot_position(self):
"""Draws a white circle on the screen at the current position of the robot."""
src.util.circle(self.x, self.y, 5)
def indicate_position(self):
""" lights up the led at its position with its colour value
"""
vertices_fill = self.make_circle_filled()
colour_opacity = 255
fill_colour = (255, 255, 255, colour_opacity)
# now fill the LED with the current light setting
self.batch.add(int(len(vertices_fill)/2), pyglet.gl.GL_POINTS, None,
('v2f', vertices_fill),
('c4B', fill_colour*int(len(vertices_fill)/2)))
def make_circle_filled(self):
verts = []
for radius in range(4, 0, -1): # LED_RADIUS-1 so we don't cover the outline of the circle
num_points = int(100.0 * radius/4.0)
for i in range(num_points):
angle = math.radians(float(i)/num_points * 360.0)
x = radius*math.cos(angle) + self.x
y = radius*math.sin(angle) + self.y
verts += [x,y]
return verts
def switch_on(self):
self.control_switch_on = True
def switch_off(self):
self.control_switch_on = False
def __init__(self, *args, **kwargs):
self.sonar_map = kwargs.pop('sonar_map')
line_map_sprite = kwargs.pop('line_map_sprite')
batch = kwargs.pop('batch')
window_width = kwargs.pop('window_width')
window_height = kwargs.pop('window_height')
robot_group = pyglet.graphics.OrderedGroup(1)
super(Pi2Go, self).__init__(src.resources.pi2go_image,
0,
0,
batch,
robot_group,
window_width=window_width,
window_height=window_height)
# drawing batch
self.batch = batch
self.group = robot_group
self.robot_name = "PI2GO"
self.radius = max(self.image.width, self.image.height) / 2.0
self.sonar_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, SONAR_OFFSET_X, 0, 0, SONAR_MIN_RANGE,
SONAR_MAX_RANGE, SONAR_BEAM_ANGLE)
self.ir_left_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, IR_OFFSET_Y, IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.ir_middle_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X_MIDDLE, 0, 0, IR_MIN_RANGE,
IR_MAX_RANGE, 0.25)
self.ir_right_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, -IR_OFFSET_Y, -IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.line_sensor_map = LineSensorMap(line_map_sprite)
self.left_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, LINE_OFFSET_Y)
self.right_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, -LINE_OFFSET_Y)
self.mouse_move_state = False
self.mouse_position = [0, 0]
self.vx = 0.0
self.vth = 0.0
self.event_handlers = [self, self.on_mouse_release, self.on_mouse_drag]
self.publish_thread = threading.Thread(target=self.publish_state_udp)
self.publish_thread.setDaemon(True)
self.publish_thread.start()
self.cmd_thread = threading.Thread(target=self.recv_commands)
self.cmd_thread.setDaemon(True)
self.cmd_thread.start()
pyglet.clock.schedule_interval(self.update_sensors, 1.0 / 30)
class Pi2Go(basicsprite.BasicSprite):
def __init__(self, *args, **kwargs):
self.sonar_map = kwargs.pop('sonar_map')
line_map_sprite = kwargs.pop('line_map_sprite')
batch = kwargs.pop('batch')
window_width = kwargs.pop('window_width')
window_height = kwargs.pop('window_height')
robot_group = pyglet.graphics.OrderedGroup(1)
super(Pi2Go, self).__init__(src.resources.pi2go_image,
0,
0,
batch,
robot_group,
window_width=window_width,
window_height=window_height)
# drawing batch
self.batch = batch
self.group = robot_group
self.robot_name = "PI2GO"
self.radius = max(self.image.width, self.image.height) / 2.0
self.sonar_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, SONAR_OFFSET_X, 0, 0, SONAR_MIN_RANGE,
SONAR_MAX_RANGE, SONAR_BEAM_ANGLE)
self.ir_left_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, IR_OFFSET_Y, IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.ir_middle_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X_MIDDLE, 0, 0, IR_MIN_RANGE,
IR_MAX_RANGE, 0.25)
self.ir_right_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, -IR_OFFSET_Y, -IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, 0.25)
self.line_sensor_map = LineSensorMap(line_map_sprite)
self.left_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, LINE_OFFSET_Y)
self.right_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, -LINE_OFFSET_Y)
self.mouse_move_state = False
self.mouse_position = [0, 0]
self.vx = 0.0
self.vth = 0.0
self.event_handlers = [self, self.on_mouse_release, self.on_mouse_drag]
self.publish_thread = threading.Thread(target=self.publish_state_udp)
self.publish_thread.setDaemon(True)
self.publish_thread.start()
self.cmd_thread = threading.Thread(target=self.recv_commands)
self.cmd_thread.setDaemon(True)
self.cmd_thread.start()
pyglet.clock.schedule_interval(self.update_sensors, 1.0 / 30)
def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers):
"""Allows the robot to be dragged around using the mouse."""
if self.mouse_move_state:
self.x = x
self.y = y
self.velocity_x = 0
self.velocity_y = 0
def recv_commands(self):
"""Thread function which handles incomming commands for an external python script via a UDP socket.
Commands are strings and take the form: <<LINEAR_VELOCITY;ANGULAR_VELOCITY>>
"""
sock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_COMMAND_PORT))
while True:
data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
if data.startswith("<<") and data.endswith(">>"):
data = data.replace("<<", "")
data = data.replace(">>", "")
values_list = data.split(";")
if len(values_list) == 2:
self.vx = float(values_list[0])
self.vth = float(values_list[1])
def publish_state_udp(self):
"""Thread function which publishes the state of the robot to an external python script via UDP socket.
State strings take the form: <<ROBOT_NAME;SONAR_RANGE;LEFT_LINE;RIGHT_LINE;LEFT_IR;MIDDLE_IR;RIGHT_IR>>
"""
sock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
while True:
ir_left = self.ir_left_sensor.get_fixed_triggered(IR_MAX_RANGE)
ir_mid = self.ir_middle_sensor.get_fixed_triggered(IR_MAX_RANGE)
ir_right = self.ir_right_sensor.get_fixed_triggered(IR_MAX_RANGE)
line_left = self.left_line_sensor.get_triggered()
line_right = self.right_line_sensor.get_triggered()
message = "<<%s;%f;%d;%d;%d;%d;%d>>" % (
self.robot_name, self.sonar_sensor.get_distance(), line_left,
line_right, ir_left, ir_mid, ir_right)
sock.sendto(message, (UDP_IP, UDP_DATA_PORT))
time.sleep(0.03)
def update_sensors(self, dt):
"""Take a new reading for each sensor."""
self.sonar_sensor.update_sensor()
self.ir_left_sensor.update_sensor()
self.ir_middle_sensor.update_sensor()
self.ir_right_sensor.update_sensor()
self.left_line_sensor.update_sensor()
self.right_line_sensor.update_sensor()
def update(self, dt):
"""Update the state of the robot. This updates the velocity of the robot based on the current velocity commands
self.vx and self.vth. Also updates the position of the sonar sensor sprite accordingly. This function will not
update the robots state if the robot is currently being moved by the user (via mouse drag). """
# Do all the normal physics stuff
super(Pi2Go, self).update(dt)
if not self.mouse_move_state:
angle_radians = -math.radians(self.rotation)
self.velocity_x = self.vx * math.cos(angle_radians)
self.velocity_y = self.vx * math.sin(angle_radians)
self.rotation -= self.vth * dt
def robot_collides_with(self, other_object):
"""Collision checking between the robot and another object. This function uses ver simple radius based collision
detection."""
# Calculate distance between object centers that would be a collision,
# assuming square resources
collision_distance = self.radius + other_object.image.width / 2.0
# Get distance using position tuples
actual_distance = src.util.distance(self.position,
other_object.position)
return actual_distance <= collision_distance
def delete(self):
"""Deletes the robot sprite."""
super(Pi2Go, self).delete()
def draw_robot_position(self):
"""Draws a white circle on the screen at the current position of the robot."""
src.util.circle(self.x, self.y, 5)
def __init__(self, *args, **kwargs):
self.sonar_map = kwargs.pop('sonar_map')
line_map_sprite = kwargs.pop('line_map_sprite')
self.static_objects = kwargs.pop('static_objects')
batch = kwargs.pop('batch')
window_width = kwargs.pop('window_width')
window_height = kwargs.pop('window_height')
robot_group = pyglet.graphics.OrderedGroup(1)
super(Initio, self).__init__(src.resources.robot_image,
0,
0,
batch,
robot_group,
window_width=window_width,
window_height=window_height)
# drawing batch
self.batch = batch
self.group = robot_group
self.robot_name = "INITIO"
self.radius = max(self.image.width, self.image.height) / 2.0
x_light_offset = self.image.width / 2
y_light_offset = self.image.height / 2
self.sonar_sensor = PanningDistanceSensor(batch=batch,
robot=self,
sonar_map=self.sonar_map,
offset_x=25,
min_range=SONAR_MIN_RANGE,
max_range=SONAR_MAX_RANGE,
beam_angle=SONAR_BEAM_ANGLE)
self.ir_left_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, IR_OFFSET_Y, IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, IR_BEAM_ANGLE)
self.ir_right_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, -IR_OFFSET_Y, -IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, IR_BEAM_ANGLE)
self.light_frontleft_sensor = FixedLightSensor(self,
x_light_offset,
y_light_offset - 10,
"FrontLeft",
drawing_colour=(255, 0,
0, 255))
self.light_frontright_sensor = FixedLightSensor(self,
x_light_offset,
-y_light_offset + 10,
"FrontRight",
drawing_colour=(0, 255,
0,
255))
self.light_backleft_sensor = FixedLightSensor(self,
-x_light_offset,
y_light_offset - 10,
"BackLeft",
drawing_colour=(0, 0,
255,
255))
self.light_backright_sensor = FixedLightSensor(
self,
-x_light_offset,
-y_light_offset + 10,
"BackRight",
drawing_colour=(255, 255, 255, 255))
self.light_sensors = []
self.light_sensors.append(self.light_frontleft_sensor)
self.light_sensors.append(self.light_frontright_sensor)
self.light_sensors.append(self.light_backleft_sensor)
self.light_sensors.append(self.light_backright_sensor)
self.line_sensor_map = LineSensorMap(line_map_sprite)
self.left_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, LINE_OFFSET_Y)
self.right_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, -LINE_OFFSET_Y)
self.mouse_move_state = False
self.mouse_position = [0, 0]
self.vx = 0.0
self.vth = 0.0
#self.schedule_lock = False
self.publish_continue = True
self.receive_continue = True
#self.sonar_sensor.update(1)
#self.sonar_sensor.update_sensor()
self.event_handlers = [self, self.on_mouse_release, self.on_mouse_drag]
self.control_switch_on = False
pyglet.clock.schedule_interval(self.update_sensors, 1.0 / 30)
self.publish_thread = threading.Thread(target=self.publish_state_udp)
self.publish_thread.setDaemon(True)
self.publish_thread.start()
self.cmd_thread = threading.Thread(target=self.recv_commands)
self.cmd_thread.setDaemon(True)
self.cmd_thread.start()
class Initio(basicsprite.BasicSprite):
def __init__(self, *args, **kwargs):
self.sonar_map = kwargs.pop('sonar_map')
line_map_sprite = kwargs.pop('line_map_sprite')
self.static_objects = kwargs.pop('static_objects')
batch = kwargs.pop('batch')
window_width = kwargs.pop('window_width')
window_height = kwargs.pop('window_height')
robot_group = pyglet.graphics.OrderedGroup(1)
super(Initio, self).__init__(src.resources.robot_image,
0,
0,
batch,
robot_group,
window_width=window_width,
window_height=window_height)
# drawing batch
self.batch = batch
self.group = robot_group
self.robot_name = "INITIO"
self.radius = max(self.image.width, self.image.height) / 2.0
x_light_offset = self.image.width / 2
y_light_offset = self.image.height / 2
self.sonar_sensor = PanningDistanceSensor(batch=batch,
robot=self,
sonar_map=self.sonar_map,
offset_x=25,
min_range=SONAR_MIN_RANGE,
max_range=SONAR_MAX_RANGE,
beam_angle=SONAR_BEAM_ANGLE)
self.ir_left_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, IR_OFFSET_Y, IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, IR_BEAM_ANGLE)
self.ir_right_sensor = FixedTransformDistanceSensor(
self, self.sonar_map, IR_OFFSET_X, -IR_OFFSET_Y, -IR_SENSOR_ANGLE,
IR_MIN_RANGE, IR_MAX_RANGE, IR_BEAM_ANGLE)
self.light_frontleft_sensor = FixedLightSensor(self,
x_light_offset,
y_light_offset - 10,
"FrontLeft",
drawing_colour=(255, 0,
0, 255))
self.light_frontright_sensor = FixedLightSensor(self,
x_light_offset,
-y_light_offset + 10,
"FrontRight",
drawing_colour=(0, 255,
0,
255))
self.light_backleft_sensor = FixedLightSensor(self,
-x_light_offset,
y_light_offset - 10,
"BackLeft",
drawing_colour=(0, 0,
255,
255))
self.light_backright_sensor = FixedLightSensor(
self,
-x_light_offset,
-y_light_offset + 10,
"BackRight",
drawing_colour=(255, 255, 255, 255))
self.light_sensors = []
self.light_sensors.append(self.light_frontleft_sensor)
self.light_sensors.append(self.light_frontright_sensor)
self.light_sensors.append(self.light_backleft_sensor)
self.light_sensors.append(self.light_backright_sensor)
self.line_sensor_map = LineSensorMap(line_map_sprite)
self.left_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, LINE_OFFSET_Y)
self.right_line_sensor = FixedLineSensor(self, self.line_sensor_map,
LINE_OFFSET_X, -LINE_OFFSET_Y)
self.mouse_move_state = False
self.mouse_position = [0, 0]
self.vx = 0.0
self.vth = 0.0
#self.schedule_lock = False
self.publish_continue = True
self.receive_continue = True
#self.sonar_sensor.update(1)
#self.sonar_sensor.update_sensor()
self.event_handlers = [self, self.on_mouse_release, self.on_mouse_drag]
self.control_switch_on = False
pyglet.clock.schedule_interval(self.update_sensors, 1.0 / 30)
self.publish_thread = threading.Thread(target=self.publish_state_udp)
self.publish_thread.setDaemon(True)
self.publish_thread.start()
self.cmd_thread = threading.Thread(target=self.recv_commands)
self.cmd_thread.setDaemon(True)
self.cmd_thread.start()
def start_robot(self):
self.publish_continue = True
self.receive_continue = True
# this method is called when the robot control switch is switched ON
self.control_switch_on = True
# release the brakes on movement
pyglet.clock.unschedule(self.stop_robot_movement)
def stop_robot(self):
# cause the publish and command threads to stop
self.publish_continue = False
self.receive_continue = False
# this method is called when the robot control switch is switched ON
self.control_switch_on = False
# stop movement
pyglet.clock.unschedule(self.stop_robot_movement)
time.sleep(0.3)
# stop robot movement using a background thread
#stop_movement_thread = threading.Thread(target=pyglet.clock.schedule_interval, args=(self.stop_robot_movement, 1.0 / 30))
#stop_movement_thread.setDaemon(True)
#stop_movement_thread.start()
pyglet.clock.schedule_interval(self.stop_robot_movement, 1.0 / 30)
def stop_robot_movement(self, dt):
# stop movement
self.velocity_x = 0.0
self.velocity_y = 0.0
self.vx = 0.0
self.vth = 0.0
def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers):
"""Allows the robot to be dragged around using the mouse."""
if self.mouse_move_state:
self.x = x
self.y = y
self.velocity_x = 0
self.velocity_y = 0
self.sonar_sensor.update(1)
def recv_commands(self):
"""Thread function which handles incomming commands for an external python script via a UDP socket.
Commands are strings and take the form: <<LINEAR_VELOCITY;ANGULAR_VELOCITY;SONAR_SERVO_ANGLE>>
"""
# Initialise the socket used by the command/receiving thread
sock_recv = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock_recv.bind((UDP_IP, UDP_COMMAND_PORT))
while True:
while self.receive_continue is True:
data, addr = sock_recv.recvfrom(
1024) # buffer size is 1024 bytes
if data.startswith("<<") and data.endswith(">>"):
data = data.replace("<<", "")
data = data.replace(">>", "")
values_list = data.split(";")
if len(values_list) == 3:
self.sonar_sensor.set_target(float(values_list[2]))
self.vx = float(values_list[0])
# if spin_time > 0.0:
# if self.schedule_lock == False:
# self.unschedule_angvelreset() # unschedule any previous schedule made.
# self.vth = float(values_list[1])
# pyglet.clock.schedule_interval(self.reset_angular_velocity, spin_time)
# self.schedule_lock = True # deny any new schedule to be made on reset_angular_velocity
# if self.vx == 0.0 and self.vth <> 0.0:
# self.is_rotating = True
# else:
self.vth = float(values_list[1])
if self.vx == 0 and self.vth <> 0:
self.is_rotating = True
else:
self.is_rotating = False
# we need to call stop_robot() again since the values for vx and vth and velocities may
# already have been reinstated by an update from the client just before the inner while loop
# above stopped (due to self.receive_continue being set to false)
#self.stop_robot()
def publish_state_udp(self):
"""Thread function which publishes the state of the robot to an external python script via UDP socket.
State strings take the form: <<ROBOT_NAME;SONAR_RANGE;LEFT_LINE;RIGHT_LINE;LEFT_IR;RIGHT_IR;
FRONT_LEFT_LIGHTSENSOR, FRONT_RIGHT_LIGHTSENSOR,
BACK_LEFT_LIGHTSENSOR, BACK_RIGHT_LIGHTSENSOR; CONTROL_SWITCH>>
"""
# Initialise the socket used by the publish thread
sock_publish = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
while True:
while self.publish_continue is True:
ir_left = self.ir_left_sensor.get_fixed_triggered(IR_MAX_RANGE)
ir_right = self.ir_right_sensor.get_fixed_triggered(
IR_MAX_RANGE)
line_left = self.left_line_sensor.get_triggered()
line_right = self.right_line_sensor.get_triggered()
light_fl = int(self.light_frontleft_sensor.value)
light_fr = int(self.light_frontright_sensor.value)
light_bl = int(self.light_backleft_sensor.value)
light_br = int(self.light_backright_sensor.value)
message = "<<%s;%f;%d;%d;%d;%d;%d;%d;%d;%d;%d>>" % (
self.robot_name, self.sonar_sensor.sonar_range, line_left,
line_right, ir_left, ir_right, light_fl, light_fr,
light_br, light_bl, self.control_switch_on)
sock_publish.sendto(message, (UDP_IP, UDP_DATA_PORT))
updated_switch_finally = False
time.sleep(0.03)
# send again, once, to update the control switch
if updated_switch_finally is False:
message = "<<%s;%f;%d;%d;%d;%d;%d;%d;%d;%d;%d>>" % (
self.robot_name, self.sonar_sensor.sonar_range, line_left,
line_right, ir_left, ir_right, light_fl, light_fr,
light_br, light_bl, self.control_switch_on)
sock_publish.sendto(message, (UDP_IP, UDP_DATA_PORT))
updated_switch_finally = True
time.sleep(0.03)
def update_sensors(self, dt):
"""Take a new reading for each sensor."""
self.sonar_sensor.update_sensor()
self.ir_left_sensor.update_sensor()
self.ir_right_sensor.update_sensor()
self.left_line_sensor.update_sensor()
self.right_line_sensor.update_sensor()
# def reset_angular_velocity(self, st):
# self.velocity_x = 0.0
# self.velocity_y = 0.0
# self.vx = 0.0
# self.vth = 0.0
# time.sleep(1.0)
# print("RESEeeeeeeeeeeeeeeeeting......^^^^^^^^^^^..........")
# self.is_rotating = False
# self.schedule_lock = False # allow a new schedule to be made on reset_angular_velocity.
#
# def unschedule_angvelreset(self):
# pyglet.clock.unschedule(self.reset_angular_velocity)
# print("unscheduled*******************+++++++++++++++++++++++++++................")
def update_light_sensors(self, simulator):
""" Updates the light sensors """
#light_source = self.get_shining_light()
# compute the angular distance of each light sensor to the light source.
# based on the angular distance, use a gaussian to determine the sensor
# value for the light source.
sensor_angles = []
for ls in self.light_sensors:
sensor_angles.append(
(ls.name, ls.update_sensor(simulator)
)) # ls.update_sensor() will update each sensor's value
#if self.position_changed() and not self.is_rotating:
# update sensor values based on euclidean distance from ray end
#dist = ls.angdistance_to_rayend(simulator)
#ls.normalise_value(dist, simulator)
return sensor_angles
def reset_light_sensors(self):
for ls in self.light_sensors:
ls.reset_beam_cone_stddev()
ls.value = 0
def update(self, dt, simulator):
"""Update the state of the robot. This updates the velocity of the robot based on the current velocity commands
self.vx and self.vth. Also updates the position of the sonar sensor sprite accordingly. This function will not
update the robots state if the robot is currently being moved by the user (via mouse drag). """
super(Initio, self).update(dt)
if not self.mouse_move_state:
angle_radians = -math.radians(self.rotation)
self.velocity_x = self.vx * math.cos(angle_radians)
self.velocity_y = self.vx * math.sin(angle_radians)
self.rotation -= self.vth * dt
self.sonar_sensor.update(dt)
self.update_light_sensors(simulator)
# Let the light ray track the robot when it moves normally
if simulator.light_source is not None and not simulator.is_ray_being_dragged \
and not simulator.ray_was_dragged:
if simulator.light_source.x < self.x:
px = self.x - self.image.width / 2
elif simulator.light_source.x == self.x:
px = self.x
else:
px = self.x + self.image.width / 2
if simulator.light_source.y < self.y:
py = self.y - self.image.height / 2
elif simulator.light_source.y == self.y:
py = self.y
else:
py = self.y + self.image.height / 2
simulator.light_follow_mouse(px, py)
def robot_collides_with(self, other_object):
"""Collision checking between the robot and another object. This function uses ver simple radius based collision
detection."""
# Calculate distance between object centers that would be a collision,
# assuming square resources
collision_distance = self.image.width / 2.0 + other_object.image.width / 3.0
# Get distance using position tuples
actual_distance = src.util.distance(self.position,
other_object.position)
return actual_distance <= collision_distance
def get_shining_light(self):
"""Checks if there is a light source in the world and returns it
"""
if self.static_objects is not None:
for obj in self.static_objects:
if obj.object_type.startswith("light"):
return obj
return None
def delete(self):
"""Deletes the robot sprite."""
super(Initio, self).delete()
def draw_robot_position(self):
"""Draws a white circle on the screen at the current position of the robot."""
src.util.circle(self.x, self.y, 5)
def indicate_position(self):
""" lights up the led at its position with its colour value
"""
vertices_fill = self.make_circle_filled()
colour_opacity = 255
fill_colour = (255, 255, 255, colour_opacity)
# now fill the LED with the current light setting
self.batch.add(int(len(vertices_fill) / 2), pyglet.gl.GL_POINTS, None,
('v2f', vertices_fill),
('c4B', fill_colour * int(len(vertices_fill) / 2)))
def make_circle_filled(self):
verts = []
for radius in range(
4, 0, -1
): # LED_RADIUS-1 so we don't cover the outline of the circle
num_points = int(100.0 * radius / 4.0)
for i in range(num_points):
angle = math.radians(float(i) / num_points * 360.0)
x = radius * math.cos(angle) + self.x
y = radius * math.sin(angle) + self.y
verts += [x, y]
return verts