class SensorsIR(SecureSerialTalksProxy): MIN_TIMESTEP = 0.05 ERROR_DIST = 1000 DEFAULT = { GET_RANGE1_OPCODE: Deserializer(SHORT(ERROR_DIST)), GET_RANGE2_OPCODE: Deserializer(SHORT(ERROR_DIST)) } # Default execute result def __init__(self, parent, uuid='sensors'): SecureSerialTalksProxy.__init__(self, parent, uuid, default_result=self.DEFAULT) self.last_time = None self.last_both = (SensorsIR.ERROR_DIST, SensorsIR.ERROR_DIST) def get_range1(self): return self.get_both_range()[0] def get_range2(self): return self.get_both_range()[1] def is_ready(self): try: return self.is_connected except: return False def check_errors(self): deser = self.execute(CHECK_ERROR_OPCODE) error1 = deser.read(SHORT) error2 = deser.read(SHORT) return error1, error2 def get_both_range(self): current_time = time.time() if self.last_time is not None and current_time - self.last_time < SensorsIR.MIN_TIMESTEP: return self.last_both deser = self.execute(GET_BOTH_RANGE_OPCODE) try: dist1 = deser.read(SHORT) except AttributeError: dist1 = SensorsIR.ERROR_DIST try: dist2 = deser.read(SHORT) except AttributeError: dist2 = SensorsIR.ERROR_DIST if dist1 <= 0: dist1 = SensorsIR.ERROR_DIST if dist2 <= 0: dist2 = SensorsIR.ERROR_DIST self.last_both = (dist1, dist2) self.last_time = time.time() return self.last_both
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 run(self): state = 'waiting' # ['waiting', 'starting', 'receiving'] type_packet = SLAVE_BYTE buffer = bytes() crc_buf = bytes() msglen = 0 while not self.stop.is_set(): # Wait until new bytes arrive try: inc = self.parent.stream.read() except serial.serialutil.SerialException: self.parent.disconnect() break # Finite state machine if state == 'waiting' and inc in [SLAVE_BYTE, MASTER_BYTE]: type_packet = inc state = 'starting' continue elif state == 'starting' and inc: msglen = inc[0] state = 'crc' continue elif state == 'crc': crc_buf += inc if (len(crc_buf) >= SERIALTALKS_CRC_SIZE): crc_val = (crc_buf[1] << 8) | crc_buf[0] state = 'receiving' continue elif state == 'receiving': buffer += inc if (len(buffer) < msglen): continue # Junk byte else: continue # Process the above message try: if (CRCcheck(buffer, crc_val)): if type_packet == SLAVE_BYTE: self.parent.process(Deserializer(buffer)) if type_packet == MASTER_BYTE: self.parent.receive(Deserializer(buffer)) else: warnings.warn("Message receive corrupted !", SerialTalksWarning) state = 'waiting' except NotConnectedError: self.parent.disconnect() break # Reset the finite state machine state = 'waiting' buffer = bytes() crc_buf = bytes()
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)
class BMS(SecureArduino): _DEFAULT = {GET_RSOC_OPCODE: Deserializer(INT(100))} def __init__(self, parent, uuid='/tmp/arduino/BMS'): SecureArduino.__init__(self, parent, uuid, BMS._DEFAULT) def get_relative_soc(self): output = self.execute(GET_RSOC_OPCODE) rsoc = output.read(INT) return rsoc def get_absolute_soc(self): output = self.execute(GET_ASOC_OPCODE) rsoc = output.read(INT) return rsoc def get_voltage(self): output = self.execute(GET_VOLTAGE_OPCODE) rsoc = output.read(INT) return rsoc def get_cycle_count(self): output = self.execute(GET_CYCLE_COUNT_OPCODE) rsoc = output.read(INT) return rsoc def get_current(self): output = self.execute(GET_CURRENT_OPCODE) rsoc = output.read(INT) return rsoc def get_average_current(self): output = self.execute(GET_AVERAGE_CURRENT_OPCODE) rsoc = output.read(INT) return rsoc def get_remaining_capacity(self): output = self.execute(GET_REMAINING_CAP_OPCODE) rsoc = output.read(INT) return rsoc def get_average_time_to_empty(self): output = self.execute(GET_AVERAGE_TIME_EMPTY_OPCODE) rsoc = output.read(INT) return rsoc def get_average_time_to_full(self): output = self.execute(GET_AVERAGE_TIME_FULL_OPCODE) rsoc = output.read(INT) return rsoc
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)
class Sensors(SecureArduino): TIMESTEP = 100 MAX_DIST = 10000 DEFAULT = {GET_SENSOR1_OPCODE: Deserializer(USHORT(MAX_DIST)), GET_SENSOR2_OPCODE: Deserializer(USHORT(MAX_DIST)), GET_SENSOR3_OPCODE: Deserializer(USHORT(MAX_DIST)), GET_SENSOR4_OPCODE: Deserializer(USHORT(MAX_DIST)), GET_SENSOR5_OPCODE: Deserializer(USHORT(MAX_DIST)), GET_SENSOR6_OPCODE: Deserializer(USHORT(MAX_DIST)), GET_SENSOR7_OPCODE: Deserializer(USHORT(MAX_DIST)), GET_SENSOR8_OPCODE: Deserializer(USHORT(MAX_DIST)), } def __init__(self, parent, uuid='SensorBoard'): SecureArduino.__init__(self, parent, uuid, default_result=self.DEFAULT) self.last_time = None try: self.addTopic(GET_ALL_TOPIC_OPCODE, self.get_all_sensors_handler, "sensors", self.TIMESTEP) except: pass self.sensor1 = self.MAX_DIST self.sensor2 = self.MAX_DIST self.sensor3 = self.MAX_DIST self.sensor4 = self.MAX_DIST self.sensor5 = self.MAX_DIST self.sensor6 = self.MAX_DIST self.sensor7 = self.MAX_DIST self.sensor8 = self.MAX_DIST @TopicHandler(USHORT, USHORT, USHORT, USHORT, USHORT, USHORT, USHORT, USHORT) def get_all_sensors_handler(self, sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8): self.sensor1 = sensor1 self.sensor2 = sensor2 self.sensor3 = sensor3 self.sensor4 = sensor4 self.sensor5 = sensor5 self.sensor6 = sensor6 self.sensor7 = sensor7 self.sensor8 = sensor8 def get_all(self): return [self.sensor1, self.sensor2, self.sensor3, self.sensor4, self.sensor5, self.sensor6, self.sensor7, self.sensor8] def get_range_left_front(self): return self.sensor1, UNUSED_SENSOR def get_range_mid_left_front(self): return self.sensor2, UNUSED_SENSOR def get_range_mid_right_front(self): return self.sensor3, UNUSED_SENSOR def get_range_right_front(self): return self.sensor4, UNUSED_SENSOR def get_range_left_back(self): return self.sensor5, UNUSED_SENSOR def get_range_mid_left_back(self): return self.sensor6, UNUSED_SENSOR def get_range_mid_right_back(self): return self.sensor7, UNUSED_SENSOR def get_range_right_back(self): return self.sensor8, UNUSED_SENSOR def get_sensor1_range(self): return self.execute(GET_SENSOR1_OPCODE).read(USHORT) , UNUSED_SENSOR def get_sensor2_range(self): return self.execute(GET_SENSOR2_OPCODE).read(USHORT) , UNUSED_SENSOR def get_sensor3_range(self): return self.execute(GET_SENSOR3_OPCODE).read(USHORT) , UNUSED_SENSOR def get_sensor4_range(self): return self.execute(GET_SENSOR4_OPCODE).read(USHORT) , UNUSED_SENSOR def get_sensor5_range(self): return self.execute(GET_SENSOR5_OPCODE).read(USHORT) , UNUSED_SENSOR def get_sensor6_range(self): return self.execute(GET_SENSOR6_OPCODE).read(USHORT) , UNUSED_SENSOR def get_sensor7_range(self): return self.execute(GET_SENSOR7_OPCODE).read(USHORT) , UNUSED_SENSOR def get_sensor8_range(self): return self.execute(GET_SENSOR8_OPCODE).read(USHORT) , UNUSED_SENSOR def is_ready(self): try: return self.is_connected except: return False def check_errors(self): deser = self.execute(CHECK_ERROR_OPCODE) error = deser.read(BYTE) return error