def srvMotorSetRotate(self, params):
# print(params)
cur_position = Lepi.motor_get_current_position(params.port)
# print(cur_position,params.value*2+cur_position)
Lepi.motor_set_target_position(params.port,
params.value * 2 + cur_position)
return SetInt32Response(params.port, params.value)
def srvServoSetU8(self, params):
if params.param_id == 0x99:
status = Lepi.servo_reset(params.id)
else:
status = Lepi.servo_write_u8(params.id, params.param_id,
params.value)
return SetServoParamResponse(status)
def srvMotorsGetInfo(self,msg): # return GetMotorsInfoResponse() # print(msg) infos = GetMotorsInfoResponse() # print(MotorInfo()) for i in [1,2,3,4,5]: info = Lepi.motor_get_info(i) # print(info) infos.motors.append(MotorInfo(info[0],info[1],info[2],info[3])) return infos
def srvSensorsGetInfo(self, params):
# return GetMotorsInfoResponse()
# print(params)
infos = GetMotorsInfoResponse()
# print(MotorInfo())
for i in [1, 2, 3, 4, 5]:
info = Lepi.sensor_get_info(6 - i)
# print(info)
infos.motors.append(MotorInfo(info[0], info[1], info[2], info[3]))
return infos
def srvServosGetInfo(self, params):
# print(params)
rsp = GetServosInfoResponse()
if len(params.ids) == 0:
ids = Lepi.servo_scan()
else:
ids = params.ids
print(ids)
for servo_id in ids:
info = ServoInfo()
info.id = servo_id
# print(servo_id)
info.min_position = Lepi.servo_read_u16(servo_id,
EEPROM.MIN_POSITION_H)
info.cur_position = Lepi.servo_read_u16(servo_id,
EEPROM.CURRENT_POSITION_H)
info.max_position = Lepi.servo_read_u16(servo_id,
EEPROM.MAX_POSITION_H)
rsp.servos.append(info)
return rsp
def srvSensorSetMode(self, params):
status = Lepi.sensor_set_mode(6 - params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvServoGetU16(self, params):
status = Lepi.servo_read_u16(params.id, params.param_id)
return SetServoParamResponse(status)
def srvSensorSetValue(self, params):
data = Lepi.sensor_set_value(6 - params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvMotorSetType(self, params):
# print(params)
res = Lepi.motor_set_type(params.port, params.value)
# print(res)
return SetInt32Response(params.port, params.value)
def cbMotorSetAngle(self, msg):
Lepi.motor_set_angle(msg.port, msg.value)
def cbMotorSetSpeed(self, msg):
Lepi.motor_set_speed(msg.port, msg.value)
def cbMotorSetPulse(self, msg):
Lepi.motor_set_pulse(msg.port, msg.value)
def __init__(self):
self.left = Lepi.MOTOR_2
self.right = Lepi.MOTOR_3
Lepi.motor_enable(self.left)
Lepi.motor_enable(self.right)
def setWheelsSpeed(self, left=0, right=0):
Lepi.motor_set_speed(self.left, int(left * 60000))
Lepi.motor_set_speed(self.right, int(right * 60000))
def srvSensorGetValue(self,msg): data = Lepi.sensor_get_value(msg.port) return GetInt32Response(data)
def srvMotorSetCurrentPosition(self, params):
# print(params)
Lepi.motor_set_current_position(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvMotorGetPosition(self, params):
# print(params)
position = Lepi.motor_get_current_position(params.port)
return GetInt32Response(position)
def srvMotorGetState(self, params):
# print(params)
value = Lepi.motor_get_state(params.port)
return GetInt32Response(value)
def srvSensorGetValue(self, params):
data = Lepi.sensor_get_value(6 - params.port)
return GetInt32Response(data)
def srvMotorSetEnable(self, params):
# print(params)
Lepi.motor_set_enable(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvSensorGetInfo(self, params):
sensor_id = Lepi.sensor_get_type(params.port)
value = Lepi.sensor_get_value(params.port)
return GetSensorInfoResponse(params.port, sensor_id, value)
def srvMotorGetEnable(self, params):
# print(params)
enabled = Lepi.motor_get_enable(params.port)
return GetInt32Response(enabled)
def srvServoSetPosition(self, params):
status = Lepi.servo_set_position(params.id, params.position, params.ms,
params.speed)
return SetServoPositionResponse(status)
def srvMotorSetSpeed(self, params):
# print(params)
Lepi.motor_set_speed(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvServoSetU16(self, params):
status = Lepi.servo_write_u16(params.id, params.param_id, params.value)
return SetServoParamResponse(status)
def srvMotorGetSpeed(self, params):
# print(params)
speed = Lepi.motor_get_speed(params.port)
return GetInt32Response(speed)
def srvSensorGetMode(self, params):
status = Lepi.sensor_get_mode(6 - params.port)
return GetInt32Response(status)
def srvMotorGetPulse(self, params):
value = Lepi.motor_get_pulse(params.port)
return GetInt32Response(value)
def srvGetFirmwareVersion(self, params):
version = Lepi.system_get_version()
return GetInt32Response(version)
def srvMotorGetPosition(self,msg): print(msg) position = Lepi.motor_get_current_position(msg.port) return GetInt32Response(position)
