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