RoboDogをラズパイ、サーボモーターSG90、超音波距離センサーHC-SR04、サーボモーターコントローラーPCA9685を使用して作ってみた!

PCA9685サーボーで制御されたサーボモーター4つで歩き、超音波距離センサーで障害物を検出し、障害物があったらバックするロボを作ってみました。

足4本ですが関節はないため、歩かせるのも試行錯誤が必要でした。犬や馬が歩いたり走ったりするような動きをしようと思いましたが、なかなかうまく進みません。結局カエルのように飛び跳ねる感じにしました。

Pythonで作ったプログラムは以下のとおりです。実際に作られる方はどうぞご自由にご使用ください。

import time

import Adafruit_PCA9685

import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BCM)

GPIO.setwarnings(False)

GPIO_TRIGGER=21

GPIO_ECHO=20

GPIO.setup(GPIO_TRIGGER, GPIO.OUT)

GPIO.setup(GPIO_ECHO, GPIO.IN)

pwm = Adafruit_PCA9685.PCA9685()

pwm.set_pwm_freq(60)

# pulse = (650-150)/180*DEGREE+150

R_0=150

R_30=233

R_60=317

R_90=400

R_120=483

R_150=567

R_180=650

L_0=650

L_30=567

L_60=483

L_90=400

L_120=317

L_150=233

L_180=150

pwm.set_pwm(1, 0, L_90)

pwm.set_pwm(2, 0, L_90)

pwm.set_pwm(3, 0, R_90)

pwm.set_pwm(4, 0, R_90)

time.sleep(1)

time.sleep(1)

def distance():

    GPIO.output(GPIO_TRIGGER, True)

    time.sleep(0.00001)

    GPIO.output(GPIO_TRIGGER, False)

    StartTime=time.time()

    StopTime=time.time()

    while GPIO.input(GPIO_ECHO)==0:

        StartTime=time.time()

    while GPIO.input(GPIO_ECHO)==1:

        StopTime=time.time()

    TimeElapsed=StopTime-StartTime

    distance=TimeElapsed*17025

    return distance

while True:

    try:

        dist = distance()

        print(dist)

        if dist<20:

            print(“near”)

            pwm.set_pwm(1, 0, L_0)

            pwm.set_pwm(2, 0, L_120)

            pwm.set_pwm(3, 0, R_0)

            pwm.set_pwm(4, 0, R_120)

            time.sleep(0.3)

            pwm.set_pwm(1, 0, L_180)

            pwm.set_pwm(2, 0, L_60)

            pwm.set_pwm(3, 0, R_180)

            pwm.set_pwm(4, 0, R_60)

            time.sleep(0.3)

        else:

            print(“far”)

            pwm.set_pwm(1, 0, L_120)

            pwm.set_pwm(2, 0, L_0)

            pwm.set_pwm(3, 0, R_120)

            pwm.set_pwm(4, 0, R_0)

            time.sleep(0.3)

            pwm.set_pwm(1, 0, L_60)

            pwm.set_pwm(2, 0, L_180)

            pwm.set_pwm(3, 0, R_60)

            pwm.set_pwm(4, 0, R_180)

            time.sleep(0.3)

    except KeyboardInterrupt:

            break

返信を残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です