Skip to content

Commit

Permalink
Merge pull request #46 from elephantrobotics/fix_myarm_handle
Browse files Browse the repository at this point in the history 
Fix myarm handle
  • Loading branch information
@wangWking
wangWking authored Oct 16, 2023
2 parents 289f0f8 + b08a12d commit 8ced525
Show file tree
Hide file tree
Showing 15 changed files with 1,173 additions and 64 deletions.
74 changes: 10 additions & 64 deletions demo/handle_control/myarm_handle_control.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,10 @@
import time
from pymycobot import MyArm
import threading
import RPi.GPIO as GPIO

mc = MyArm("/dev/ttyAMA0", debug=True)

# The default initial point can be changed, if you want to change, you should change 'command' and 'zero' at the same time
# command = [144.8, -66.9, 185.3, 178.47, 0.87, -115.07]
# zero = [144.8, -66.9, 185.3, 178.47, 0.87, -115.07]
#command = [127.7, 0.8, 234.5, -179.9, 1.05, 0.52]
#zero = [127.7, 0.8, 234.5, -179.9, 1.05, 0.52]
command = [111.8, -63.8, 236.3, -179.82, -0.35, -29.18]
zero = [111.8, -63.8, 236.3, -179.82, -0.35, -29.18]

Expand All @@ -20,30 +15,6 @@
is_enable = False
is_zero = False
gripper_value = 1
print('start handle_control')
# 初始化
#GPIO.setmode(GPIO.BCM)
# 引脚20/21分别控制电磁阀和泄气阀门
#GPIO.setup(20, GPIO.OUT)
#GPIO.setup(21, GPIO.OUT)


# 开启吸泵
def pump_on():
# 打开电磁阀
GPIO.output(20, 0)


# 停止吸泵
def pump_off():
# 关闭电磁阀
GPIO.output(20, 1)
time.sleep(0.05)
# 打开泄气阀门
GPIO.output(21, 0)
time.sleep(1)
GPIO.output(21, 1)
time.sleep(0.05)


def control():
Expand All @@ -54,45 +25,37 @@ def control():
command[0] += 2
if command[0] > 285:
command[0] = 280
# mc.send_coords(command, speed)
print('x + coords---->:', command[0])
mc.send_coord(1, command[0], speed)
#mc.send_angles([0, -30, 0, -90, 0, -60, 0], 30)
elif action == 2:
command[0] -= 2
if command[0] < -285:
command[0] = -280
print('x - coords---->:', command[0])
# mc.send_coords(command, speed)
mc.send_coord(1, command[0], speed)
#mc.send_angles([0, 30, 0, -90, 0, -60, 0], 30)
elif action == 3:
command[1] += 2
if command[1] > 285:
command[1] = 280
print('y + coords---->:', command[1])
mc.send_coord(2, command[1], speed)
#mc.send_angles([60, -30, 0, -90, 0, -60, 0], 30)
elif action == 4:
command[1] -= 2
if command[1] < -285:
command[1] = -280
print('y - coords---->:', command[1])
mc.send_coord(2, command[1], speed)
#mc.send_angles([-60, -30, 0, -90, 0, -60, 0], 30)
elif action == 5:
command[2] += 2
if command[2] > 450:
command[2] = 445
print('z1111')
mc.send_coord(3, command[2], speed)
elif action == 6:
command[2] -= 2
if command[2] < -120:
command[2] = -118
mc.send_coord(3, command[2], speed)
elif action == 7:
#is_enable = True
if mc.is_all_servo_enable() != 1:
mc.set_color(0, 0, 0)
time.sleep(0.5)
Expand All @@ -107,7 +70,6 @@ def control():
mc.set_color(255, 0, 0)
is_enable = False
else:
#is_enable = True
mc.set_color(0, 0, 0)
time.sleep(0.5)
mc.set_color(0, 255, 0)
Expand All @@ -122,30 +84,26 @@ def control():
is_enable = True
action = 0
elif action == 8:
#gripper_value += 5
#if gripper_value > 100:
# gripper_value = 100
#mc.set_gripper_value(gripper_value, speed)
gripper_value = 95
mc.set_gripper_value(gripper_value, speed, 1)
command[2] += 2
if command[2] > 450:
command[2] = 445
print('z + coords---->:', command[2])
mc.send_coord(3, command[2], speed)
elif action == 9:
#gripper_value -= 5
#if gripper_value < 0:
# gripper_value = 0
#mc.set_gripper_value(gripper_value, speed)
command[2] -= 2
if command[2] < -120:
command[2] = -118
print('z - coords---->:', command[2])
mc.send_coord(3, command[2], speed)
elif action == 10:
pump_on()
gripper_value = 95
mc.set_gripper_value(gripper_value, speed)
action = 0
elif action == 11:
pump_off()
gripper_value = 5
mc.set_gripper_value(gripper_value, speed)
action = 0
elif action == 12:
command[3] += 10
Expand All @@ -172,13 +130,12 @@ def control():
command[5] += 10
if command[5] > 450:
command[5] = 440
print('z222')
#mc.send_coord(6, command[5], speed)
# mc.send_coord(6, command[5], speed)
elif action == 17:
command[5] -= 10
if command[5] < -120:
command[5] = -118
#mc.send_coord(6, command[5], speed)
# mc.send_coord(6, command[5], speed)
elif action == 18:
# start_time = time.time()
time.sleep(2)
Expand All @@ -200,11 +157,8 @@ def control():
elif action == 21:
time.sleep(2)
if action == 21:
print('isisisisiisisisisisisisisiis:', is_enable)
mc.send_angles([-30, 0, 0, -90, 0, -90, 0], 30)
time.sleep(3)
# print('init_coords:', mc.get_coords())
# mc.send_coords(zero, 20)
command = zero.copy()
print('command', command)
action = 0
Expand All @@ -213,7 +167,7 @@ def control():
else:
# print(action)
if action == 7:
#is_enable = True
# is_enable = True
statue = 0
for _ in range(3):
statue = mc.is_all_servo_enable()
Expand Down Expand Up @@ -286,7 +240,6 @@ def main():
# 获取所有按键状态信息
for i in range(buttons):
button = joystick.get_button(i)
print('*' * 30)
print('i value:', i)
print('button---------->', button)

Expand Down Expand Up @@ -344,18 +297,14 @@ def main():
action = 0
print("button " + str(i) + ": " + str(button))
print('action--->', action)
print('*' * 30)
print('\n')
# 轴转动事件
elif event_.type == pygame.JOYAXISMOTION:
axes = joystick.get_numaxes()
# 获取所有轴状态信息
# while True:
for i in range(axes):
axis = joystick.get_axis(i)
print('*' * 30)
print('i value:', i)
print('axis---------->', axis)
print('i value: {} axis value: {}'.format(i, axis))
# res[i] = axis
if i == 1:
if axis < -3.0517578125e-05:
Expand Down Expand Up @@ -418,9 +367,6 @@ def main():
start_time = 0
action = 0
print("axis " + str(i) + ": " + str(axis))
print('action--->', action)
print('*' * 30)
print('\n')
# 方向键改变事件
elif event_.type == pygame.JOYHATMOTION:
# hats = joystick.get_numhats()
Expand Down
Binary file added demo/myArm_demo/.xiaqi.py.swp
View file
Binary file not shown.
82 changes: 82 additions & 0 deletions demo/myArm_demo/all_run_test.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
#!/usr/bin/python
# -*- coding:utf-8 -*-
# @File : all_run_test.py
# @Author : Wang Weijian
# @Time : 2023/07/11 15:38:45
# @function: the script is used to do something
# @version : V1

import time
from pymycobot.myarm import MyArm

mc = MyArm('/dev/ttyAMA0')

init_angles = [
[90, 0, 0, 0, 0, 0, 0], # zero point
[-90, 0, 0, -90, 0, -90, 0], # first init point
[90, 0, 0, -90, 0, -90, 0], # second init point
[0, 80, 0, 0, 0, 0, 0], # four
[-150, 80, 0, 0, 0, 0, 0],
[150, 80, 0, 0, 0, 0, 0], # 6
[155.12, -79.27, -0.26, 76.46, 5.71, 45.0, -0.08], # 7
[-29.53, -48.11, 165, -89.2, 0.41, -43.56, 0.35], # 8
[87.62, 80, 165, -82.96, 165, -73.38, 0.26], # 9
[-114.16, 80, 165, -82.96, 165, -104.58, 0.26], # 10
[17.84, -2.1, 165, -100, 165, 64.16, -6.24], # 11
[-11.07, -11.68, 2.1, -93.42, 3.07, -9.14, -45.35], # 12
[-19.68, -16.96, -165, -84.28, -161.19, 1.05, -43.76] , # 13
[-108.89, -78.39, -165, -89.64, 165, 110, -0.7], # 14
[160.57, -78.92, 165, -84.63, -163.3, 1.05, -43.76], # 15
]

low_speed = 10
medium_speed = 50
high_speed = 100
timet = int(3)


def main():
"""
体现myarm三种不同速度的多种运动姿态
:return: None
"""
mc.send_angles(init_angles[0], medium_speed)
time.sleep(3+timet)

mc.send_angles(init_angles[1], low_speed)
time.sleep(10+timet)

mc.send_angles(init_angles[2], high_speed)
time.sleep(2+timet)

mc.send_angles(init_angles[3], high_speed)
time.sleep(4+timet)
mc.send_angles(init_angles[4], high_speed)
time.sleep(4.5+timet)
mc.send_angles(init_angles[5], high_speed)
time.sleep(6+timet)
mc.send_angles(init_angles[6], medium_speed)
time.sleep(4+timet)
mc.send_angles(init_angles[7], medium_speed)
time.sleep(8+timet)
mc.send_angles(init_angles[8], medium_speed)
time.sleep(4+timet)
mc.send_angles(init_angles[9], high_speed)
time.sleep(4+timet)
mc.send_angles(init_angles[10], high_speed)
time.sleep(4+timet)
mc.send_angles(init_angles[11], high_speed)
time.sleep(4+timet)
mc.send_angles(init_angles[12], high_speed)
time.sleep(4+timet)
mc.send_angles(init_angles[13], medium_speed)
time.sleep(8+timet)
mc.send_angles(init_angles[14], high_speed)
time.sleep(5+timet)
mc.send_angles(init_angles[0], medium_speed)
time.sleep(7.5+timet)



if __name__ == '__main__':
main()
76 changes: 76 additions & 0 deletions demo/myArm_demo/gripper_aikit_v2.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
import time
from pymycobot.myarm import MyArm


mc = MyArm('/dev/ttyAMA0')


init_angles = [
[-60, 0, 0, -90, 0, -90, 0], # first init point
[0, 0, 0, -90, 0, -90, 0], # second init point
]

box_angles = [
[-47.9, 17.31, 0.17, -89.91, -0.17, -56.07, 0.0], # D
[-27.59, 44.82, -1.75, -48.95, 0.0, -55.89, -0.0], # C
[52.99, 18.36, -1.4, -86.57, -0.17, -55.89, -0.0], # A
[87.97, 18.28, -1.4, -86.57, -0.35, -71.19, -0.0], # B
]


# 开启
def gripper_on():
mc.set_gripper_state(0, 100)
time.sleep(0.5)


# 关闭
def gripper_off():
mc.set_gripper_state(1, 100)
time.sleep(0.5)


def move():
"""
myarm使用夹爪模拟aikitV2套装抓取木块
"""
mc.send_angles(init_angles[0], 50)
time.sleep(3)

mc.send_angles(init_angles[1], 50)
time.sleep(3)
gripper_on()
mc.send_angles([0.0, 26.27, 0.17, -72.86, -0.17, -77.51, 0.0], 50)
time.sleep(3)

# mc.send_angles([0.0, -47.63, 0.17, -77.43, 0.08, -55.72, 0.0], 50)
mc.send_angles([-2.02, 41.74, 0.43, -86.13, -0.17, -46.05, 0.0], 50)


time.sleep(3)

gripper_off()
time.sleep(2)
tmp = []
while True:
if not tmp:
tmp = mc.get_angles()
else:
break
time.sleep(0.5)
tmp[6] = 0.0
print(tmp)
mc.send_angles([tmp[0], 0, 0, -90, -0.0, -90, tmp[6]], 50)
# mc.send_angles([0, 0, 0, -90, 0, -90, 0], 50)
time.sleep(3)
mc.send_angles(box_angles[3], 50)
time.sleep(4)
gripper_on()
time.sleep(2)
mc.send_angles(init_angles[0], 50)
time.sleep(4)


if __name__ == '__main__':
gripper_off()
move()
Loading

0 comments on commit 8ced525

Please sign in to comment.