Stack Block

1. Introduction

This experiment is the opposite of the previous lesson "Nature Porter". It grabs blocks of different colors from the surrounding areas in the order of yellow, red, green, and blue, and stacks them into the gray area in the middle.

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

The way to arrange the blocks is shown in the figure below:

After running the code, the DOFBOT will stack the blocks. The final effect is shown in the figure below:

2. Code Content

Code path: /home/yahboom/Dofbot/3.ctrl_Arm/11.heap_up.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))
        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)

xxxxxxxxxx
# Define position parameters for different locations.
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]

xxxxxxxxxx
# Move DOFBOT to a ready position for grabbing
arm_clamp_block(0)
arm_move(p_mould, 1000)
time.sleep(1)

xxxxxxxxxx
# Grab the block from yellow area and stack it to the bottom layer in the middle.
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)

xxxxxxxxxx
# Grab the block from red area and stack it to the second layer in the middle.
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)

xxxxxxxxxx
# Grab the block from green area and stack it to the third layer in the middle.
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)

xxxxxxxxxx
# Grab the block from blue area and stack it to the fourth layer in the middle.
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)

del Arm # Release Arm object