主页
电子
模型
机器人
互动实验
机器人
ROS机器人Diego 1#制作(十五)机械臂的控制---通过键盘控制机械臂舵机
robot
2020年4月30日
582
为了便于调试,特意写了一个通过键盘控制机械臂舵机的代码,来调试舵机,代码是基于teleop_twist_keyboard修改的。在arduino_node.py中已经声明了舵机读和写的ROS Service: ``` # A service to position a PWM servo rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler) # A service to read the position of a PWM servo rospy.Service('~servo_read', ServoRead, self.ServoReadHandler) ``` 配合上一篇微博中arduino端的舵机驱动,我们只需要实现这两个ROS Service的client就可以控制舵机了,具体原理请看代码中的注释代码如下: ``` #!/usr/bin/env python import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy from geometry_msgs.msg import Twist from ros_arduino_msgs.srv import * from math import pi as PI import sys, select, termios, tty msg = """ Reading from the keyboard and Publishing to Twist, Servo or sensor! --------------------------- Moving around: u i o j k l , : up (+z) . : down (-z) m : stop ---------------------------- Left arm servo control:控制机器人左臂,每次调整0.09弧度 + 1 2 3 4 5 6 - q w e r t y ---------------------------- Right arm servo control:控制机器人右臂,每次调整0.09弧度 + a s d f g h - z x c v b n p : init the servo 初始化舵机 CTRL-C to quit """ moveBindings = { 'i':(1,0,0,0), 'o':(1,0,0,1), 'j':(-1,0,0,-1), 'l':(-1,0,0,1), 'u':(1,0,0,-1), 'k':(-1,0,0,0), ',':(0,0,1,0), '.':(0,0,-1,0), } #右臂舵机的控制数组,1表示增加0.09弧度,0表示减少0.09弧度 rightArmServo={ '1':(0,1), '2':(1,1), '3':(2,1), '4':(3,1), '5':(4,1), '6':(5,1), 'q':(0,0), 'w':(1,0), 'e':(2,0), 'r':(3,0), 't':(4,0), 'y':(5,0), } #左臂舵机的控制数组,1表示增加0.09弧度,0表示减少0.09弧度 leftArmServo={ 'a':(6,1), 's':(7,1), 'd':(8,1), 'f':(9,1), 'g':(10,1), 'h':(11,1), 'z':(6,0), 'x':(7,0), 'c':(8,0), 'v':(9,0), 'b':(10,0), 'n':(11,0), } #手臂臂舵机的控制命令数组 armCmd={ '[':(0,1),#左臂初始化 ']':(1,1),#右臂初始化 } def getradians(angle): return PI*angle/180 def getKey(): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key #舵机当前位置读取,调用servo_read service def servoRead(servoNum): rospy.wait_for_service('/arduino/servo_read') try: readServo=rospy.ServiceProxy('/arduino/servo_read',ServoRead) return readServo(servoNum) except rospy.ServiceException, e: print "Service call failed: %s"%e #舵机位置写,根据舵机标号设置舵机的位置0~π,注意这里的value对应的是弧度 def servoWrite(servoNum, value): rospy.wait_for_service('/arduino/servo_write') try: servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite) servo_write(servoNum,value) print servoNum print value except rospy.ServiceException, e: print "Service call failed: %s"%e #初始化右臂舵机角度,要根据实际情况调整舵机角度 def initRightArm(): servoWrite(0,getradians(60)) servoWrite(1,getradians(80)) servoWrite(2,getradians(90)) servoWrite(3,getradians(90)) servoWrite(4,getradians(90)) servoWrite(5,getradians(90)) #初始化左臂舵机角度,要根据实际情况调整舵机角度 def initLeftArm(): servoWrite(6,getradians(90)) servoWrite(7,getradians(90)) servoWrite(8,getradians(90)) servoWrite(9,getradians(90)) servoWrite(10,getradians(90)) servoWrite(11,getradians(90)) def vels(speed,turn): return "currently:\tspeed %s\tturn %s " % (speed,turn) if __name__=="__main__": settings = termios.tcgetattr(sys.stdin) pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1) rospy.init_node('teleop_twist_keyboard') speed = rospy.get_param("~speed", 0.5) turn = rospy.get_param("~turn", 1.0) x = 0 y = 0 z = 0 th = 0 status = 0 try: print msg print vels(speed,turn) while(1): key = getKey() print key if key in moveBindings.keys(): x = moveBindings[key][0] y = moveBindings[key][1] z = moveBindings[key][2] th = moveBindings[key][3] elif key in leftArmServo.keys():#左臂舵机控制 value=servoRead(leftArmServo[key][1]).value if(leftArmServo[key][1]==0): value=value-getradians(5) if value<=0: value=0 else: value=value+getradians(5) if value>=PI: value=PI servoWrite(leftArmServo[key][0], value) elif key in rightArmServo.keys():#右臂舵机控制 value=servoRead(rightArmServo[key][1]).value if(rightArmServo[key][1]==0): value=value-getradians(5) if value<=0: value=0 else: value=value+getradians(5) if value>=PI: value=PI servoWrite(rightArmServo[key][0], value) elif key in armCmd.keys():#舵机初始化 if(armCmd[key][0]==0): initRightArm() elif(armCmd[key][0]==1): initLeftArm() else: x = 0 y = 0 z = 0 th = 0 if (key == '\x03'): break twist = Twist() twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed; twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn pub.publish(twist) except BaseException,e: print e finally: twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 pub.publish(twist) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) ``` 这里需要强调一下硬件的使用过程中,一定要保证供电的功率足够,否则的话会出现串口通信异常,机械臂乱动等现象,这都是电源供电不足的问题导致的。
树莓派
Arduino
机器人
ROS
0 评论
发表评论
搜索
公众号
少而好学,如日出之阳。
最新文章
模型
又一款乐高弹射器
robot
2021年2月17日
模型
MOC-2412 大皮卡
robot
2021年2月17日
模型
乐高烈火摩托车
robot
2021年2月17日
模型
乐高弹射器
robot
2021年2月17日
模型
乐高弩箭
robot
2021年2月17日
模型
乐高小手枪
robot
2021年2月17日
小编推荐
机器人
ROS机器人Diego 1#制作(三十二)机器视觉 -整合Tensorflow MNIST,玩数字识别
robot
2020年4月30日
50K
电子
树莓派制造低成本交互式显微镜
robot
2020年6月27日
50K
电子
树莓派制造的绘图机器人
robot
2020年4月13日
50K
电子
特斯拉+树莓派实现车牌识别检测系统
robot
2020年4月16日
50K
电子
三维激光扫描建模仪(基于树莓派)
robot
2020年4月11日
50K
发表评论