【Dofbot Jetson nano】学习过程分享3

1、控制RGB灯。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 获取机械臂的对象
Arm = Arm_Device()
time.sleep(.1)


def main():
    
    while True:
        Arm.Arm_RGB_set(50, 0, 0) #RGB亮红灯
        time.sleep(.5)
        Arm.Arm_RGB_set(0, 50, 0) #RGB亮绿灯
        time.sleep(.5)
        Arm.Arm_RGB_set(0, 0, 50) #RGB亮蓝灯
        time.sleep(.5)

        print(" END OF LINE! ")

try :
    main()
except KeyboardInterrupt:
    # 释放Arm对象
    del Arm
    print(" Program closed! ")
    pass

2、控制蜂鸣器。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 获取机械臂的对象
Arm = Arm_Device()
time.sleep(.1)

# 蜂鸣器自动响100毫秒后关闭
b_time = 1
Arm.Arm_Buzzer_On(b_time)
time.sleep(1)

# 蜂鸣器自动响300毫秒后关闭
b_time = 3
Arm.Arm_Buzzer_On(b_time)
time.sleep(1)

# 蜂鸣器一直响
Arm.Arm_Buzzer_On()
time.sleep(1)

# 关闭蜂鸣器
Arm.Arm_Buzzer_Off()
time.sleep(1)

del Arm  # 释放掉Arm对象

3、控制单个舵机。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 单独控制一个舵机运动到某个角度
id = 6

Arm.Arm_serial_servo_write(id, 90, 500)
time.sleep(1)

# 控制一个舵机循环切换角度
id = 6

def main():
    while True:
        Arm.Arm_serial_servo_write(id, 120, 500)
        time.sleep(1)
        Arm.Arm_serial_servo_write(id, 50, 500)
        time.sleep(1)
        Arm.Arm_serial_servo_write(id, 120, 500)
        time.sleep(1)
        Arm.Arm_serial_servo_write(id, 180, 500)
        time.sleep(1)

    
try :
    main()
except KeyboardInterrupt:
    print(" Program closed! ")
    pass

del Arm  # 释放掉Arm对象

4、读取舵机当前位置。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 读取所有舵机的角度，并循环打印出来
def main():

    while True:
        for i in range(6):
            aa = Arm.Arm_serial_servo_read(i+1)
            print(aa)
            time.sleep(.01)
        time.sleep(.5)
        print(" END OF LINE! ")

    
try :
    main()
except KeyboardInterrupt:
    print(" Program closed! ")
    pass

# 单独控制一个舵机运动后，再读取它的角度
id = 6
angle = 150

Arm.Arm_serial_servo_write(id, angle, 500)
time.sleep(1)

aa = Arm.Arm_serial_servo_read(id)
print(aa)

time.sleep(.5)

del Arm  # 释放掉Arm对象

5、一次控制6个舵机。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 同时控制六个舵机运动，逐渐变换角度。
def ctrl_all_servo(angle, s_time = 500):
    Arm.Arm_serial_servo_write6(angle, 180-angle, angle, angle, angle, angle, s_time)
    time.sleep(s_time/1000)


def main():
    dir_state = 1
    angle = 90
    
    # 让舵机复位归中
    Arm.Arm_serial_servo_write6(90, 90, 90, 90, 90, 90, 500)
    time.sleep(1)

    
    while True:
        if dir_state == 1:
            angle += 1
            if angle >= 180:
                dir_state = 0
        else:
            angle -= 1
            if angle <= 0:
                dir_state = 1
        
        ctrl_all_servo(angle, 10)
        time.sleep(10/1000)
#         print(angle)

    
try :
    main()
except KeyboardInterrupt:
    print(" Program closed! ")
    pass

del Arm  # 释放掉Arm对象

6、机械臂上下摆动。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 循环控制机械臂上下左右摆动
def main():
    # 让舵机复位归中
    Arm.Arm_serial_servo_write6(90, 90, 90, 90, 90, 90, 500)
    time.sleep(1)


    while True:
        # 控制3号和4号舵机上下运行
        Arm.Arm_serial_servo_write(3, 0, 1000)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 180, 1000)
        time.sleep(1)
        
        # 控制1号舵机左右运动
        Arm.Arm_serial_servo_write(1, 180, 500)
        time.sleep(.5)
        Arm.Arm_serial_servo_write(1, 0, 1000)
        time.sleep(1)
        
        # 控制舵机恢复初始位置
        Arm.Arm_serial_servo_write6(90, 90, 90, 90, 90, 90, 1000)
        time.sleep(1.5)


try :
    main()
except KeyboardInterrupt:
    print(" Program closed! ")
    pass

del Arm  # 释放掉Arm对象

7、机械臂跳舞。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device


Arm = Arm_Device()
time.sleep(.1)

time_1 = 500
time_2 = 1000
time_sleep = 0.5

# 机械臂循环跳舞
def main():
    # 让舵机复位归中
    Arm.Arm_serial_servo_write6(90, 90, 90, 90, 90, 90, 500)
    time.sleep(1)
    
    while True:
        Arm.Arm_serial_servo_write(2, 180-120, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 120, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 60, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(2, 180-135, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 135, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 45, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(2, 180-120, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 120, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 60, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(2, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 90, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(2, 180-80, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 80, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 80, time_1)
        time.sleep(time_sleep)



        Arm.Arm_serial_servo_write(2, 180-60, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 60, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 60, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(2, 180-45, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 45, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 45, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(2, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(3, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 90, time_1)
        time.sleep(.001)
        time.sleep(time_sleep)



        Arm.Arm_serial_servo_write(4, 20, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(6, 150, time_1)
        time.sleep(.001)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(4, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(6, 90, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(4, 20, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(6, 150, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(4, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(6, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(1, 0, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(5, 0, time_1)
        time.sleep(time_sleep)



        Arm.Arm_serial_servo_write(3, 180, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 0, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(6, 180, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(6, 0, time_2)
        time.sleep(time_sleep)



        Arm.Arm_serial_servo_write(6, 90, time_2)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(1, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(5, 90, time_1)
        time.sleep(time_sleep)

        Arm.Arm_serial_servo_write(3, 90, time_1)
        time.sleep(.001)
        Arm.Arm_serial_servo_write(4, 90, time_1)
        time.sleep(time_sleep)

        print(" END OF LINE! ")

try :
    main()
except KeyboardInterrupt:
    print(" Program closed! ")
    pass

del Arm  # 释放掉Arm对象

8、机械臂记忆动作。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 打开学习模式，扩展板上的RGB灯呈现呼吸灯状态，同时机械臂所有舵机都进入关闭扭矩状态，
# 即可以自由掰动，可以把机械臂掰动到要记住的位置上。
Arm.Arm_Button_Mode(1)

# 在学习模式下，每次运行此cell，记录当前的动作保存到扩展板，同时扩展板上的RGB灯会切换颜色，
# 如果出现了红色呼吸灯，则表示学习的动作组已经满（20组）。
# 此命令也可以替换成按扩展板上的K1键，两者的功能是一致的。
Arm.Arm_Action_Study()

# 关闭学习模式。关闭呼吸灯
Arm.Arm_Button_Mode(0)

# 读取当前记录的动作组数量
num = Arm.Arm_Read_Action_Num()
print(num)

# 单次运行动作组
Arm.Arm_Action_Mode(1)

# 循环运行动作组
Arm.Arm_Action_Mode(2)

# 停止动作组
Arm.Arm_Action_Mode(0)

# 清空动作组，清空完成扩展板上的RGB灯会亮绿色。
# 注意：一旦清空已记录的动作组则无法恢复。
Arm.Arm_Clear_Action()

del Arm  # 释放掉Arm对象

9、机械臂夹方块。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 定义夹积木块函数，enable=1：夹住，=0：松开
def arm_clamp_block(enable):
    if enable == 0:
        Arm.Arm_serial_servo_write(6, 60, 400)
    else:
        Arm.Arm_serial_servo_write(6, 135, 400)
    time.sleep(.5)

    
# 定义移动机械臂函数,同时控制1-5号舵机运动，p=[S1,S2,S3,S4,S5]
def arm_move(p, s_time = 500):
    for i in range(5):
        id = i + 1
        if id == 5:
            time.sleep(.1)
            Arm.Arm_serial_servo_write(id, p[i], int(s_time*1.2))
        else :
            Arm.Arm_serial_servo_write(id, p[i], s_time)
        time.sleep(.01)
    time.sleep(s_time/1000)

# 机械臂向上移动
def arm_move_up():
    Arm.Arm_serial_servo_write(2, 90, 1500)
    Arm.Arm_serial_servo_write(3, 90, 1500)
    Arm.Arm_serial_servo_write(4, 90, 1500)
    time.sleep(.1)

# 定义不同位置的变量参数
p_mould = [90, 130, 0, 0, 90]
p_top = [90, 80, 50, 50, 270]
p_Brown = [90, 53, 33, 36, 270]

p_Yellow = [65, 22, 64, 56, 270]
p_Red = [117, 19, 66, 56, 270]

p_Green = [136, 66, 20, 29, 270]
p_Blue = [44, 66, 20, 28, 270]

# 让机械臂移动到一个准备抓取的位置
arm_clamp_block(0)
arm_move(p_mould, 1000)
time.sleep(1)

# 从灰色积木块位置抓取一块积木放到黄色积木块的位置上。
arm_move(p_top, 1000)
arm_move(p_Brown, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Yellow, 1000)
arm_clamp_block(0)


arm_move(p_mould, 1000)

time.sleep(1)

# 从灰色积木块位置抓取一块积木放到红色积木块的位置上。
arm_move(p_top, 1000)
arm_move(p_Brown, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Red, 1000)
arm_clamp_block(0)

arm_move_up()
arm_move(p_mould, 1100)

time.sleep(1)

# 从灰色积木块位置抓取一块积木放到绿色积木块的位置上。
arm_move(p_top, 1000)
arm_move(p_Brown, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Green, 1000)
arm_clamp_block(0)

arm_move_up()
arm_move(p_mould, 1100)

time.sleep(1)

# 从灰色积木块位置抓取一块积木放到蓝色积木块的位置上。
arm_move(p_top, 1000)
arm_move(p_Brown, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Blue, 1000)
arm_clamp_block(0)

arm_move_up()
arm_move(p_mould, 1100)

time.sleep(1)

del Arm  # 释放掉Arm对象

10、大自然搬运工。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 定义夹积木块函数，enable=1：夹住，=0：松开
def arm_clamp_block(enable):
    if enable == 0:
        Arm.Arm_serial_servo_write(6, 60, 400)
    else:
        Arm.Arm_serial_servo_write(6, 135, 400)
    time.sleep(.5)

    
# 定义移动机械臂函数,同时控制1-5号舵机运动，p=[S1,S2,S3,S4,S5]
def arm_move(p, s_time = 500):
    for i in range(5):
        id = i + 1
        if id == 5:
            time.sleep(.1)
            Arm.Arm_serial_servo_write(id, p[i], int(s_time*1.2))
        else :
            Arm.Arm_serial_servo_write(id, p[i], s_time)
        time.sleep(.01)
    time.sleep(s_time/1000)

# 机械臂向上移动
def arm_move_up():
    Arm.Arm_serial_servo_write(2, 90, 1500)
    Arm.Arm_serial_servo_write(3, 90, 1500)
    Arm.Arm_serial_servo_write(4, 90, 1500)
    time.sleep(.1)

# 定义不同位置的变量参数
p_mould = [90, 130, 0, 0, 90]
p_top = [90, 80, 50, 50, 270]

p_Yellow = [65, 22, 64, 56, 270]
p_Red = [117, 19, 66, 56, 270]

p_Green = [136, 66, 20, 29, 270]
p_Blue = [44, 66, 20, 28, 270]


p_layer_4 = [90, 72, 49, 13, 270]
p_layer_3 = [90, 66, 43, 20, 270]
p_layer_2 = [90, 63, 34, 30, 270]
p_layer_1 = [90, 53, 33, 36, 270]

# 让机械臂移动到一个准备抓取的位置
arm_clamp_block(0)
arm_move(p_mould, 1000)
time.sleep(1)

# 搬运第四层的积木块到黄色区域
arm_move(p_top, 1000)
arm_move(p_layer_4, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Yellow, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move_up()
arm_move(p_mould, 1100)
    
# time.sleep(1)

# 搬运第三层的积木块到红色区域
arm_move(p_top, 1000)
arm_move(p_layer_3, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Red, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move_up()
arm_move(p_mould, 1100)
    
# time.sleep(1)

# 搬运第二层的积木块到绿色区域
arm_move(p_top, 1000)
arm_move(p_layer_2, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Green, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move_up()
arm_move(p_mould, 1100)
    
# time.sleep(1)

# 搬运第一层的积木块到蓝色区域
arm_move(p_top, 1000)
arm_move(p_layer_1, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_Blue, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move_up()
arm_move(p_mould, 1100)
    
# time.sleep(1)

del Arm  # 释放掉Arm对象

11、叠罗汉。

#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(.1)

# 定义夹积木块函数，enable=1：夹住，=0：松开
def arm_clamp_block(enable):
    if enable == 0:
        Arm.Arm_serial_servo_write(6, 60, 400)
    else:
        Arm.Arm_serial_servo_write(6, 135, 400)
    time.sleep(.5)

    
# 定义移动机械臂函数,同时控制1-5号舵机运动，p=[S1,S2,S3,S4,S5]
def arm_move(p, s_time = 500):
    for i in range(5):
        id = i + 1
        if id == 5:
            time.sleep(.1)
            Arm.Arm_serial_servo_write(id, p[i], int(s_time*1.2))
        elif id == 1 :
            Arm.Arm_serial_servo_write(id, p[i], int(3*s_time/4))

        else:
            Arm.Arm_serial_servo_write(id, p[i], int(s_time))
        time.sleep(.01)
    time.sleep(s_time/1000)

# 定义不同位置的变量参数
p_mould = [90, 130, 0, 0, 90]
p_top = [90, 80, 50, 50, 270]


p_layer_4 = [90, 76, 40, 17, 270]
p_layer_3 = [90, 65, 44, 17, 270]
p_layer_2 = [90, 65, 25, 36, 270]
p_layer_1 = [90, 48, 35, 30, 270]

p_Yellow = [65, 22, 64, 56, 270]
p_Red = [118, 19, 66, 56, 270]

p_Green = [136, 66, 20, 29, 270]
p_Blue = [44, 66, 20, 28, 270]

# 让机械臂移动到一个准备抓取的位置
arm_clamp_block(0)
arm_move(p_mould, 1000)
time.sleep(1)

# 夹取黄色区域的方块堆叠到中间最底层的位置。
arm_move(p_top, 1000)
arm_move(p_Yellow, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_layer_1, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move(p_mould, 1100)
    
# time.sleep(1)

# 夹取红色区域的方块堆叠到中间第二层的位置。
arm_move(p_top, 1000)
arm_move(p_Red, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_layer_2, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move(p_mould, 1100)

# time.sleep(1)

# 夹取绿色区域的方块堆叠到中间第三层的位置。
arm_move(p_top, 1000)
arm_move(p_Green, 1000)
arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_layer_3, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move(p_mould, 1100)
    
# time.sleep(1)

# 夹取蓝色区域的方块堆叠到中间第四层的位置。
arm_move(p_top, 1000)
arm_move(p_Blue, 1000)

arm_clamp_block(1)

arm_move(p_top, 1000)
arm_move(p_layer_4, 1000)
arm_clamp_block(0)

time.sleep(.1)

arm_move(p_mould, 1100)
    
# time.sleep(1)