def get_pos(): """Get the robot's current position""" try: x = con.convert_ir_cm(sendCommand(COMMANDS.GetSensorValue, receiver=IR_X)) except ZeroDivisionError: x = 40 try: y = con.convert_ir_cm(sendCommand(COMMANDS.GetSensorValue, receiver=IR_Y)) except ZeroDivisionError: y = 40 return x, y
def init_motors(): """Intialise the motors""" for motor in MOTORS: if motor == LEFT_FRONT_E or motor == RIGHT_BACK_E: sendCommand(COMMANDS.SetMotorTargetPosition, receiver=motor, val=target0) elif motor == RIGHT_FRONT_E or motor == LEFT_BACK_E: sendCommand(COMMANDS.SetMotorTargetPosition, receiver=motor, val=target1) elif motor == LEFT_FRONT or motor == LEFT_BACK: sendCommand(COMMANDS.SetMotorTargetPosition, receiver=motor, val=target2) elif motor == RIGHT_BACK: sendCommand(COMMANDS.SetMotorTargetPosition, receiver=motor, val=target0) elif motor == RIGHT_FRONT: sendCommand(COMMANDS.SetMotorTargetPosition, receiver=motor, val=target1)
def walk_E(): """Make the robot move to the east""" sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK_E, val=250) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK, val=target0) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK_E, val=target0) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK_E, val=774) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK, val=target1) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK_E, val=target1) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT_E, val=774) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT, val=target1) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT_E, val=target1) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT_E, val=250) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT, val=target0) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT_E, val=target0)
def walk_S(): """Make the robot move to the south""" sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK_E, val=774) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK, val=target1) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK_E, val=target1) rospy.sleep(0.5) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT_E, val=774) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT, val=target0) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT_E, val=target1) rospy.sleep(0.5) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK_E, val=300) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK, val=target0) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_BACK, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_BACK_E, val=target0) rospy.sleep(0.5) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT_E, val=300) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT, val=target2) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=RIGHT_FRONT, val=target1) sendCommand(COMMANDS.SetMotorTargetPosition, receiver=LEFT_FRONT_E, val=target0) rospy.sleep(0.5)
def convert_ir_cm(ir_val):
"""Convert values from the IR sensors into centimetres
:param ir_val: IR sensor value
"""
return 98.098 * (1 / (ir_val ** 0.406))
def avg(vals):
"""Compute the average value of a list of values
:param vals: The list of values
"""
r = 0
for v in vals:
r += v
return r / len(vals)
if __name__ == '__main__':
rospy.init_node('test_node', anonymous=True)
v = sendCommand(COMMANDS.GetSensorValue, receiver=1)
print 'sensor value: %f' % v
print 'function returns: %f' % convert_dms_cm(v)
# vals = []
# if raw_input('go? ') == 'y':
# for i in xrange(1000):
# print i
# vals.append(sendCommand(COMMANDS.GetSensorValue, receiver=1))
# print avg(vals)
