Porter

1. Introduction

The purpose of this experiment is to stack four blocks of different colors from bottom to top in the order of blue, green, red, and yellow, and place them on the gray block in the middle. Then run the code, and the DOFBOT will execute in order: grab the fourth layer block and place it in the yellow area, grab the third layer block and place it in the red area, grab the second layer block and place it in the green area, and grab the bottom layer block and place it in the blue area. The way to arrange the blocks is shown in the figure below:

Note: Due to inevitable mechanical errors of DOFBOT servos, the block landing positions can be adjusted according to actual results.

After running the code, the DOFBOT will move the blocks to their corresponding positions. The final effect is shown in the figure below:

2. Code Content

Code path: /home/yahboom/Dofbot/3.ctrl_Arm/10.move_block.ipynb

xxxxxxxxxx
#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device
# Create DOFBOT object
Arm = Arm_Device()
time.sleep(.1)

​x
# Define function to clamp blocks, enable=1: clamp, =0: release
def arm_clamp_block(enable):
    if enable == 0:
        Arm.Arm_serial_servo_write(6, 60, 400)
    else:
        Arm.Arm_serial_servo_write(6, 130, 400)
        time.sleep(.5)
​
# Define function to move DOFBOT, simultaneously control servos 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)
​
# DOFBOT moves up
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)

xxxxxxxxxx
# Define position parameters for different locations. These can be adjusted according to actual needs.
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]

xxxxxxxxxx
# Move DOFBOT to a ready position for grabbing
arm_clamp_block(0)
arm_move(p_mould, 1000)
time.sleep(1)
​
# Move the fourth layer block to yellow area
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)

xxxxxxxxxx
# Move the third layer block to red area
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)

xxxxxxxxxx
# Move the second layer block to green area
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)

xxxxxxxxxx
# Move the first layer block to blue area
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)

del Arm # Release Arm object