メカナムホイールラジコンを作るその2

前回の続き。

メカナムホイールラジコンを作るその1toys.poppo-ya.com

ラジコン本体。

筐体固定部

筐体は、LEGOの大きさと同じにして、いくつかのパーツをネジで組み立てた。

特に、モーターと本体の接合部は、細かく調整。

本体とのケーブル接続部が筐体の上部にでるようにした。

ホイールは向きがあるので、対抗同士が同じ向きになるようにする。

motor2040配線

M5StickCへの配線は、TX/RX とGND, 5V out

電源は、USB-Cのバッテリーから取る。

program

Serial からの文字列をreadして、モータ制御を行う。

motor2040 control · Gistpython

# SPDX-License-Identifier: MIT

import time
import board
import pwmio
import digitalio
import busio
from adafruit_motor import motor

# Motor constants
FREQUENCY = 25000               # Chose a frequency above human hearing
DECAY_MODE = motor.SLOW_DECAY   # The decay mode affects how the motor
                                # responds, with SLOW_DECAY having improved spin
                                # threshold and speed-to-throttle linearity

# Serial I/F
uart = busio.UART(board.TX, board.RX, baudrate=115200, timeout=5)

# Create a digitalinout object for the user switch
user_sw = digitalio.DigitalInOut(board.USER_SW)
user_sw.direction = digitalio.Direction.INPUT
user_sw.pull = digitalio.Pull.UP

# Create the pwm objects
pwm_a_p = pwmio.PWMOut(board.MOTOR_A_P, frequency=FREQUENCY)
pwm_a_n = pwmio.PWMOut(board.MOTOR_A_N, frequency=FREQUENCY)
pwm_b_p = pwmio.PWMOut(board.MOTOR_B_P, frequency=FREQUENCY)
pwm_b_n = pwmio.PWMOut(board.MOTOR_B_N, frequency=FREQUENCY)
pwm_c_p = pwmio.PWMOut(board.MOTOR_C_P, frequency=FREQUENCY)
pwm_c_n = pwmio.PWMOut(board.MOTOR_C_N, frequency=FREQUENCY)
pwm_d_p = pwmio.PWMOut(board.MOTOR_D_P, frequency=FREQUENCY)
pwm_d_n = pwmio.PWMOut(board.MOTOR_D_N, frequency=FREQUENCY)

# Create the motor objects
mot_a = motor.DCMotor(pwm_a_p, pwm_a_n)
mot_b = motor.DCMotor(pwm_b_p, pwm_b_n)
mot_c = motor.DCMotor(pwm_c_p, pwm_c_n)
mot_d = motor.DCMotor(pwm_d_p, pwm_d_n)

# Set the motor decay modes (if unset the default will be FAST_DECAY)
mot_a.decay_mode = DECAY_MODE
mot_b.decay_mode = DECAY_MODE
mot_c.decay_mode = DECAY_MODE
mot_d.decay_mode = DECAY_MODE

def button_pressed():
    return not user_sw.value

def stop():
    mot_a.throttle = 0
    mot_c.throttle = 0
    mot_b.throttle = 0
    mot_d.throttle = 0

def left(speed):
    mot_a.throttle = -1 * speed
    mot_b.throttle = -1 * speed
    mot_c.throttle = speed
    mot_d.throttle = speed

def right(speed):
    mot_a.throttle = speed
    mot_b.throttle = speed
    mot_c.throttle = -1 * speed
    mot_d.throttle = -1 * speed

def back(speed):
    mot_a.throttle = -1 * speed
    mot_b.throttle = speed
    mot_c.throttle = -1 * speed
    mot_d.throttle = speed

def forward(speed):
    mot_a.throttle = speed
    mot_b.throttle = -1 * speed
    mot_c.throttle = speed
    mot_d.throttle = -1 * speed

def freely(vector):
    mot_a.throttle = None
    mot_b.throttle = None
    mot_c.throttle = None
    mot_d.throttle = None

def spin(speed):
    mot_a.throttle = speed
    mot_b.throttle = speed
    mot_c.throttle = speed
    mot_d.throttle = speed

# Run the motor sequence
print("ready for UART")
while True:

    data = uart.readline()
    if data is not None:
        data_string = ''.join([chr(b) for b in data])
        print(data_string, end="")
        print("")

        if len(data_string) < 3:
            continue
        c = data_string[2]
        if 'l' in c:
            left(0.3)
        elif 'L' in c:
            left(0.4)

        elif 'f' in c:
            forward(0.3)
        elif 'F' in c:
            forward(0.4)

        elif 'b' in c:
            back(0.3)
        elif 'B' in c:
            back(0.4)

        elif 'r' in c:
            right(0.3)
        elif 'R' in c:
            right(0.4)

        elif 'x' in c:
            spin(0.3)
        elif 'X' in c:
            spin(0.4)

        elif 's' in c:
            stop()
    else:
        print("timeout")

    if button_pressed():  # Exit if button is pressed
        break
    time.sleep(0.001)