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)