DOFBOT Grab Block

1. Introduction

The purpose of this experiment is to move blocks from the gray area in the middle to blocks of different colors in the surrounding areas. First, place the yellow block in the gray area, then run the code cells in order up to the sixth cell (grab a block from the gray block position and place it at the yellow block position). At this time, the DOFBOT will automatically grab the block placed in the gray area and put it into the yellow area, then return to the ready position. Before running the seventh code cell, place the red block in the gray area, then run the seventh cell (grab a block from the gray block position and place it at the red block position). This way, the red block will also be grabbed to the red area. The operation method for other blocks is the same.

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

2. Code Content

Code path: /home/yahboom/Dofbot/3.ctrl_Arm/9.clamp_block.ipynb

The following code needs to be executed step by step. Do not run all at once. Before grabbing a block, place the block in the middle gray block position, and only place one block at a time.

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 modified as needed.
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]

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

xxxxxxxxxx
# Grab a block from gray block position and place it at yellow block position.
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)

xxxxxxxxxx
# Grab a block from gray block position and place it at red block position.
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)

xxxxxxxxxx
# Grab a block from gray block position and place it at green block position.
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)

xxxxxxxxxx
# Grab a block from gray block position and place it at blue block position.
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 # Release Arm object