ロボットアームをつくったよ

EEZYbotARM MK3 + ESP32でロボットアームを動かすまでのメモです。

3Dプリンタが必要です。また、動かし方がメインです。

デモ

以下が完成品となります。

https://twitter.com/tw_kotatu/status/1388722344055558145

システム図

HTTPで制御していますが、githubには、MQTT版も配置してあります

機材一覧

部品名	個数	備考
EEZYbotArm MK3パーツ ※1	-	MakerBot logoよりダウンロードし、印刷
ボールベアリング ※2	1	606RS
M2ネジ ※2	1	-
M3ネジ + ナット ※2	16	-
シャフト ※3	1	-
ESP32 DevKitC	1	ステッピングモータ/サーボモータ制御
ステッピングモータ+ 28BYJ-48 ULN2003ドライバーボード セット	3	ステッピングモータ (5V駆動のモノ)
サーボモータ	1	SG90, Miuzei サーボモーター等
USBケーブル 0.4m TK-USB1	1	モータ電源供給用
USBアダプタ	1	2.5A以上供給できるもの
ブレッドボード	2	ESP32がのれば1つでも可
ジャンパ線	適量	-

• ※1 : 部品によって2つ印刷が必要
  • 公式のMATERIALS参照
• ※2 : ネジは後述する動画の7:00あたりを確認のこと
• ※3 : ホームセンターで銅パイプ3mmを購入し、適度な長さに切りました

機材の印刷／組み立て

印刷と組み立てについては、víctor Romeroさんが公開されているYoutube

https://www.youtube.com/watch?v=XgjEnG4SVNU

が非常にわかりやすいです。

公式サイトで組み立て方は工事中(UNDER CONSTRUCTION)となっています（2021.05.04時点）

組み立て時の注意事項

組み立て時に気づいたことは以下です

• 組み立て動画と比べると私の3Dプリンタの印刷精度が悪い
  • 大きめに印刷されていたため、紙やすりなどで削りました
  • 穴が小さくネジどまりが悪かったため、はんだごてなのでネジ穴を拡張しました
• 3Mのネジは長めのモノを購入すべきだった
  • 手持ちの25mm, 20mmを使ったため、ネジをきつく締めるとパーツに負荷がかかりモータが動かせない
  • 長めであれば、袋ナットを使いパーツに対する負荷を減らせたような気がします
• 各モータの動作範囲を確認しながらやるべきだった
  • 最初にある程度組み立てを行ってからでは、調整ができない部分がありました
  • 結果、分解⇔組み立ての手戻りが多くなってしまいました

自身の反省ですが、参考になれば幸いです。

接続

接続図

ピン配置

デバイス	ESP接続先
SERVOモータ - 信号線	16
ステッピングモータ-1 - IN1	25
ステッピングモータ-1 - IN2	26
ステッピングモータ-1 - IN3	27
ステッピングモータ-1 - IN4	13
ステッピングモータ-2 - IN1	17
ステッピングモータ-2 - IN2	5
ステッピングモータ-2 - IN3	18
ステッピングモータ-2 - IN4	19
ステッピングモータ-3 - IN1	15
ステッピングモータ-3 - IN2	21
ステッピングモータ-3 - IN3	22
ステッピングモータ-3 - IN4	23

準備

ESP32のMicroPythonセットアップ

ESP32側のコードは、MicroPythonで実装します。
ESP32用のMicroPythonファームは、MicroPython - Firmware for Generic ESP32 moduleに配置されています。

今回は、安定版の"esp32-20210418-v1.15.bin"を使用しました。

詳細なセットアップ手順に関しては、下記が参考になります。

• MicroPython - ESP32 での MicroPython の始め方
• 備忘録 - ESP32-VSCode-microPythonでの開発環境の構築

VSCodeでの環境づくり

VSCodeの拡張機能 - Pymakrを使用します。
セットアップ手順については、下記が参考になります。

• 備忘録 - ESP32-VSCode-microPythonでの開発環境の構築

コーディング

下記に示すコードは、

https://github.com/kotaproj/roboticArm

のsrc_httpd, src_httpcとなります。

HTTP Server - ESP32側

概要

ESP32側は、3つのファイルで構成されます。

• main.py
  • HTTPサーバースレッド
  • SSID, PASSは自身の環境に合わせる必要があります
• stepper.py
  • ステッピングモータ管理クラス
  • 動作の詳細は、ESP32 - MicroPythonを使い、ステッピングモータを制御する
• servo.py
  • サーボモータ管理クラス

コード

main.py

import sys
import machine
import socket
import time

from stepper import Stepper
from servo import Servo

# servo
SERVO_NO1_PIN = (16)

# smotor
MOTOR_STEPS = (2048)

SMOTOR_NO1_PIN1 = (25)
SMOTOR_NO1_PIN2 = (26)
SMOTOR_NO1_PIN3 = (27)
SMOTOR_NO1_PIN4 = (13)

SMOTOR_NO2_PIN1 = (17)
SMOTOR_NO2_PIN2 = (5)
SMOTOR_NO2_PIN3 = (18)
SMOTOR_NO2_PIN4 = (19)

SMOTOR_NO3_PIN1 = (15)
SMOTOR_NO3_PIN2 = (21)
SMOTOR_NO3_PIN3 = (22)
SMOTOR_NO3_PIN4 = (23)

# Set your Wifi SSID and password
SSID = "XXXXXXXXXXXX"
PASS = "XXXXXXXXXXXX"

class HttpdProc():
    
    def __init__(self):
        self._servo = {}
        self._servo["no1"] = Servo(SERVO_NO1_PIN)
        self._smotor = {}
        self._smotor["no1"] = Stepper(MOTOR_STEPS, SMOTOR_NO1_PIN1, SMOTOR_NO1_PIN2, SMOTOR_NO1_PIN3, SMOTOR_NO1_PIN4)
        self._smotor["no2"] = Stepper(MOTOR_STEPS, SMOTOR_NO2_PIN1, SMOTOR_NO2_PIN2, SMOTOR_NO2_PIN3, SMOTOR_NO2_PIN4)
        self._smotor["no3"] = Stepper(MOTOR_STEPS, SMOTOR_NO3_PIN1, SMOTOR_NO3_PIN2, SMOTOR_NO3_PIN3, SMOTOR_NO3_PIN4)
        return

    def _do_connect(self):
        import network
        wlan = network.WLAN(network.STA_IF)
        wlan.active(True)
        if not wlan.isconnected():
            print('connecting to network...')
            # wlan.connect('essid', 'password')
            wlan.connect(SSID, PASS)
            while not wlan.isconnected():
                pass
                # time.sleep(1)
                time.sleep(3)
                print(".")
        print('network config:', wlan.ifconfig())
        time.sleep(3)
        print('connect - done!!!')
        return


    def run(self):
        self._do_connect()

        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.bind(('', 80))
        s.listen(5)

        while True:
            # If an exception occurs, reboot.
            try:
                conn, addr = s.accept()
                print("Got a connection from %s" % str(addr))
                request = conn.recv(1024* 2)
                request = str(request)
                print(request)

                # shutdown - For farm writing
                if "exit_htttpd" in request:
                    sys.exit()

                if len(request) == 0:
                    print("nodata!!!")
                    conn.close()
                    continue

                # parse
                # ex. GET http://ipadr:80?name=smotor&no=1&val=-2048&end=dummy
                #       => name = "smotor", no = "1", val = "-2048"
                name = request[request.find("name=") + len("name="): request.find("&no=")]
                no = request[request.find("&no=") + len("&no="): request.find("&val=")]
                val = request[request.find("&val=") + len("&val="):request.find("&end=")]

                # http response
                response = "response_dummy"
                conn.send("HTTP/1.1 200 OK")
                conn.send("Content-Type: text/html; encoding=utf8\nContent-Length: ")
                conn.send(str(len(response)))
                conn.send("\nConnection: close\n")
                conn.send("\n")
                conn.send(response)
                conn.close()

                if "servo" == name:
                    self._servo["no" + no].set_duty(int(val))
                if "smotor" == name:
                    self._smotor["no" + no].step(int(val))
            except:
                print('Error -> reboot')
                machine.deepsleep(3*1000)


def main():
    httpd_th = HttpdProc()
    httpd_th.run()
    return

# main()

stepper.py

import time
from machine import Pin


class Stepper():
    def __init__(self,  number_of_steps,
                 motor_pin_1, motor_pin_2, motor_pin_3, motor_pin_4):
        self.step_number = 0                   # which step the motor is on
        self.direction = 0                     # motor direction
        self.last_step_time = 0                # time stamp in us of the last step taken
        self.number_of_steps = number_of_steps  # total number of steps for this motor

        # setup the pins on the microcontroller:
        self.motor_pin_1 = Pin(motor_pin_1, Pin.OUT)
        self.motor_pin_2 = Pin(motor_pin_3, Pin.OUT)
        self.motor_pin_3 = Pin(motor_pin_2, Pin.OUT)
        self.motor_pin_4 = Pin(motor_pin_4, Pin.OUT)

        # pin_count is used by the stepMotor() method:
        self.pin_count = 4

        self.set_speed()
        return

    def set_speed(self, what_speed=10):
        ''' Sets the speed in revs per minute
        '''
        self.step_delay = 60 * 1000 * 1000 // self.number_of_steps // what_speed
        return

    def step(self, steps_to_move, auto_stop=True):
        ''' Moves the motor steps_to_move steps.  If the number is negative,
            the motor moves in the reverse direction.
        '''
        steps_left = abs(steps_to_move)  # how many steps to take

        # determine direction based on whether steps_to_mode is + or -:
        self.direction = 1 if steps_to_move > 0 else 0

        # decrement the number of steps, moving one step each time:
        while steps_left > 0:
            now = time.ticks_us()
            # move only if the appropriate delay has passed:
            if time.ticks_diff(now, self.last_step_time) >= self.step_delay:
                # get the timeStamp of when you stepped:
                self.last_step_time = now
                # increment or decrement the step number,
                # depending on direction:
                if self.direction == 1:
                    self.step_number += 1
                    if self.step_number == self.number_of_steps:
                        self.step_number = 0
                else:
                    if self.step_number == 0:
                        self.step_number = self.number_of_steps
                    self.step_number -= 1

                # decrement the steps left:
                steps_left -= 1
                # step the motor to step number 0, 1, 2, 3
                self._step_motor(self.step_number % 4)

        if auto_stop:
            self.stop()
        return

    def _step_motor(self, this_step):
        ''' Moves the motor forward or backwards.
              if (this->pin_count == 4) {
        '''
        # 1010
        if this_step == 0:
            self.motor_pin_1.value(True)
            self.motor_pin_2.value(False)
            self.motor_pin_3.value(True)
            self.motor_pin_4.value(False)
        # 0110
        elif this_step == 1:
            self.motor_pin_1.value(False)
            self.motor_pin_2.value(True)
            self.motor_pin_3.value(True)
            self.motor_pin_4.value(False)
        # 0101
        elif this_step == 2:
            self.motor_pin_1.value(False)
            self.motor_pin_2.value(True)
            self.motor_pin_3.value(False)
            self.motor_pin_4.value(True)
        # 1001
        elif this_step == 3:
            self.motor_pin_1.value(True)
            self.motor_pin_2.value(False)
            self.motor_pin_3.value(False)
            self.motor_pin_4.value(True)
        return

    def stop(self):
        self.motor_pin_1.value(False)
        self.motor_pin_2.value(False)
        self.motor_pin_3.value(False)
        self.motor_pin_4.value(False)
        return

servo.py

import time
from machine import Pin, PWM

class Servo():
    def __init__(self, pin_no, duty=70, freq=50):
        self._servo = PWM(Pin(pin_no), freq=50, duty=duty)
        time.sleep(0.1)
        self._servo.duty(duty)
        return
    def set_duty(self, duty):
        self._servo.duty(duty)
        return

実行の仕方

> from main import main
> main()
# WiFiアクセスポイントへ接続後、HTTPサーバ動作

HTTP Client - PC側

概要

PC側は、1つのファイルで構成されます。

• httpc.py
  • HTTPクライアント動作
  • requestsにて、ESP32に対しGETメソッドでアクセスします
  • ex.http://ipadr:80?name=servo&no=1&val=70&end=dummy
  • 環境に合わせて、ESP32_URLの変更が必要です
  • DEMO映像と同じ動作となります(各ステッピングモータ動作後、サーボモータを動作)

コード

httpc.py

import time
import requests
from collections import OrderedDict

# Set the IP address of ESP32
ESP32_URL = "http://XXX.XXX.XXX.XXX:80"

def run_servo(val=70):
    """GET method
    - ex. http://ipadr:80?name=servo&no=1&val=70&end=dummy
    Args:
        val ([int]): [55-80]
    """
    payload = OrderedDict()
    payload['name'] = 'servo'
    payload['no'] = str(1)
    payload['val'] = str(val)
    payload['end'] = "dummy"
    r = requests.get(ESP32_URL, params=payload)
    print(r)
    return


def run_smmotor(no=1, val=0):
    """GET method
    - ex. http://ipadr:80?name=smotor&no=1&val=-2048&end=dummy
    Args:
        no ([int]): [1-3]
        val ([int]): [-2048 - +2048]
    """
    payload = OrderedDict()
    payload['name'] = 'smotor'
    payload['no'] = str(no)
    payload['val'] = str(val)
    payload['end'] = "dummy"
    r = requests.get(ESP32_URL, params=payload)
    print(r)
    return


def main():
    run_smmotor(1, -512)
    time.sleep(0.5)
    run_smmotor(1, 512)
    time.sleep(0.5)
    run_smmotor(2, 512)
    time.sleep(0.5)
    run_smmotor(2, -512)
    time.sleep(0.5)
    run_smmotor(3, 512)
    time.sleep(0.5)
    run_smmotor(3, -512)
    time.sleep(0.5)
    run_servo(80)
    time.sleep(0.5)
    run_servo(55)
    time.sleep(0.5)


if __name__ == "__main__":
    main()

実行の仕方

python httpc.py

DEMOの動作になればOKです🎊。
サーボの角度（Dutyの指定範囲）やステッピングモータの回す範囲は、
それぞれの環境で変わると思いますので、試行錯誤しながら合わせこむことが必要です。