前回から一歩進んで前進する動作までできました。
3Dプリンタの材質がPLAというのもあってか摩擦がないところだと滑ってしまいますが、そうでない場所であれば（動画ははんだ付け用パッド）スムーズに前進してくれます。
動作を考える上で各サーボの動きを角度に置き換えないといけないのですが、結構頭の体操になりますね。強化学習とか使うとうまいことできるようになるんだろうな。人間が考える動きよりも効率的な動かし方を学習してくれるんだろうか。
プログラム
from machine import Pin, PWM
import time
SV_FREQ = 50.0 #Hz
MAX_DUTY = 65535.0 #uint16 max
MIN_SV_PULSE = 0.5 #ms
MAX_SV_PULSE = 2.5 #ms
def get_pulse_width(angle):
pulse_ms = MIN_SV_PULSE + (MAX_SV_PULSE - MIN_SV_PULSE)*angle/180.0
x = (int)(MAX_DUTY * (pulse_ms*SV_FREQ/1000.0))
return x
correction = [0, 0, 10, 0, 0, 0, -10, -5]
temp_angle = [90, 90, 90, 90, 90, 90, 90, 90]
servo = []
angle_i = [ 90, 90, 90, 90, 90, 90, 90, 90]# Initial position
angle = [ [ 60, 90, 90, 90, 90, 90, 60, 90],
[ 60, 60, 90, 60, 90, 120, 60, 120],
[ 90, 60, 120, 60, 120, 120, 90, 120],
[ 90, 90, 120, 90, 120, 90, 90, 90],
[ 90, 120, 120, 120, 120, 60, 90, 60],
[ 60, 120, 90, 120, 90, 60, 60, 60] ]
servo.append(PWM(Pin(2))) #Front Right Leg
servo.append(PWM(Pin(3))) #Front Right Shoulder
servo.append(PWM(Pin(4))) #Front Left Leg
servo.append(PWM(Pin(5))) #Front Left Shoulder
servo.append(PWM(Pin(6))) #Rear Right Leg
servo.append(PWM(Pin(7))) #Rear Right Shoulder
servo.append(PWM(Pin(8))) #Rear Left Leg
servo.append(PWM(Pin(9))) #Rear Left Shoulder
snum = len(servo)
divide = 5 # Number of divisions between frames
div_counter = 0 # Counting divisions
key_frame = 0 # Current keyframe
next_key_frame = 1 # Next frame
# Move to initial position
for i in range(snum):
servo[i].freq(int(SV_FREQ))
servo[i].duty_u16(get_pulse_width(angle_i[i] + correction[i]))
time.sleep(1.0)
while True:
#Update keyframe
div_counter += 1
if div_counter >= divide:
div_counter = 0
key_frame = next_key_frame
next_key_frame += 1
if next_key_frame > 5:
next_key_frame = 0 # Back to angle[0]
#Degree calculation
for i in range(snum):
temp_angle[i] = angle[key_frame][i] + (angle[next_key_frame][i] - angle[key_frame][i])*div_counter/divide
print(key_frame)
# Servo drive
for i in range(snum):
servo[i].duty_u16(get_pulse_width(int(temp_angle[i])+correction[i]))
time.sleep(0.05)次はターンを実装します。
ターンできれば障害物検知で歩き回るようにできるので、そこまでやって距離計で障害物検知を検討していきます。
コメント