DOFBOT Grab Blocks

1. Introduction

This experiment aims to move building blocks from the central cross area to different colored block areas around the perimeter. First, place the yellow block in the cross area, then run the code units sequentially up to the sixth unit (grab a block from the cross position and place it at the yellow block position). The robotic arm will automatically grab the block placed in the cross area and move it to the yellow area, then return to the ready position. Before running the seventh code unit, place the red block in the cross area, then run the seventh unit (grab a block from the cross position and place it at the red block position). The red block will then be moved to the red area. The same operation applies to other colored blocks.

2. Code Content

Code path:

~/dofbot_pro/dofbot_ctrl/scripts/09.clamp_block.ipynb

The following code content needs to be executed step by step, not all at once. Before grabbing blocks, place the blocks in the central cross position, and only place one block at a time.

​x
#!/usr/bin/env python3
#coding=utf-8
import time
from Arm_Lib import Arm_Device
​
# Create robotic arm object
Arm = Arm_Device()
time.sleep(.1)
​
​
from dofbot_utils.robot_controller import Robot_Controller
robot = Robot_Controller()

xxxxxxxxxx
# Define block grabbing function, enable=1: grab, =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, 135, 400)
    time.sleep(.5)
​
    
# Define robotic arm movement function, simultaneously controls servos 1-5 movement, 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)
​
# Robotic arm moves upward
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 variable parameters for different positions
p_mould = robot.P_LOOK_AT
p_top = robot.P_TOP
p_Brown = robot.P_CENTER
​
p_Yellow = robot.P_YELLOW
p_Red = robot.P_RED
​
p_Green = robot.P_GREEN
p_Blue = robot.P_BLUE

xxxxxxxxxx
# Move the robotic arm to a ready-to-grab position
arm_clamp_block(0)
arm_move(p_mould, 1000)
time.sleep(1)

xxxxxxxxxx
# Grab a block from the center recognition area and place it at the 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 the center recognition area and place it at the 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 the center recognition area and place it at the 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 the center recognition area and place it at the 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)

xxxxxxxxxx
del Arm # Release the Arm object