class Sensors(SecureArduino):
# Default execute result
_DEFAULT = {
_GET_LEFT_SWITCH_OPCODE:
Deserializer(BYTE(0)),
_GET_RIGHT_SWITCH_OPCODE:
Deserializer(BYTE(0)),
_GET_NORMAL_OPCODE:
Deserializer(FLOAT(1000) + FLOAT(0) + FLOAT(1000) + FLOAT(0)),
_GET_MESURE_SENSOR_OPCODE:
Deserializer(INT(1000) + INT(1000))
}
def __init__(self, parent, uuid='sensors'):
SecureArduino.__init__(self, parent, uuid, Sensors._DEFAULT)
def get_normal(self, delta_time):
output = self.execute(_GET_NORMAL_OPCODE, INT(delta_time))
av_std, av_var, ar_std, ar_var = output.read(FLOAT, FLOAT, FLOAT,
FLOAT)
return ((av_std, av_var), (ar_std, ar_var))
def get_mesure(self, **kwargs):
if ROBOT_ID == R128_ID:
return (1000, 1000)
output = self.execute(_GET_MESURE_SENSOR_OPCODE, **kwargs)
ar, av = output.read(INT, INT)
if ar == 0 and av == 0:
return (1000, 1000)
if self._compid == "sensorsAr":
return ar, av
return ar, av
def wait(self, threshold, timeout=2):
init_time = time.time()
left, right = self.get_mesure()
while (left < threshold or right < threshold) \
and time.time()-init_time < timeout:
time.sleep(0.2)
left, right = self.get_mesure()
print(left, right)
if not (left > threshold and right > threshold):
raise TimeoutError()
def activate(self):
self.send(_ACTIVATE_SENSORS_OPCODE)
def desactivate(self):
self.send(_DESACTIVATE_SENSORS_OPCODE)
def get_left_switch(self):
output = self.execute(_GET_LEFT_SWITCH_OPCODE)
return bool(output.read(BYTE))
def get_right_switch(self):
output = self.execute(_GET_RIGHT_SWITCH_OPCODE)
return bool(output.read(BYTE))
def set_message(self, message, mode=None, speed=None):
if not len(message) < 20:
raise ValueError('message length must be less than 20 characters')
if mode is None and len(message) > 1:
mode = SLIDE_MODE
elif mode is None:
mode = ANIMATION_MODE
self.send(SET_MATRIX_MESSAGE_OPCODE, BYTE(self.matrix_id), BYTE(mode),
STRING(message))
if speed is not None:
self.set_speed(speed)
def upload_char_pattern(self, char, pattern):
lines = []
pattern = pattern.convert('L')
for y in range(8):
line = 0
for x in range(pattern.width):
digit = ~(pattern.getpixel((x, y)) // 255)
line = (line << 1) | (digit & 1)
line <<= 8 - pattern.width
lines.append(BYTE(line))
self.send(SET_EEPROM_CHAR_LEDMATRIX_OPCODE, CHAR(ord(char)), *lines,
BYTE(pattern.width))
def turnonthespot(self, theta, direction=None, way='forward'):
if direction is None:
self.send(START_TURNONTHESPOT_OPCODE, FLOAT(theta),
BYTE({
'forward': 0,
'backward': 1
}[way]))
else:
self.send(START_TURNONTHESPOT_DIR_OPCODE, FLOAT(theta),
BYTE({
'clock': 0,
'trig': 1
}[direction]))
def start_purepursuit(self):
self.send(
START_PUREPURSUIT_OPCODE,
BYTE({
self.NO_DIR: 0,
self.FORWARD: 0,
self.BACKWARD: 1
}[self.direction]), FLOAT(self.final_angle))
def set_workspace(self, id, ws):
self.send(_SET_PARAMETERS_OPCODE, BYTE(id), \
FLOAT(ws.x_min), \
FLOAT(ws.x_max), \
FLOAT(ws.y_min), \
FLOAT(ws.y_max), \
FLOAT(ws.elbow_or))
time.sleep(0.01)
def upload_char_pattern(self, char, pattern):
segments = 0
pattern = pattern.convert('L')
for i, (x, y) in enumerate([(1, 0), (2, 1), (2, 3), (1, 4), (0, 3), (0, 1), (1, 2)]):
digit = ~(pattern.getpixel((x, y)) // 255)
segments = (segments << 1) | (digit & 1)
segments = (segments << 1) | (char == '.')
self.send(SET_EEPROM_CHAR_IPDISPLAY_OPCODE,
CHAR(ord(char)), BYTE(segments))
class ColorSensorArray(SecureArduino):
INTEGRATION_TIME_2_4MS = 0
INTEGRATION_TIME_24MS = 1
INTEGRATION_TIME_50MS = 2
INTEGRATION_TIME_101MS = 3
INTEGRATION_TIME_154MS = 4
INTEGRATION_TIME_700MS = 5
GAIN_1X = 0
GAIN_4X = 1
GAIN_16X = 2
GAIN_60X = 3
CUP_COLOR_ESTIMATE_RED = 0
CUP_COLOR_ESTIMATE_GREEN = 1
CUP_COLOR_ESTIMATE_ERROR = 2
DEFAULT = {
CSA_GET_CUP_COLOR_ESTIMATE_OPCODE: Deserializer(BYTE(CUP_COLOR_ESTIMATE_ERROR)),
CSA_GET_RGB_OPCODE: Deserializer(FLOAT(255) + FLOAT(255) + FLOAT(255))
}
def __init__(self, parent, uuid='ColorSensors'):
SecureArduino.__init__(self, parent, uuid, self.DEFAULT)
def get_cup_color_estimate(self, channel: int) -> int:
return self.execute(CSA_GET_CUP_COLOR_ESTIMATE_OPCODE, BYTE(channel)).read(BYTE)
def get_rgb(self, channel: int) -> (float, float, float):
return self.execute(CSA_GET_RGB_OPCODE, BYTE(channel)).read(FLOAT, FLOAT, FLOAT)
def set_integration_time(self, channel: int, it: int):
self.send(CSA_SET_INTEGRATION_TIME_OPCODE, BYTE(channel), BYTE(it))
def set_gain(self, channel: int, gain: int):
self.send(CSA_SET_GAIN_OPCODE, BYTE(channel), BYTE(gain))
def enable(self, channel: int):
self.send(CSA_ENABLE_OPCODE, BYTE(channel))
def disable(self, channel: int):
self.send(CSA_DISABLE_OPCODE, BYTE(channel))
def leds_on(self):
self.send(CSA_LEDS_ON_OPCODE)
def leds_off(self):
self.send(CSA_LEDS_OFF_OPCODE)
def reset(self):
self.send(CSA_RESET)
def purepursuit(self,
waypoints,
direction='forward',
finalangle=None,
lookahead=None,
lookaheadbis=None,
linvelmax=None,
angvelmax=None,
**kwargs):
if len(waypoints) < 2:
raise ValueError('not enough waypoints')
self.send(RESET_PUREPURSUIT_OPCODE)
for x, y in waypoints:
self.send(ADD_PUREPURSUIT_WAYPOINT_OPCODE, FLOAT(x), FLOAT(y))
if lookahead is not None:
self.set_parameter_value(PUREPURSUIT_LOOKAHEAD_ID, lookahead,
FLOAT)
if lookaheadbis is not None:
self.set_parameter_value(PUREPURSUIT_LOOKAHEADBIS_ID, lookaheadbis,
FLOAT)
if linvelmax is not None:
self.set_parameter_value(POSITIONCONTROL_LINVELMAX_ID, linvelmax,
FLOAT)
if angvelmax is not None:
self.set_parameter_value(POSITIONCONTROL_ANGVELMAX_ID, angvelmax,
FLOAT)
if finalangle is None:
finalangle = math.atan2(waypoints[-1][1] - waypoints[-2][1],
waypoints[-1][0] - waypoints[-2][0])
self.direction = {
'forward': self.FORWARD,
'backward': self.BACKWARD
}[direction]
self.final_angle = finalangle
self.send(START_PUREPURSUIT_OPCODE,
BYTE({
'forward': 0,
'backward': 1
}[direction]), FLOAT(finalangle))
def setLEDAlarm(self, LEDAlarm):
self.send(_SET_LED_ALARM_OPCODE, BYTE(LEDAlarm))
def setShutdownAlarm(self, SAlarm):
self.send(_SET_SUTDOWN_ALARM_OPCODE, BYTE(SAlarm))
def setSRL(self, srl):
self.send(_SET_SRL_OPCODE, BYTE(srl))
def setRDT(self, rdt):
self.send(_SET_RDT_OPCODE, BYTE(rdt))
def set_default_message(self, message, mode, speed):
if not len(message) < 8:
raise ValueError(
'default message length must be less than 8 characters')
self.send(SET_EEPROM_DEFAULT_MESSAGE_OPCODE, BYTE(self.matrix_id),
INT(speed), BYTE(mode), STRING(message))
def setVoltageLimit(self, DVoltage, UVoltage):
self.send(_SET_VOLTAGE_LIMIT_OPCODE, BYTE(DVoltage), BYTE(UVoltage))
def on(self, nb):
self.send(LED_ON_OPCODE, BYTE(nb))
def setEndlessMode(self, Status):
self.send(_SET_ENDLESS_MODE_OPCODE, BYTE(Status))
def get_velocities_wanted(self, real_output=False):
output = self.execute(GET_VELOCITIES_WANTED_OPCODE,
BYTE(int(real_output)))
return output.read(FLOAT, FLOAT)
def get_parameter_value(self, id, valuetype):
output = self.execute(GET_PARAMETER_VALUE_OPCODE, BYTE(id))
value = output.read(valuetype)
return value
def setCSlope(self, CWCSlope, CCWCSlope):
self.send(_SET_CSLOPE_OPCODE, BYTE(CWCSlope), BYTE(CCWCSlope))
def setCMargin(self, CWCMargin, CCWCMargin):
self.send(_SET_CMARGIN_OPCODE, BYTE(CWCMargin), BYTE(CCWCMargin))
def off(self, nb):
self.send(LED_OFF_OPCODE, BYTE(nb))
def get_joint(self):
output = self.execute(_GET_PARAMETERS_OPCODE, BYTE(JOINTS_ID))
value = output.read(FLOAT, FLOAT, FLOAT)
return JointPoint(*value)
def setTempLimit(self, Temp):
self.send(_SET_TEMP_LIMIT_OPCODE, BYTE(Temp))
def attach(self, id):
self.send(_ATTACH_OPCODE, BYTE(id))
def set_speed(self, speed): # `speed` in milliseconds
self.send(SET_SPEED_MATRIX_OPCODE, BYTE(self.matrix_id), INT(speed))
def set_parameter_value(self, id, value, valuetype):
self.send(SET_PARAMETER_VALUE_OPCODE, BYTE(id), valuetype(value))
time.sleep(0.01)
def detach(self):
self.send(_DETACH_OPCODE, BYTE(id))
class WheeledBase(SecureSerialTalksProxy):
_DEFAULT = {
GET_CODEWHEELS_COUNTERS_OPCODE: Deserializer(LONG(0) + LONG(0)),
POSITION_REACHED_OPCODE: Deserializer(BYTE(0) + BYTE(0)),
GET_VELOCITIES_WANTED_OPCODE: Deserializer(FLOAT(0) + FLOAT(0)),
GET_POSITION_OPCODE: Deserializer(FLOAT(0) + FLOAT(0) + FLOAT(0)),
GET_VELOCITIES_OPCODE: Deserializer(FLOAT(0) + FLOAT(0)),
GET_PARAMETER_VALUE_OPCODE: Deserializer(LONG(0) + LONG(0))
}
FORWARD = 1
BACKWARD = 2
NO_DIR = 0
LATCH_TIMESTEP = 0.2
class Parameter():
def __init__(self, parent, id, type):
self.parent = parent
self.id = id
self.type = type
def get(self):
return self.parent.get_parameter_value(self.id, self.type)
def set(self, value):
self.parent.set_parameter_value(self.id, value, self.type)
def __init__(self, parent, uuid='wheeledbase'):
SecureSerialTalksProxy.__init__(self, parent, uuid,
WheeledBase._DEFAULT)
self.left_wheel_radius = WheeledBase.Parameter(self,
LEFTWHEEL_RADIUS_ID,
FLOAT)
self.left_wheel_constant = WheeledBase.Parameter(
self, LEFTWHEEL_CONSTANT_ID, FLOAT)
self.left_wheel_maxPWM = WheeledBase.Parameter(self,
LEFTWHEEL_MAXPWM_ID,
FLOAT)
self.right_wheel_radius = WheeledBase.Parameter(
self, RIGHTWHEEL_RADIUS_ID, FLOAT)
self.right_wheel_constant = WheeledBase.Parameter(
self, RIGHTWHEEL_CONSTANT_ID, FLOAT)
self.right_wheel_maxPWM = WheeledBase.Parameter(
self, RIGHTWHEEL_MAXPWM_ID, FLOAT)
self.left_codewheel_radius = WheeledBase.Parameter(
self, LEFTCODEWHEEL_RADIUS_ID, FLOAT)
self.left_codewheel_counts_per_rev = WheeledBase.Parameter(
self, LEFTCODEWHEEL_COUNTSPERREV_ID, LONG)
self.right_codewheel_radius = WheeledBase.Parameter(
self, RIGHTCODEWHEEL_RADIUS_ID, FLOAT)
self.right_codewheel_counts_per_rev = WheeledBase.Parameter(
self, RIGHTCODEWHEEL_COUNTSPERREV_ID, LONG)
self.codewheels_axletrack = WheeledBase.Parameter(
self, ODOMETRY_AXLETRACK_ID, FLOAT)
self.odometry_slippage = WheeledBase.Parameter(self,
ODOMETRY_SLIPPAGE_ID,
FLOAT)
self.wheels_axletrack = WheeledBase.Parameter(
self, VELOCITYCONTROL_AXLETRACK_ID, FLOAT)
self.max_linacc = WheeledBase.Parameter(self,
VELOCITYCONTROL_MAXLINACC_ID,
FLOAT)
self.max_lindec = WheeledBase.Parameter(self,
VELOCITYCONTROL_MAXLINDEC_ID,
FLOAT)
self.max_angacc = WheeledBase.Parameter(self,
VELOCITYCONTROL_MAXANGACC_ID,
FLOAT)
self.max_angdec = WheeledBase.Parameter(self,
VELOCITYCONTROL_MAXANGDEC_ID,
FLOAT)
self.spin_shutdown = WheeledBase.Parameter(
self, VELOCITYCONTROL_SPINSHUTDOWN_ID, BYTE)
self.linvel_KP = WheeledBase.Parameter(self, LINVELPID_KP_ID, FLOAT)
self.linvel_KI = WheeledBase.Parameter(self, LINVELPID_KI_ID, FLOAT)
self.linvel_KD = WheeledBase.Parameter(self, LINVELPID_KD_ID, FLOAT)
self.angvel_KP = WheeledBase.Parameter(self, ANGVELPID_KP_ID, FLOAT)
self.angvel_KI = WheeledBase.Parameter(self, ANGVELPID_KI_ID, FLOAT)
self.angvel_KD = WheeledBase.Parameter(self, ANGVELPID_KD_ID, FLOAT)
self.linpos_KP = WheeledBase.Parameter(self,
POSITIONCONTROL_LINVELKP_ID,
FLOAT)
self.angpos_KP = WheeledBase.Parameter(self,
POSITIONCONTROL_ANGVELKP_ID,
FLOAT)
self.max_linvel = WheeledBase.Parameter(self,
POSITIONCONTROL_LINVELMAX_ID,
FLOAT)
self.max_angvel = WheeledBase.Parameter(self,
POSITIONCONTROL_ANGVELMAX_ID,
FLOAT)
self.linpos_threshold = WheeledBase.Parameter(
self, POSITIONCONTROL_LINPOSTHRESHOLD_ID, FLOAT)
self.angpos_threshold = WheeledBase.Parameter(
self, POSITIONCONTROL_ANGPOSTHRESHOLD_ID, FLOAT)
self.lookahead = WheeledBase.Parameter(self, PUREPURSUIT_LOOKAHEAD_ID,
FLOAT)
self.lookaheadbis = WheeledBase.Parameter(self,
PUREPURSUIT_LOOKAHEADBIS_ID,
FLOAT)
self.x = 0
self.y = 0
self.theta = 0
self.previous_measure = 0
self.direction = self.NO_DIR
self.final_angle = 0
self.latch = None
self.latch_time = None
def set_openloop_velocities(self, left, right):
self.send(SET_OPENLOOP_VELOCITIES_OPCODE, FLOAT(left), FLOAT(right))
def get_codewheels_counter(self, **kwargs):
output = self.execute(GET_CODEWHEELS_COUNTERS_OPCODE, **kwargs)
left, right = output.read(LONG, LONG)
return left, right
def set_velocities(self, linear_velocity, angular_velocity):
self.send(SET_VELOCITIES_OPCODE, FLOAT(linear_velocity),
FLOAT(angular_velocity))
def purepursuit(self,
waypoints,
direction='forward',
finalangle=None,
lookahead=None,
lookaheadbis=None,
linvelmax=None,
angvelmax=None,
**kwargs):
if len(waypoints) < 2:
raise ValueError('not enough waypoints')
self.send(RESET_PUREPURSUIT_OPCODE)
for x, y in waypoints:
self.send(ADD_PUREPURSUIT_WAYPOINT_OPCODE, FLOAT(x), FLOAT(y))
if lookahead is not None:
self.set_parameter_value(PUREPURSUIT_LOOKAHEAD_ID, lookahead,
FLOAT)
if lookaheadbis is not None:
self.set_parameter_value(PUREPURSUIT_LOOKAHEADBIS_ID, lookaheadbis,
FLOAT)
if linvelmax is not None:
self.set_parameter_value(POSITIONCONTROL_LINVELMAX_ID, linvelmax,
FLOAT)
if angvelmax is not None:
self.set_parameter_value(POSITIONCONTROL_ANGVELMAX_ID, angvelmax,
FLOAT)
if finalangle is None:
finalangle = math.atan2(waypoints[-1][1] - waypoints[-2][1],
waypoints[-1][0] - waypoints[-2][0])
self.direction = {
'forward': self.FORWARD,
'backward': self.BACKWARD
}[direction]
self.final_angle = finalangle
self.send(START_PUREPURSUIT_OPCODE,
BYTE({
'forward': 0,
'backward': 1
}[direction]), FLOAT(finalangle))
def start_purepursuit(self):
self.send(
START_PUREPURSUIT_OPCODE,
BYTE({
self.NO_DIR: 0,
self.FORWARD: 0,
self.BACKWARD: 1
}[self.direction]), FLOAT(self.final_angle))
def turnonthespot(self, theta, direction=None, way='forward'):
if direction is None:
self.send(START_TURNONTHESPOT_OPCODE, FLOAT(theta),
BYTE({
'forward': 0,
'backward': 1
}[way]))
else:
self.send(START_TURNONTHESPOT_DIR_OPCODE, FLOAT(theta),
BYTE({
'clock': 0,
'trig': 1
}[direction]))
def isarrived(self, **kwargs):
output = self.execute(POSITION_REACHED_OPCODE, **kwargs)
isarrived, spinurgency = output.read(BYTE, BYTE)
if bool(spinurgency):
raise RuntimeError('spin urgency')
return bool(isarrived)
def get_velocities_wanted(self, real_output=False):
output = self.execute(GET_VELOCITIES_WANTED_OPCODE,
BYTE(int(real_output)))
return output.read(FLOAT, FLOAT)
def wait(self, timestep=0.1, timeout=200, command=None, **kwargs):
init_time = time.time()
while not self.isarrived(**kwargs):
time.sleep(timestep)
if (time.time() - init_time > timeout) and (not command is None):
print("RESCUE wheeledbase !")
command()
time.sleep(timestep)
def goto_delta(self, x, y):
self.send(GOTO_DELTA_OPCODE, FLOAT(x) + FLOAT(y))
def goto(self,
x,
y,
theta=None,
direction=None,
finalangle=None,
lookahead=None,
lookaheadbis=None,
linvelmax=None,
angvelmax=None,
**kwargs):
# Compute the preferred direction if not set
if direction is None:
x0, y0, theta0 = self.get_position()
if math.cos(math.atan2(y - y0, x - x0) - theta0) >= 0:
direction = 'forward'
else:
direction = 'backward'
# Go to the setpoint position
self.purepursuit([self.get_position()[0:2],
(x, y)], direction, finalangle, lookahead,
lookaheadbis, linvelmax, angvelmax)
self.wait(**kwargs)
# Get the setpoint orientation
if theta is not None:
self.turnonthespot(theta)
self.wait(**kwargs)
def stop(self):
self.set_openloop_velocities(0, 0)
def set_position(self, x, y, theta):
self.send(SET_POSITION_OPCODE, FLOAT(x), FLOAT(y), FLOAT(theta))
def reset(self):
self.set_position(0, 0, 0)
def get_position(self, **kwargs):
output = self.execute(GET_POSITION_OPCODE, **kwargs)
self.x, self.y, self.theta = output.read(FLOAT, FLOAT, FLOAT)
self.previous_measure = time.time()
return self.x, self.y, self.theta
def get_position_latch(self, **kwargs):
if self.latch is None or time.time(
) - self.latch_time > self.LATCH_TIMESTEP:
self.latch = self.get_position(**kwargs)
self.latch_time = time.time()
return self.latch
def get_position_previous(self, delta):
if time.time() - self.previous_measure > delta:
self.get_position()
return self.x, self.y, self.theta
def get_velocities(self, **kwargs):
output = self.execute(GET_VELOCITIES_OPCODE, **kwargs)
linvel, angvel = output.read(FLOAT, FLOAT)
return linvel, angvel
def set_parameter_value(self, id, value, valuetype):
self.send(SET_PARAMETER_VALUE_OPCODE, BYTE(id), valuetype(value))
time.sleep(0.01)
def get_parameter_value(self, id, valuetype):
output = self.execute(GET_PARAMETER_VALUE_OPCODE, BYTE(id))
value = output.read(valuetype)
return value
def reset_parameters(self):
self.send(RESET_PARAMETERS_OPCODE)
def save_parameters(self):
self.send(SAVE_PARAMETERS_OPCODE)
def setID(self, newID):
self.send(_SET_ID_OPCODE, BYTE(newID))