def set_led_light(self, led_number, intensity=0):
if led_number > 7 or led_number < 0:
if self.debug:
err_print(prefix="SET LED LIGHT",
message=["LED NUMBER SHOULD BE BETWEEN 0 AND 7"])
elif intensity not in (0, 1):
pass
else:
self.led_lights[led_number] = intensity
pack = vrep.simxPackInts([led_number, intensity])
vrep.simxSetStringSignal(self.client_id, 'EPUCK_LED' + self.suffix,
pack, vrep.simx_opmode_oneshot_wait)
def _write_actuators(self):
"""
Write in the robot the actuators values. Don't use directly,
instead use 'step()'
"""
# We make a copy of the actuators list
actuators = self._actuators_to_write[:]
for m in actuators:
if m[0] == 'L':
# Leds
# Sent as packed string to ensure update of the wanted LED
test = vrep.simxPackInts([m[1] + 1, m[2]])
vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test,
vrep.simx_opmode_oneshot_wait)
elif m[0] == 'D':
# Set motor speed
# maxVel = 120.0 * math.pi / 180.0
# maxVel of ePuck firmware is 1000
# => 120 * pi / (180*1000) = pi/1500
velLeft = m[1] * math.pi / 1500.0
velRight = m[2] * math.pi / 1500.0
if velLeft > 120.0 * math.pi / 180.0:
velLeft = 120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velLeft < -120.0 * math.pi / 180.0:
velLeft = -120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velRight > 120.0 * math.pi / 180.0:
velRight = 120.0 * math.pi / 180.0
self._debug("velRight too high, thresholded")
if velRight < -120.0 * math.pi / 180.0:
velRight = -120.0 * math.pi / 180.0
self._debug("velRRight too high, thresholded")
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft',
velLeft, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight',
velRight, vrep.simx_opmode_oneshot)
elif m[0] == 'P':
# Motor position
self._debug('WARNING: Motor position not yet implemented!')
else:
self._debug('Unknown actuator to write')
self._actuators_to_write.remove(m)
return
def _write_actuators(self):
"""
Write in the robot the actuators values. Don't use directly,
instead use 'step()'
"""
# We make a copy of the actuators list
actuators = self._actuators_to_write[:]
for m in actuators:
if m[0] == 'L':
# Leds
# Sent as packed string to ensure update of the wanted LED
test = vrep.simxPackInts([m[1]+1, m[2]])
vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test, vrep.simx_opmode_oneshot_wait)
elif m[0] == 'D':
# Set motor speed
# maxVel = 120.0 * math.pi / 180.0
# maxVel of ePuck firmware is 1000
# => 120 * pi / (180*1000) = pi/1500
velLeft = m[1] * math.pi / 1500.0
velRight = m[2] * math.pi / 1500.0
if velLeft > 120.0 * math.pi / 180.0:
velLeft = 120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velLeft < -120.0 * math.pi / 180.0:
velLeft = -120.0 * math.pi / 180.0
self._debug("velLeft too high, thresholded")
if velRight > 120.0 * math.pi / 180.0:
velRight = 120.0 * math.pi / 180.0
self._debug("velRight too high, thresholded")
if velRight < -120.0 * math.pi / 180.0:
velRight = -120.0 * math.pi / 180.0
self._debug("velRRight too high, thresholded")
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft', velLeft, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight', velRight, vrep.simx_opmode_oneshot)
elif m[0] == 'P':
# Motor position
self._debug('WARNING: Motor position not yet implemented!')
else:
self._debug('Unknown actuator to write')
self._actuators_to_write.remove(m)
return
