2019b/Member

課題3

A地点または(および)A'地点からスタートし、C,E,Fの交差点上にある缶に置かれているボールをそれぞれC', E', F' 地点にある空き缶に置くロボットの製作
2019b-mission2.png

ロボット

構造

一台に二つのインテリジェントブロックを使いました。上のインテリジェントブロックはタイヤのモーター、ストックしているボールを送り出す装置のモーター、二つのカラーセンサー、超音波センサー、タッチセンサーにつながっており、下のインテリジェントブロックはアームについている3つのモーターにつながっています。
IMG_20200207_160453.jpg
アームは人の手に例えると手(指の根本)、手首、肘の3つの関節があります。手と肘はほとんど直接モーターの動力を伝えていますが、腕の軽量化のために手首は肘から歯車をいくつか介してモーターの動力を伝えています。これを含めたアームの製作はハード面において最も苦労した部分だと思います。

1581652930825.jpg  
1581652930784.jpg
下の写真は歯車でどのように動力を伝えるかを考えている段階で作った簡易的なモデルです。茶色の軸がある部分が肘のモーターと手首のモーター(黒い歯車がある方が手首のモーター、灰色の歯車がある方が肘のモーター)に相当し、灰色の軸と黒の軸が一直線上に通ってる部分が肘に相当し、黒い軸一本が通っている部分が手首に相当します。ここでは手首と肘が近いのでわかりづらいですが赤いパーツとそれと同じように動く灰色のパーツが前腕に相当する部分です。動作の制御については手首を固定しながら肘を動かす場合は肘と手首のを同じ速度で回さなければならないという点に注意しなければいけません。この点に注意すれば、手首と肘をそれぞれ可動域内で任意の角度にすることができます。

20200201_153241700.jpg   20200201_153251596.jpg

動作の流れ

アームについているカラーセンサーを使って缶の上のボールを認識し、アームを使ってボールを持ち上げ、上のインテリジェントブロックの上にあるボールをためておく部分にボールを入れ、ボールを置く空き缶を超音波センサーで認識したらモーターを使ってボールを送り出し、上のインテリジェントブロックと下のインテリジェントブロックの間にある道にボールを通して、アームではみ出ないようにして、缶の上に置くという仕組みになっています。以下の画像のような流れで動作します。


  1. 1581652929472.jpg

  2. 1581652930066.jpg

  3. 1581652928585.jpg

  4. 1581652928669.jpg

  5. 1581652928712.jpg

  6. 1581652928864.jpg

  7. 1581652928903.jpg

  8. 1581652928933.jpg

  9. 1581652929434.jpg

プログラム

本番では課題用のプログラムを用意するのが間に合わなかったため、本番で説明用に使ったプログラムと課題用に使おうと思っていたプログラムを分けて書きます。

説明に使ったプログラム

アームを操作するプログラム

#!/usr/bin/env python3
from ev3dev2.motor import OUTPUT_B,OUTPUT_C,MoveTank,LargeMotor,Motor,OUTPUT_A,OUTPUT_D
from ev3dev2.sensor.lego import ColorSensor,TouchSensor,UltrasonicSensor
import paho.mqtt.client as mqtt
from time import sleep

m_e = LargeMotor(OUTPUT_B)
m_w = LargeMotor(OUTPUT_A)
m_set_e = MoveTank(OUTPUT_B,OUTPUT_A)
m_h = Motor(OUTPUT_C)

v_e = 10
v_w = 10
v_h = 5

m_h.on(0)
m_w.on(0)
m_e.on(0)

t_wait = 0.5 


class joint:
    def __init__(self,part,deg_up,deg_set,deg_down,v): #part=
                                                           #モーター(m_eまたはm_wまたはm_h)
                                                       #deg_up=
                                                           #up()実行時にpartで指定したモーターの角度を何度にするか
                                                       #deg_down=
                                                           #down()実行時にpartで指定したモーターの角度を何度にするか
                                                       #deg_set=
                                                           #set()実行時にpartで指定したモーターの角度を何度にするか
        self.deg_up = deg_up
        self.deg_set = deg_set
        self.deg_down = deg_down
        self.part = part
        self.v = v

    def up(self): #partで指定したモーターの角度を、アームがボールを持ち上げたときの角度にする
        self.part.on_to_position(self.v,self.deg_up,block=True)
    def set(self): #partで指定したモーターの角度を、ボールを置くときの角度にする
        self.part.on_to_position(self.v,self.deg_set,block=True)
    def down(self): #partで指定したモーターの角度を、ボールを探すときの角度にする
        self.part.on_to_position(self.v,self.deg_down,block=True)

elbow = joint(m_e,0,-165,-200,v_e)
wrist = joint(m_w,140,-150,-130,v_w)
hand = joint(m_h,0,100,10,v_w)

#-----------------------------------------------------
t_reset=1
v_e_reset = 10
v_w_reset = 20
v_h_reset = 10

def on_until_stalled(m,v): #モーターが止まるまで回す関数,m=モーター(m_eまたはm_wまたはm_h),v=モーターを回す速度
    m.on(v)
    sleep(t_reset)
    while m.speed>0:
        m.on(v)

def arm_reset(): #アームがある程度どんな形でも特定の形にしてアームのモーターの角度を全てリセットする関数
    m_set_e.on_for_degrees(v_e_reset,v_e_reset,90)
#hand
    print("hand")
    on_until_stalled(m_h,-v_h_reset)
    m_h.on(0)
#wrist
    print("wrist")
    m_e.stop(stop_action="hold")
    on_until_stalled(m_w,v_w_reset)
    m_w.on(0)
    m_w.stop(stop_action='hold')
#elbow
    print("elbow")
    on_until_stalled(m_e,v_e_reset)
    m_w.stop(stop_action='coast')
    m_set_e.on_for_degrees(-v_e_reset,-v_e_reset,45)
    m_e.stop(stop_action='hold')
    on_until_stalled(m_w,-v_w_reset)
    m_w.on(0)
    m_w.stop(stop_action='hold')
    m_e.on_for_degrees(v_e_reset,45)
    m_w.stop(stop_action='coast')
    on_until_stalled(m_e,v_e_reset)
#reset
    m_h.on(0)
    m_w.on(0)
    m_e.on(0)
    sleep(t_reset)
    m_h.reset()
    m_w.reset()
    m_e.reset()
#------------------------------
def fetch_ball(): #ボールをつかむ関数
    hand.up()
    sleep(t_wait)
    wrist.up()
    sleep(t_wait)
    elbow.up()
    sleep(t_wait)
    hand.set()
    reset()

    
def set_arm(): #アームをボールを探すときのアームの形にする関数
    hand.set()
    sleep(t_wait)
    wrist.set()
    sleep(t_wait)
    elbow.set()


def down_arm(): #アームをボールを置くときのアームの形にする関数
    hand.down()
    sleep(t_wait)
    elbow.down()
    sleep(t_wait)
    wrist.down()


#-----------------------------

def main(): #入力に応じてアームを動かす
    while True:
        i = input("fb,sa,da,reset,stop:") #fb,sa,da,reset,stopのいずれかを入力する。
        if i=="fb": #入力されたものがfbならばfetch_ball()を実行する。
            fetch_ball()
        elif i=="sa": #入力されたものがsaならばset_arm()を実行する。
            set_arm()
        elif i=="da": #入力されたものがdaならばdown_arm()を実行する。
            down_arm()
        elif i=="reset": #入力されたものがresetならばarm_reset()を実行する。
            arm_reset()
        elif i=="stop": #入力されたものがstopならばをプログラムを止める。
            break
        else: #入力されたものがfb,sa,da,reset,stopの中のどれでもない場合"not e,w,h,reset,stop"を表示する。
            print("not e,w,h,reset,stop")
        print("")
    print("end")

if __name__ == "__main__":
    main()

ライントレースするプログラム

#!/usr/bin/env python3
from ev3dev2.motor import OUTPUT_B,OUTPUT_C,MoveTank,LargeMotor,Motor,OUTPUT_A
from ev3dev2.sensor.lego import ColorSensor,TouchSensor,UltrasonicSensor
from ev3dev2.sensor import INPUT_1,INPUT_4
import paho.mqtt.client as mqtt
from time import sleep

address = input("address:")

m_r = LargeMotor(OUTPUT_C)
m_l = LargeMotor(OUTPUT_B)
tank = MoveTank(OUTPUT_B,OUTPUT_C)
cs = ColorSensor(INPUT_4)
cs2 = ColorSensor(INPUT_1)
ts = TouchSensor()
us = UltrasonicSensor()
m_pb = Motor(OUTPUT_A)
v_pb = 5

threshold = 37
v_s = 12.5
v_w = 5.0
v_b = 10
k_s = 0.25
k_w = 2.25
k_b = 0.75
cs_val_black = 10
cs_th_d_max = 20
csd_s_max = 5

v_is = 5

cs.mode ="COL-REFLECT"
cs2.mode ="RGB-RAW"
u s.mode ="US-DIST-CM"
 
a1_is=-0.15
a2_is=1.2

v_s_fb = 12.5
v_w_fb = 5.0
v_b_fb = 10
k_s_fb = 0.25
k_w_fb = 2.25
k_b_fb = 0.75

v_s_pb = 12.5
v_w_pb = 5.0
v_b_pb = 10
k_s_pb = 0.25
k_w_pb = 2.25
k_b_pb = 0.75

#1-------------------------------------------------------------------------------------
#2---intersection----------------------------------------------------------------------------------

def intersection(sign,list): """交差点や急なカーブに差し掛かった時
                                listとsignによって指定された道の指定された境界に行く関数
                             """
                             #sign:
                                 #値          :整数(-1または1)
                                 #どういう値か:
                                     #整数(-1または1):intersection()実行までにとっていた境界が右か左かを表す数字
                                                       #(右なら1左なら-1をとる)
                             #list:
                                 #値          :["i",整数,"r"または"l"]
                                 #どういう値か:
                                     #"i"         :intersection()を実行することをあらわす文字
                                     #整数        :選ぶ道が何番目にあるかを表す数字
                                                     """
                                                     (分岐を中心として
                                                     右の境界をとっていたとき時計回りに
                                                     左の境界をとっていたとき反時計回りに数える)
                                                     """
                                     #"r"または"l":選ぶ道の右と左の境界のうちどちらをとるか]
    print("list:",list)
    n_w = int(list[1])  # number

    if list[2]=="r":    # sign
        sign_is = 1
    elif list[2]=="l":
        sign_is = -1

    tank.on((a1_is+sign)*v_is,(a1_is-sign)*v_is)
    while True:
        if cs.value()>threshold:
            break

    tank.on((a2_is-sign)*v_is,(a2_is+sign)*v_is)
    for i in range(n_w):
        while True:
             if cs.value()<=cs_val_black:
                break
        while True:
            if cs.value()>threshold:
                break
    if sign_is*sign==1:
        tank.on(sign_is*v_is,-sign_is*v_is)
        while True:
            if cs.value()<=cs_val_black:
                break
        while True:
            if cs.value()>threshold:
                break

    tank.on(0,0)
    return sign_is

#3----follow_line---------------------------------------------------------------------------------

def follow_line(sign_fl): #交差点までライントレースする関数
    while True:
    #v_r,v_l
        cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold
        if cs_d <= -csd_s_max:
            v_fl=v_b
            k_fl=v_fl*k_b/cs_th_d_max
        elif -csd_s_max < cs_d < csd_s_max:
             v_fl=v_s
            k_fl=v_fl*k_s/cs_th_d_max
        else:
            v_fl=v_w
            k_fl=v_fl*k_w/cs_th_d_max
        v_r_fl = v_fl+sign_fl*k_fl*cs_d
        v_l_fl = v_fl-sign_fl*k_fl*cs_d
    #after_fl
        if cs.value()<cs_val_black: #intersection
            after_fl_fl = "list"
            break
        if ts.is_pressed: #touch_sensor 
            after_fl_fl = "ts_break"
            break
    #move
        tank.on(v_l_fl,v_r_fl)
    tank.off()
    return after_fl_fl

#5-------------------------------------------------------------------------------------
def main(): #入力に応じて分岐点までライントレースをする。
    print("hidaritaiya")
    tank.on_for_degrees(10,0,50)
    sleep(3)
    while True:
        i=input("n_w,rl_is,rl=") #,で区切った1つの数字と二つの文字(整数,"r"または"l","r"または"l")を入力する。
                                 #整数と最初の"r"または"l":
                                     #intersection()で使うlist=["i",整数,"r"または"l"]に使う
                                 #最後の"r"または"l"    :
                                     #follow_line()で使うsign_flとintersection()で使うsignに使う。
        list_add=i.split(",")
        list_add[0]=int(list_add[0].strip())
        list_add[1]=list_add[1].strip()

        if list_add[2].strip() == "r":
            sign = 1
        elif list_add[2].strip() == "l":
            sign = -1
        list =["i",list_add[0],list_add[1]]

        if ts.is_pressed: #break
            print("touch sensor break")
            break
        after_fl = follow_line(sign) #follow_line()
        if ts.is_pressed: #break
            print("touch sensor break")
            break
        sign = intersection(sign,list) #intersection()
        if ts.is_pressed: #break
             print("touch sensor break")
            break
    print("end")
    tank.on(0,0)

if __name__ == "__main__":
    main()

課題用のプログラム

上のインテリジェントブロックで使うプログラム

#!/usr/bin/env python3
from ev3dev2.motor import OUTPUT_B,OUTPUT_C,MoveTank,LargeMotor,Motor,OUTPUT_A
from ev3dev2.sensor.lego import ColorSensor,TouchSensor,UltrasonicSensor
from ev3dev2.sensor import INPUT_1,INPUT_4
import paho.mqtt.client as mqtt
from time import sleep
 
address = input("address:")

m_r = LargeMotor(OUTPUT_C)
m_l = LargeMotor(OUTPUT_B)
tank = MoveTank(OUTPUT_B,OUTPUT_C)
cs = ColorSensor(INPUT_4)
cs2 = ColorSensor(INPUT_1)
ts = TouchSensor()
us = UltrasonicSensor()
m_stocker = Motor(OUTPUT_A)
v_stocker = 5

threshold = 45
v_s = 12.5
v_w = 5.0
v_b = 10
k_s = 0.25
k_w = 1.5
k_b = 0.75
cs_val_black = 10
cs_th_d_max = 20
csd_s_max = 3

v_is = 5

cs.mode ="COL-REFLECT"
cs2.mode ="RGB-RAW"
us.mode ="US-DIST-CM"

a1_is=-0.15
a2_is=1.2

v_s_fb = 12.5
v_w_fb = 5
v_b_fb = 10
k_s_fb = 0.25
k_w_fb = 1.75
k_b_fb = 0.75
 
v_s_pb = 12.5
v_w_pb = 5.0
v_b_pb = 10
k_s_pb = 0.25
k_w_pb = 2.25
k_b_pb = 0.75

#1-------------------------------------------------------------------------------------

def read_list(file_name): #file_nameで指定されたファイルを読み込み、listにまとめる関数
                          #file_name=ファイルの名前
    main_break_bool =  False
    list=[]
    try:
        with open(file_name, "r") as f:
            list_raw = f.readlines()
            for i in list_raw:
                if i.strip() != '':
                    list.append(i.split(","))
            for i in range(len(list)):
                for j in range(len(list[i])):
                    list[i][j] = list[i][j].strip()
            list.append("end")
            print("raw list:",list_raw)
            print("list:",list)
            print("")
            #if not (check_list_bool(list)):
            #    print("file content error")
            #    raise Exception
    except:
        print("file read error")
        main_break_bool=True
    return (list,main_break_bool) #list:
                                      #交差点または急なカーブに差し掛かったときに何をするかを表すリスト
                                  #main_break_bool:
                                      #ファイル読み込みでエラーがあればTrue、なければFalseをとる変数

#2---intersection----------------------------------------------------------------------------------

def intersection(sign,list): """交差点や急なカーブに差し掛かった時
                                listとsignによって指定された道の指定された境界に行く関数
                             """
                             #sign:
                                 #値          :整数(-1または1)
                                 #どういう値か:
                                     #整数(-1または1):intersection()実行までにとっていた境界が右か左かを表す数字
                                                       #(右なら1左なら-1をとる)
                             #list:
                                 #値          :["i",整数,"r"または"l"]
                                 #どういう値か:
                                     #"i"         :intersection()を実行することをあらわす文字
                                     #整数        :選ぶ道が何番目にあるかを表す数字
                                                     """
                                                     (分岐を中心として
                                                     右の境界をとっていたとき時計回りに
                                                     左の境界をとっていたとき反時計回りに数える)
                                                     """
                                     #"r"または"l":選ぶ道の右と左の境界のうちどちらをとるか]
    print("list:",list)
    n_w = int(list[1])  # number

    if list[2]=="r":    # sign
        sign_is = 1
    elif list[2]=="l":
        sign_is = -1

    tank.on((a1_is+sign)*v_is,(a1_is-sign)*v_is)
    while True:
        if cs.value()>threshold:
            break

    tank.on((a2_is-sign)*v_is,(a2_is+sign)*v_is)
    for i in range(n_w):
        while True:
            if cs.value()<=cs_val_black:
                break
        while True:
            if cs.value()>threshold:
                break
    if sign_is*sign==1:
        tank.on(sign_is*v_is,-sign_is*v_is)
        while True:
            if cs.value()<=cs_val_black:
                break
        while True:
            if cs.value()>threshold:
                break

    tank.on(0,0)
    return sign_is

#--fb--------------------

def fb(): #3つの缶の上のボールを全てとる動作をまとめた関数
    fb1()
    fb2()
    follow_line(-1)
    intersection(-1,["i","2","r"])
    fb3()
    

def fb1(): #Cにあるボールをとる関数
    sign=-1
    reset_arm()
    follow_line_pb(sign,65)
    tank.on_for_degrees(-v_s,-v_s,150)
    turn180(sign)
    set_arm()
    follow_line_fb((-1)*sign,20,0.7)
    fetch_ball()
    tank.on_for_degrees(-v_s,-v_s,110)
    turn180((-1)*sign)
    turn180(sign)

def fb2(): #Fにあるボールをとる関数 
    sign=-1
    reset_arm()
    follow_line_pb(sign,100)
    tank.on_for_degrees(-v_s,-v_s,200)
    turn180(sign)
    set_arm()
    follow_line_fb((-1)*sign,20,0.7)
    fetch_ball()
    tank.on_for_degrees(-v_s,-v_s,0)
    turn90((-1)*sign)

def fb3(): #Eにあるボールをとる関数 
    sign=1
    reset_arm()
    turn180((-1)*sign)
    set_arm()
    follow_line_fb((-1)*sign,20,0.7) #cs sum,ratio
    fetch_ball()
    tank.on_for_degrees(-v_s,-v_s,0)
    turn90((-1)*sign)

#--pb--------------------
def pb(): #3つの缶の上全てにボールを置く動作をまとめた関数
    pb1()
    pb2()
    follow_line(-1)
    intersection(-1,["i","2","r"])
    pb3()
    

def pb1(): #Cにある缶にボールをおく関数
    sign=-1
    reset_arm()
    follow_line_pb(sign,100)
    down_arm()
    put_ball()
    put_ball()
    reset_arm()
    tank.on_for_degrees(-v_s,-v_s,150)
    turn180(sign)

def pb2(): #Fにある缶にボールをおく関数
    sign=1
    reset_arm()
    follow_line_pb(sign,100)
    down_arm()
    put_ball()
    put_ball()
    reset_arm()
    tank.on_for_degrees(-v_s,-v_s,200)
    turn90(sign)

def pb3(): #Eにある缶にボールをおく関数
    sign=-1
    tank.on_for_degrees(-v_s,-v_s,60)
    tank.on_for_degrees(-v_s,v_s,60)
    reset_arm()
    turn180(sign)
    follow_line_pb(sign,100)
    tank.on_for_degrees(-v_s,-v_s,150)
    turn90((-1)*sign)

#3----follow_line---------------------------------------------------------------------------------

def follow_line(sign_fl): #交差点までライントレースする関数
    while True:
    #v_r,v_l
        cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold
        if cs_d <= -csd_s_max:
            v_fl=v_b
            k_fl=v_fl*k_b/cs_th_d_max
        elif -csd_s_max < cs_d < csd_s_max:
            v_fl=v_s
            k_fl=v_fl*k_s/cs_th_d_max
        else:
            v_fl=v_w
            k_fl=v_fl*k_w/cs_th_d_max
        v_r_fl = v_fl+sign_fl*k_fl*cs_d
        v_l_fl = v_fl-sign_fl*k_fl*cs_d
    #after_fl
        if cs.value()<cs_val_black:
            after_fl_fl = "list"
            break
        if ts.is_pressed:
            after_fl_fl = "ts_break"
            break
    #move
        tank.on(v_l_fl,v_r_fl)
    tank.off()
    return after_fl_fl

def follow_line_fb(sign,val_break1,val_break2): #缶の上のボールをアームがとれる位置までライントレースする関数
    while cs2.value(0)+cs2.value(1)+cs2.value(2)<val_break1 or max(cs2.value(0),cs2.value(1),cs2.value(2)) / (cs2.value(0)+cs2.value(1)+cs2.value(2)+1)<val_break2:
        cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold
        if cs_d <= -csd_s_max:
            v_fb=v_b_fb
            k_fb=v_fb*k_b_fb/cs_th_d_max
        elif -csd_s_max < cs_d < csd_s_max:
            v_fb=v_s_fb
            k_fb=v_fb*k_s_fb/cs_th_d_max
        else:
            v_fb=v_w_fb
            k_fb=v_fb*k_w_fb/cs_th_d_max
        v_r_fb = v_fb+sign*k_fb*cs_d
        v_l_fb = v_fb-sign*k_fb*cs_d
        tank.on(v_l_fb,v_r_fb)
    tank.off()

def follow_line_pb(sign,val_break): #缶の上にボールをのせることができる位置までライントレースする関数
    while us.value()>val_break:
        cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold
        if cs_d <= -csd_s_max:
            v_pb=v_b_pb
            k_pb=v_pb*k_b_pb/cs_th_d_max
        elif -csd_s_max < cs_d < csd_s_max:
            v_pb=v_s_pb
            k_pb=v_pb*k_s_pb/cs_th_d_max
        else:
            v_pb=v_w_pb
            k_pb=v_pb*k_w_pb/cs_th_d_max
        v_r_pb = v_pb+sign*k_pb*cs_d
        v_l_pb = v_pb-sign*k_pb*cs_d
        tank.on(v_l_pb,v_r_pb)
    tank.off()

#4---u_turn,fb,pb----------------------------------------------------------------------------------
def reset_arm(): #通信を行いarm_reset()を実行させる
    client = mqtt.Client()
    client.connect(address,1883,60)
    client.publish("arm","reset_arm")
    client.disconnect()
    sleep(3)

def set_arm(): #通信を行いこの関数とは別のset_arm()を実行させる
    client = mqtt.Client()
    client.connect(address,1883,60)
    client.publish("arm","set_arm")
    client.disconnect()
    sleep(3)

def fetch_ball(): #通信を行いこの関数とは別のfetch_ball()を実行させる
    client = mqtt.Client()
    client.connect(address,1883,60)
    client.publish("arm","fetch_ball")
    client.disconnect()
    sleep(3)

def down_arm(): #通信を行いこの関数とは別のdown_arm()を実行させる
    client = mqtt.Client()
    client.connect(address,1883,60)
    client.publish("arm","down_arm")
    client.disconnect()
    sleep(3)

def put_ball(): #ストックしているボールを一つ送り出す
    sleep(2)
    m_stocker.on_for_degrees(v_stocker,60)
    sleep(2)
    
#5-------------------------------------------------------------------------------------
def main():
    main_break_bool=False
    index_list = 0
    after_fl="after_fl"

#read file
    #list
    print("start:read list")
    (list,main_break_bool)=read_list("list1")

#initialize
    if list[0][1] == "r":
        sign = 1
    elif list[0][1] == "l":
        sign = -1
    index_list += 1
    
#move
    if not main_break_bool: 
        while cs.value()>cs_val_black:
            tank.on(10,10)
        tank.on(0,0)

        while True: #main
            after_fl = follow_line(sign)    #follow_line

            if after_fl == "list":

                if list[index_list]=="end": #end
                    tank.on_for_degrees(v_fc,v_fc,270)
                    print("list:end")
                    break

                else: #list
                    if list[index_list][0] == "i":
                        print("intersection")
                        sign = intersection(sign,list[index_list])
                    if list[index_list][0] == "f":
                        print("fetch_ball")
                        sign = intersection(sign,list[index_list])
                        fb()
                    if list[index_list][0] == "p":
                        print("put_ball")
                        sign = intersection(sign,list[index_list])
                        pb()
                    index_list += 1
                    print("index:",index_list)

            elif after_fl == "ts_break": #break
                print("touch sensor break")
                break

            else:
                print("after_fl==after_fl")

            after_fl="after_fl"
            print("")
    else:
        print("main break")
    print("end")
    tank.on(0,0)

if __name__ == "__main__":
    main()

下のインテリジェントブロックで使うプログラム

#!/usr/bin/env python3
from ev3dev2.motor import OUTPUT_B,OUTPUT_C,MoveTank,LargeMotor,Motor,OUTPUT_A,OUTPUT_D
from ev3dev2.sensor.lego import ColorSensor,TouchSensor,UltrasonicSensor
import paho.mqtt.client as mqtt
from time import sleep

m_e = LargeMotor(OUTPUT_B)
m_w = LargeMotor(OUTPUT_A)
m_set_e = MoveTank(OUTPUT_B,OUTPUT_A)
m_h = Motor(OUTPUT_C)

v_e = 10
v_w = 10
v_h = 5

m_h.on(0)
m_w.on(0)
m_e.on(0)

t_wait = 0.5 

class joint:
    def __init__(self,part,deg_up,deg_set,deg_down,v): #part=
                                                           #モーター(m_eまたはm_wまたはm_h)
                                                       #deg_up=
                                                           #up()実行時にpartで指定したモーターの角度を何度にするか
                                                       #deg_down=
                                                           #down()実行時にpartで指定したモーターの角度を何度にするか
                                                       #deg_set=
                                                           #set()実行時にpartで指定したモーターの角度を何度にするか
        self.deg_up = deg_up
        self.deg_set = deg_set
        self.deg_down = deg_down
        self.part = part
        self.v = v

    def up(self): #partで指定したモーターの角度を、アームがボールを持ち上げたときの角度にする
        self.part.on_to_position(self.v,self.deg_up,block=True)
    def set(self): #partで指定したモーターの角度を、ボールを置くときの角度にする
        self.part.on_to_position(self.v,self.deg_set,block=True)
    def down(self): #partで指定したモーターの角度を、ボールを探すときの角度にする
        self.part.on_to_position(self.v,self.deg_down,block=True)

elbow = joint(m_e,0,-165,-200,v_e)
wrist = joint(m_w,140,-150,-130,v_w)
hand = joint(m_h,0,100,10,v_w)

#-----------------------------------------------------
t_reset=1
v_e_reset = 10
v_w_reset = 20
v_h_reset = 10

def on_until_stalled(m,v): #モーターが止まるまで回す関数,m=モーター(m_eまたはm_wまたはm_h),v=モーターを回す速度
    m.on(v)
    sleep(t_reset)
    while m.speed>0:
        m.on(v)
def arm_reset(): #アームがある程度どんな形でも特定の形にしてアームのモーターの角度を全てリセットする関数
    m_set_e.on_for_degrees(v_e_reset,v_e_reset,90)
#hand
    print("hand")
    on_until_stalled(m_h,-v_h_reset)
    m_h.on(0)
#wrist
    print("wrist")
    m_e.stop(stop_action="hold")
    on_until_stalled(m_w,v_w_reset)
    m_w.on(0)
    m_w.stop(stop_action='hold')
#elbow
    print("elbow")
    on_until_stalled(m_e,v_e_reset)
    m_w.stop(stop_action='coast')
    m_set_e.on_for_degrees(-v_e_reset,-v_e_reset,45)
    m_e.stop(stop_action='hold')
    on_until_stalled(m_w,-v_w_reset)
    m_w.on(0)
    m_w.stop(stop_action='hold')
    m_e.on_for_degrees(v_e_reset,45)
    m_w.stop(stop_action='coast')
    on_until_stalled(m_e,v_e_reset)
#reset
    m_h.on(0)
    m_w.on(0)
    m_e.on(0)
    sleep(t_reset)
    m_h.reset()
    m_w.reset()
    m_e.reset()
#------------------------------
def fetch_ball(): #ボールをつかむ関数
    hand.up()
    sleep(t_wait)
    wrist.up()
    sleep(t_wait)
    elbow.up()
    sleep(t_wait)
    hand.set()
    reset()

    
def set_arm(): #アームをボールを探すときのアームの形にする関数
    hand.set()
    sleep(t_wait)
    wrist.set()
    sleep(t_wait)
    elbow.set()


def down_arm(): #アームをボールを置くときのアームの形にする関数
    hand.down()
    sleep(t_wait)
    elbow.down()
    sleep(t_wait)
    wrist.down()


#-----------------------------

def on_connect(client,userdata,flags,rc):
    print("Connected with result code:" +str(rc))
    client.subscribe("arm")

def on_message(client,userdata,message):
    mes_str =  message.payload.decode()
    print("message topic:" + message.topic)
    print("message:" + mes_str)

    if mes_str=="fetch_ball":
        fetch_ball()
    elif mes_str=="set_arm":
        set_arm()
    elif mes_str=="reset_arm":
        reset_arm()
    elif mes_str=="down_arm":
        down_arm()

def main():
    address = input("address:")
    client = mqtt.Client()
    client.on_connect = on_connect
    client.on_message = on_message
    client.connect(address,1883,60)
    
    client.loop_start()
    main_bool = True
    while main_bool:
        a=input()
        main_bool = a!="stop"
    print("end")
    client.loop_stop()
    client.disconnect()

if __name__ == "__main__":
    main()

読み込むテキストファイル

まずこのファイルの書き方を書きます。
一行目を除いたそれぞれの行では、main()でのfollow_line()を実行中に分岐点に差し掛かった時にどのような動作をするかを指定します。 一行目には「s,1」または「s,-1」と書きます。これはプログラム開始時に右と左のどちらの境界をとっているということにするかを表しており、右ならば1左ならば-1を選びます。 残りの行ではintersection()だけを実行させるときは「"i",整数,"r"または"l"」、fb()を実行させるときは「"f",整数,"r"または"l"」、pb()を実行させるときは「"p",整数,"r"または"l"」を書きます。整数と"r"または"l"についてはintersection()の説明の中に書いてあります。fb()とpb()実行するときの整数と"r"または"l"はfb()とpb()の実行直前に行うintersection()がうまく動作するように書きます。

s,l
i,1,r
f,1,l
i,2,l
i,1,r
i,1,r
p,1,l

結果と感想

ハード面についてのアイデアをまとめる段階が少し雑になってしまい、メンバーの意見を十分に取り入れきれず、ロボットの構造のアイデアが固まってからもその実現や細かい変更に多くの時間を使い、本番の前々日までハード面の修正が必要となり、最終的にプログラムを試す時間が少なくなってしまいました。また、プログラムは主に私が作ることになっており他のメンバーにも手伝ってもらえることにはなっていましたが、プログラムを分担して作れるように考えることとプログラムに関する情報共有が不十分であったことからプログラムをほとんど一人で作らざるを得ませんでした。最終的に本番の前日から徹夜して大まかに作ってあったプログラムを修正しつつ作りましたが、インテリジェントブロックへのプログラムのコピーにかなりの時間がかかり、数値の調整ができず、本番には間に合いませんでした。関数ごとの挙動に関するデバッグは終わっていたので、もう少し時間があれば完成しそうというところまでは作れていました。これらのことを通して改めてグループで何かを作ることの難しさと計画的に進めることの重要性を感じましたが、それでも一人では今回作ったようなものを作るのは難しかったと思うので、これからこのような協力して何かを作る機会があった時にはこの経験を生かしてより良い結果となるようにしていきたいです。


添付ファイル: file1581652930784.jpg 2件 [詳細] file1581652930825.jpg 6件 [詳細] file20200201_153251596.jpg 7件 [詳細] file20200201_153241700.jpg 9件 [詳細] file1581652929434.jpg 8件 [詳細] file1581652928933.jpg 5件 [詳細] file1581652928903.jpg 5件 [詳細] file1581652928864.jpg 6件 [詳細] file1581652928712.jpg 5件 [詳細] file1581652930066.jpg 11件 [詳細] file1581652929472.jpg 2件 [詳細] file1581652928669.jpg 5件 [詳細] file1581652928585.jpg 6件 [詳細] fileIMG_20200207_160453.jpg 11件 [詳細] file2019b-mission2.png 9件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2020-02-15 (土) 11:43:27