|
|
@@ -41,8 +41,8 @@ w/x : increase/decrease only linear speed by 10%
|
|
|
e/c : increase/decrease only angular speed by 10%
|
|
|
4/5 : increase/decrease gripper angle by 5°
|
|
|
|
|
|
-1 : 钻头上移(同时钻头反转退出)
|
|
|
-2 : 钻头下降(同时钻头正转打孔)
|
|
|
+1 : 钻头下降(同时钻头正转打孔)
|
|
|
+2 : 钻头上移(同时钻头反转退出)
|
|
|
3 : 运送树苗链条转动
|
|
|
4 : 放下树苗
|
|
|
5 : 返回原位
|
|
|
@@ -87,10 +87,10 @@ bitDownBindings={
|
|
|
chainRotateBindings={
|
|
|
'3':(),
|
|
|
}
|
|
|
-gripperopenBindings={
|
|
|
+putDownBindings={
|
|
|
'4':(5),
|
|
|
}
|
|
|
-grippercloseBindings={
|
|
|
+returnBindings={
|
|
|
'5':(-5),
|
|
|
}
|
|
|
|
|
|
@@ -104,10 +104,6 @@ def getKey():
|
|
|
def vels(speed, turn):
|
|
|
return "currently:\tspeed %s\tturn %s " % (speed,turn)
|
|
|
|
|
|
-def angles(angle):
|
|
|
- return "currently:\tangle %s " % (angle)
|
|
|
-
|
|
|
-
|
|
|
#调用service: mobilebase_arduino/gripper_angle控制夹爪开闭
|
|
|
def control_180Servo(servoID, angle):
|
|
|
rospy.wait_for_service("mobilebase_arduino/gripper_angle")
|
|
|
@@ -189,28 +185,22 @@ if __name__=="__main__":
|
|
|
y = 0
|
|
|
z = 0
|
|
|
th = 0
|
|
|
- control_360Servo(2, 1)
|
|
|
- time.sleep(0.3)
|
|
|
+ control_360Servo(2, 3)
|
|
|
+ time.sleep(0.05)
|
|
|
#停止舵机转动
|
|
|
- control_360Servo(1, 2)
|
|
|
- elif key in gripperopenBindings.keys():
|
|
|
+ control_360Servo(2, 2)
|
|
|
+ elif key in putDownBindings.keys():
|
|
|
x = 0
|
|
|
y = 0
|
|
|
z = 0
|
|
|
th = 0
|
|
|
- print(angles(angle))
|
|
|
- if (angle < 65 and angle >=0):
|
|
|
- angle = angle + gripperopenBindings[key]
|
|
|
- client_srv()
|
|
|
- elif key in grippercloseBindings.keys():
|
|
|
+ control_180Servo(3, 0)
|
|
|
+ elif key in returnBindings.keys():
|
|
|
x = 0
|
|
|
y = 0
|
|
|
z = 0
|
|
|
th = 0
|
|
|
- print(angles(angle))
|
|
|
- if (angle <= 65 and angle >0):
|
|
|
- angle = angle + grippercloseBindings[key]
|
|
|
- client_srv()
|
|
|
+ control_180Servo(3, 160)
|
|
|
else:
|
|
|
x = 0
|
|
|
y = 0
|
