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))
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
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 up(self):
self.send(_SET_POSITION_ARM_OPCODE, INT(UP_ARM))
def deploy(self):
self.send(_SET_POSITION_ARM_OPCODE, INT(DEPLOYED_ARM))
def put(self, gripper_id):
self.send(_PUT_CUP_OPCODE, INT(gripper_id))
def setBD(self, newBD):
self.send(_SET_BD_OPCODE, INT(newBD))
def set_orientation(self, o):
self.send(CALIBRATE_COMPAS_OPCODE, INT(o))
def write_position(self, pos_p2, pos_p1):
self.send(_SET_POSITION_PUSHERS_OPCODE, INT(pos_p1), INT(pos_p2))
def close(self):
self.send(_SET_POSITION_GRIPPER_OPCODE, INT(CLOSED_GRIPPER))
def open(self):
self.send(_SET_POSITION_GRIPPER_OPCODE, INT(OPEN_GRIPPER))
def write_outdoor(self, position):
result = self.execute(_SET_POSITION_OPCODE, INT(position))
return result.read(INT)
def get_color(self, gripper_id):
ret = self.execute(_GET_COLOR_CUP_OPCODE, INT(gripper_id))
return ret.read(STRING)
def set_speed(self, speed): # `speed` in milliseconds
self.send(SET_SPEED_MATRIX_OPCODE, BYTE(self.matrix_id), INT(speed))
def up_r(self):
self.right_pusher = UP_PUSHER2
self.send(_SET_POSITION_PUSHERS_OPCODE, INT(self.right_pusher),
INT(self.left_pusher))
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 down(self):
self.right_pusher = DOWN_PUSHER2
self.left_pusher = DOWN_PUSHER1
self.send(_SET_POSITION_PUSHERS_OPCODE, INT(self.right_pusher),
INT(self.left_pusher))
def setPunch(self, punch):
self.send(_SET_PUNCH_OPCODE, INT(punch))
def write_position(self, position):
self.send(_SET_POSITION_ARM_OPCODE, INT(position))
def setMaxTorqueRAM(self, MaxTorque):
self.send(_SET_MAX_TORQUE_RAM_OPCODE, INT(MaxTorque))
def get(self, gripper_id):
self.send(_GET_CUP_OPCODE, INT(gripper_id))
