２０１２の僕の自律型ロボットの開発についてまとめられた技術的文書です。

1. 自律型サッカーロボットの研究開発
作成者
出村賢聖
３年７組
金沢市立野田中学校
2012 年 8 月 31 日

2. 1
目次
序 論 2
今まで開発したロボット 3
新型機 7
コンセプト 7
ハードウェア 9
ソフトウエア 10
戦術 10
PID 制御 13
結 論 14
参 考文 献 14
付 録： メインプログラム 15

3. 2
序 論
私はロボカップという国際プロジェクトに参加することにより、自律型サッカーロボット
の研究開発をはじめた。ロボカップとは、
「西暦 2050 年「サッカーの世界チャンピオンチームに勝てる、自律型ロボットのチー
ムを作る」という夢に向かって人工知能やロボット工学などの研究を推進し、様々な分
野の基礎技術として波及させることを目的としたランドマーク・プロジェクトです。」
である[1]。
ロボカップには 4 つの分野があり、私はその中のロボカップジュニア[2]という分野で
研究に参加している。大学生や企業の研究者が参加している他３つのロボカップリーグ
とは違い、１９歳以下の子供達対象で、次世代の研究者と技術者を育てることを目的と
している。
ロボカップジュニアにもサッカー、ダンス、レスキューの３つのチャレンジがあるが、私
は、一番競技人口の多い、サッカーチャレンジに参加している。自作ロボットを使って 2
対 2 でサッカー競技を行う。前後半 10 分ずつで、ハーフタイムが 5 分ある。ロボットの
サイズは直径 22cm の円筒に収まらなければならない。
ロボットは赤外線発光ボールを追うための赤外線センサや壁、相手ロボットを検知す
るセンサなどを搭載している。方位を探知する地磁気センサも使用される。自律で動く
ためにロボットにはマイコンが搭載され、パソコン等でプログラムを作成してロボットを自
律制御させる。
今回の研究開発では 2010 年 12 月 から 2012 年 8 月までの研究内容を対象とし、本
稿では私が開発したロボットの詳細並びに今後の課題を述べる。
自律型ロボットとは人間
がリモコン操作で動くの
ではなく、ロボットが与え
られたプログラムとセン
サ情報に従い自動で動
くもの。
ロボカップジュニアの全
国大会（ジャパンオープ
ン 2012）には、地方大
会から数えると約 3400
名の子供が参加した。

4. 3
今まで開発したロボット
今の新型機ロボットに行き着くために試行錯誤して開発した一連のロボットについて
説明する。
図 1 : 初めてのオムニロボット（第１世代）
図 2 : 改造機 （第２世代） 図 3 : Japan Open2011 出場ロボット （第３世代）
図 5 : クリスマスバージョンロボット (第 4世代)図 4 : 関西オープン用ロボット

5. 4
図 4 : オムニロボットの動き方
図 1 のロボットは初めて製作した第 1 世代のロボットである。2010 年 12 月、中１の冬
休みに取り掛かった。この頃はロボットの知識がほとんどなかったため、指導者の構想
や用意した部品や材料を使い製作した。小学生の頃からレゴのマインドストームやいく
つかのロボットキットを組み立て、ライントレースなどの簡単なプログラミングをして小学
校の自由研究にも提出したことはあったのだが、自分で１からロボットをつくるのは初め
てだった。指導者から用意されたオムニホイールを使っていることが最大の特徴である。
オムニロボットとは全方位に無回転で移動できる機構を搭載したロボットの総称である。
図 6 のようにオムニロボットは 360 度あらゆる方向に動くことができる。自作ロボットに関
しては全くの初心者だったので、ロボカップジュニア用のモーター取り付けボードを使い、
私自身インターネットで色々なロボットを調べ、参考にして作成した。モーターに配線を
取り付けるための半田ごてや、配線が相手ロボットと絡まらないようカバーをつくるのが
難しく、大変だった。 制作に 3 カ月余りかかった。このロボットで中学校の科学部の 4
人でロボカップジュニア石川ブロック大会に参加したのだが、レゴのロボットキットを使っ
た小学生チームに敗れ 3 位入賞となった。
図 2 のロボットは Japan Open2011(日本大会)のルールの重量制限 1250g に収まるよ
う軽量化した第 2 世代のロボットである。石川ブロック大会では 1500g までの重量制限
だったが、Japan Open2011 ではルールが改正され 1250g に抑えなければいけなかった。
そのため、4 輪を3 輪へ、シャーシをアクリルからポリエチレンに変え、200g 軽量化させ
た。重量に余裕ができたのでドリブル装置を付け、1230g の重量に仕上がった。ドリブル
装置とはボールを車輪でバックスピンさせ、ボールの保持率を上げる機構である。重量
制限(1250g)の規定を守り、ドリブル装置を搭載することはロボット初心者の私にとって難
しかったが、指導者にアドバイスを受けながら作成した。
図 3 のロボットは Japan Open2011 (昨年の日本大会)に出場した第 3 世代のロボット
だ。毎日ロボット研究開発に明け暮れていたため、この頃にはロボットが大分わかるよう
になり、指導者のアイディアではなく、自分で考えた画期的な機構を沢山取り入れた。
自分にとってはいわば芸術作品のようなロボットである。作成の理由は、オムニホイール
が金属製で、試合中グレースケールの床を破いて退場させられる恐れがあったためだ
が、他のハードウェアも改造しようと思っており、今回はドリブル装置とキック装置をラチ
オムニホイールとは進行
方向の真横に移動でき
るように小さな樽状のタ
イヤがついた特殊な車
輪。

6. 5
ェットにより統合した機構（RSS: Ratchet Spin System）を開発した。これは 1 個のモータ
ーをラチェットにより正回転時はドリブル装置、逆回転時はキック装置を駆動させるもの
であり、軽量化を図った。
今までは減速比 15:1 のモーターを３個使用し３輪を制御していたが、姿勢制御が難
しかったので、後方のモーターのみ減速比 30:1 のパワーがあり、進む速度が遅く制御
しやすいものに変更した、3 輪の制御簡易化を狙った機構（KODS：Kensei Omni Drive
System）を発明した。
次に 105 システムである。これは、前方２個のモーター取り付け角度を 105°にして
前進、後進の速度を高める機構である。このように、ハードウェアの開発に力を注いだ。
図 5 : ＲＳＳと KODS
図 6：軽量化
ラチットとは回転方向を
1 方向に制限する機構。
歯車と歯止めで構成さ
れる。
減速比とはギヤの入力
と出力の回転数の比。
モーターは高回転だが
トルクが低いのでギヤと
組み合わせて使用。減
速比 30:1 はモーターの
回転数がギヤにより
1/30 になるが、トルクは
30 倍になる。

7. 6
また、RSS を搭載するため、さらなる軽量化が必要だった。そのため、ニッケル水素電
池 6 本から LiFePO4 電池にしたことで 170g 軽量化し、金属スペーサーを樹脂スペー
サーに交換して更に 30g の軽量化を図った。前回より 200g 軽量化したため、RSS を搭
載することができた。 Japan Open2011 に出場したロボットでキック装置とドリブル装置を
搭載したのは、このロボットだけであった。
図 4 のロボットは第 3 世代をマイナーチェンジしたものである。Japan Open2011 の問
題点を解決するために赤外線センサ（ボールを認識する）が周囲の光に干渉しないよう
ブラインド（覆い）をつけ、ロボットの自己位置を検出するため距離を検出する超音波セ
ンサ 4 つ垂直に付けた。これにより、床の色の濃さを利用した床センサの位置情報を統
合することにより自己位置を正確に推定することが可能となった。
図 5 は 2011 年 12 月に開催されたロボカップジュニア石川ブロック大会用に開発した
軽量化を極めた第 4 世代のロボットである。外装、ハンドル付き、キック装置付きで 800g
という超軽量ロボットを作り出すことができた。今回は赤外線センサを６個に増やした。
壁沿いブラインド攻撃（図 14）というロボカップジュニアの強いチームが使う戦法を石川
ブロックの選手や保護者に見せる必要があった。そのために、外装をつけた。この頃に
なるとロボットのハード製作にも余裕がでてきて、１２月の大会ということでクリスマスの飾
りつけをロボットに施し、デザインにも拘ることができた。
LiFePO4 電池はリチウ
ム燐酸鉄を極板に使
用。安全性、急速充電、
大電流放電、メインテナ
ンスが容易なことからホ
ビーから産業用まで近
年広く利用。

8. 7
新型機
コンセプト
昨年の日本大会 Japan Open2011（図３のロボット）では、初心者だった私はロボット
開発ばかりに取り掛かっていたあまり、プログラムに時間をかける余裕がなかった。その
ため、思いどおりにロボットが動かなかった。ボールを検出する赤外線センサの取り付け
位置が悪かったうえ、ハードウェアも欠点だらけだった。
それは、今までモノづくり経験がなかった私が最強のロボットを作ろうとして、新しい機
構を編み出すことばかりに時間をとられ、その機構やロボット自体に欠陥がないかソフト
ウエアで十分確かめていなかったからである。
その失敗がもう二度とないよう、昨年の上半期以内にロボット技術を学び、経験を積
み、下半期にロボットを製作し、ソフトウエアを開発した。今までの反省をもとに、第 5 世
代になる新型機のコンセプトを次に決めた。
 安定したロボットを作り上げること
安定したロボットを作るために、今までプラスチック製の植木鉢の受け皿をベースにし
ていたが、モーターなどを取り付けるとベースが歪んでしまいロボットの移動が安定しな
かった。そこで、ベースの素材を低発砲塩化ビニール板に変更し、一枚の板から切り出
すことにより問題点を解決した。図 9 の赤く見える板がベースである。さらに、ロボットの
移動を安定させるために 4 輪オムニ機構を採用した。
また、ボールを安定して検出できるように、ロボットの側面にボールセンサを 8 個取り
付け、死角をなくした。開発したロボットを図 9 に、スペックを表１に示す。
図 9： Japan Open2012 に出場した Saint ロボット (第 5 世代)
低発砲塩化ビニール板
は軽量性(比重 0.7)、優
れた加工性から建築模
型や工作材料として広く
使われている。ただし、
耐久性はさほどなく、強
い衝撃により割れる場合
がある。

9. 8
Japan Open2012 には、金沢市立野田中学校 3 年赤澤秀斗とチーム(DKT Shoot)を
組み、石川県代表として出場した。予選では、その後、世界大会で優勝する関西ブロッ
ク１位の GOROGORO チームを破り、第一シードを獲得したが、決勝トーナメントではロ
ボットの故障のため敗れ、ベスト 16 になってしまい、日本代表として世界大会に参加す
ることができなかった。しかし、このジャパンオープンで私のロボットの性能が話題になっ
た。審判や長くロボカップに参加しているチームの指導者から｢ダントツの動きのロボット
があった｣といくつかのブログに書かれ、話題となった。
そのおかげか、今年の 7 月 23 日から 30 日の間に、中国で開かれた国際大会
SICTIC2012(International School Student ICT Innovation Competition of China) [3,4]
に招待され、参加することとなった。SICTIC2012 は，中国教育省が主催する第 1 回国
際学生 ICT イノベーション大会であり、この大会は２１世紀の中学生と高校生の IT(情
報技術)リテラシーの向上並びに情報技術を媒介とした学生の国際交流を促進すること
を目的としている。
SICTIC2012 でも、私は赤澤秀斗とチーム(DKT Shoot)を組み、日本代表としてロボッ
トサッカー競技に図 10 のロボットで出場した。このロボットは Japan Open2012 の問題点
を解決するために、モーターを 2 枚の板で挟むことによりロボットを丈夫にし、耐故障性
を高めた。SICTIC2012 ではロボットの重量が 1200g と更に制限されたこともあり、キック
装置の搭載を止めた。それ以外は、Japan Open のロボットと概ね同じである。
競技会では今年のロボカップジュニア世界大会で 12 位となったドイツ高校生チーム
と互角に戦い、ロボットの優秀さが認められ優秀賞(the prize for excellence of Robot
Soccer Competition)を受賞した。
図 10： 国際大会 SICTIC2012 に出場したロボット(Saint)
Japan Open2012 の予
選では 5 チームずつ 14
グループに分かれリー
グ戦を行い、各グルー
プ上位 2 チームが決勝
トーナメントに進出する。
DKT Shoot は予選、4
勝 0 敗、62 得点 18 失
点、Ｅグループ 1 位で通
過した。得失点差の全
体順位では 70 チーム
中 4 位であった。なお、
決勝トーナメントは 2 回
戦で四国ブロック代表
KAIDA チームに敗れ
ベスト 16 の成績だっ
た。なお、KAIDA チー
ムは 2012 年の世界大
会で 3 位入賞した。
DKT Shoot はキック戦
術と赤澤秀斗
あかざわしゅう と
をかけて
命名された DKT は
DKT ロボットスクールの
意味。なお、DKT はロ
ボットスクール共同設立
者の出村、監物、歳森
の頭文字。

10. 9
ハードウェア
市販のマイコンに自分で切り出したベース（土台）やマイコンやセンサなどの電子部
品を組み合わせ、ロボットを一から自作した。ほとんどのチームのロボットは、市販されて
いるロボカップジュニア用パーツを組み合わせて作ったものである。私はこのように、一
からロボットを自作しているチームは、サッカーA リーグでは、世界でもごく少数に限られ
るということがわかった。 また、プログラミング言語は C 言語 を使用している。
本ロボットの最大の特徴は、あらゆる面において高速なことである。
まず、スピードだが、加速度が他のロボットと比べて圧倒的に大きい。ロボットの最高
速度は、Saint（私が開発したロボット）が 143.9 [cm / s]、Japan Open2012 を予選突破し
たロボットの多くが 146.5 [cm / s]とさほど差はない。だが、トルクは、Saint が 86.2 [N・
cm], 予選突破ロボットが 38.4 [N・ cm]と 約 2.3 倍の差があるので、最高速度に達する
まで時間が半分以下であり、平均速度は圧倒的に高速である。
スペック
タイプ X 型（モーターの配置が X 状に 4 つ付いている）
オムニ（全方位移動機構）
ベース（土台） 低発泡塩化ビニール
マイコン Arduino Mega 2560
駆動系 Joinmax 製 DC モーター
High Power Motor 3526: 4 個
Joinmax 製 オムニホイール
Omniwheel Pack ：1 個 (ホイール 4 個入り）
Joinmax 製 電源分配器
Power Distributor 4114： 1 個
センサ Joinmax 製 赤外線センサ
flame sensor : 8 個
Parallax 製 超音波センサ
Ping : 4 個
ダイセン製 地磁気センサ
多機能電子コンパス DSR-1005 1 個
付属品 GROVE ベースシールド 1 個
GROVE シリアル LCD 1 個
重量 1150g
言語開発環境 Arduino IDE (C 言語/C++)
電圧 DC13.2V (LiFePO4 電池 2 個)
表１： Saint ロボットのスペック
C 言語は自動車や家電
製品などの組み込み用
コンピュータに広くつか
われているコンピュータ
言語である。
Arduino はイタリア製の
マイコンボートと開発環
境の総称。安価でありオ
ープンソースのハードウ
ェアであることから教育
や趣味などで広く使わ
れている。ロボカップジ
ュニアの自作ロボットで
は良く使われている。

11. 10
次に、処理速度は,Saint は 16MHz,予選突破ロボットは 8MHz と 2 倍の差があるの
で、ボールの反応率や条件分岐の速度が高いので、制御並び判断の速度が相手の一
歩先以上をいける。
もう一つの特徴は、視力（ボール検出能力）が抜群に良いことだ。大抵のチームは、
ボールをコートの半分以下の面積の中でしか発見できないが、Saint はコート上のすべ
ての場所でボールを検出できる。しかも、中立点にボールを置くときなど、審判が一時
ボールを空中に上げるが、殆どのチームは地面にあるボールしか検出できない、つまり
２次元でしかボールを発見できないが、Saint は空中にあるボールを検出できる。つまり、
3 次元の視力を持っている。さらに、環境光（周囲の光）をロボットがボールと見間違える
現象があり、それを改善するためには視力を下げるしかないが、Saint の赤外線センサ
は環境光に干渉しにくいため、視力を下げる必要がほぼない。
ソフトウエア
戦術
考え方
「2 台で攻め、２台で守る。」
昨年のロボット戦術もこれを採用した。この戦術を使うと自分のチームのロボットが一
台欠けても、攻撃並びに防御を両立できる。しかも、2 台の守備は世界大会出場のドイ
ツのチームに前半戦一点も入れさせなかった鉄壁の防御、2 台で攻めると日本ではトッ
プ４の得点率である。
この戦術を実現するためには、総合戦術を考える必要があった。5 つの基本戦術と 2
つの主流戦術を合併した。
基本戦術
① 回り込み (図 11)
② 掻き出し (図 12)
③ 自陣待機 (図 13)
④ ブラインド攻撃 (図 14)
⑤ シュート (図 15)

12. 11
回り込みとは、図 11 に示すように自分の陣地にはボールを攻めず、円、四角等の軌
道を描いてボールを敵陣に回し、オウンゴールを阻止する戦術である。私はオムニロボ
ットの特性を使い、ずっと敵陣の方向を向いているプログラムを書いた。回り込むときは
平行移動を使い、正多角形の軌道を描く。
掻き出しは、図 12 のようにゴールの角や壁につまったボールを移動させて攻めにつ
なげる戦術である。Japan Open2012 では超音波センサが試合中衝撃で外れてしまい、
掻き出しプログラムが作動しなかった。しかしそれが意外と功を奏した。
自陣待機は、図 13 のようにロボットがボールを発見できないとき自陣に戻ることで防
御を固めることができる戦術である。自陣にいる場合は回り込みする必要がないので、
攻めの速度が上がることによって、形勢逆転のカウンター攻撃につながる。この戦術を
極めることにより、敵のブラインド攻撃で得点されることがほぼなくなった。
図 12 : 掻き出し
図 71 : 回り込み
図 13 : 自陣待機
図 14 : シュート 図 15 : ブラインド攻撃

13. 12
シュートは、図 14 のようにキック装置を使いボールを敵陣まで飛ばして得点する戦術
である。Japan Open2012 の時、このロボットにはキック装置が付いていた。威力は強かっ
たが、あまり得点源にはならなかった。SICTIC2012 ではロボットを軽量化する必要があ
ったため、キック装置を外した。
ブラインド攻撃は、ボールを壁とロボットで挟み、ボールを発見できないように死角を
作りながら攻める戦術である。私が昨年出場した Japan Open2011 では高校生チーム
（日本代表として世界大会に参加）にこの戦術を使われ、全くボールが発見できず、一
方的に攻められ完敗してしまった。
主流戦術
① 正面突破 (図 16)
② ブラインド＆オウンゴール (図 17)
正面突破戦術とは、図 16 に示すように相手のゴールに向かい一直線で攻める戦術
である。守りが堅いチームには得点しにくいが、Saint の速度が圧倒的に速く、トルクも強
いためボールの支配率が圧倒的である。そのため、少しでも敵が隙をみせれば得点で
きる。予選ではこの戦術がとても役に立ち、大量得点の礎となった。
ブラインド＆オウンゴール戦術は、図 17 のようにブラインド攻撃を仕掛け、次に高速
のボールをキーパーにぶつけることにより、オウンゴールをさせ、得点する戦術である。
この戦術はキーパーがいない場合でもブラインド攻撃という形で得点できるので、戦術
に穴がない。この戦術を思いついた理由は、石川ブロック大会 2012（図４ロボット）にブ
ラインド攻撃を使用しており、キーパーにボールが当たり得点したことがあったためひら
めいた。石川ブロック所属、同じくベスト 16 位に今年なったチームにはこの戦術がとて
図 16 : 正面突破 図 17 : ブラインド& オウンゴール

14. 13
も有効だった。 この戦術は今回の切り札だったので、強いチーム以外には発動する条
件を多くして発動率を下げた。関西ブロック１位の GOROGORO チーム（その後今年世
界大会優勝）と対戦するときに使ったが、この戦術が全く通用しなかった。キーパーのベ
ース（土台）が衝撃を吸収する素材を使っていて、ボールがキーパーに衝突すると静止
するようになっていた。おかげで大苦戦を強いられた。結果としては勝ったのだが、ロボ
カップジュニアの歴史があり、毎年のように日本代表として世界大会に出場するチーム
を誕生させている関西ブロックは強い。
PID 制御
今回、私が最も力を注いだのが PID 制御である。
PID 制御とは、P（Proportional, 比例）制御、I（Integral, 積分）制御、D（Derivative,微
分）制御を統合したものである。この制御を使い、以前より目標の方位により正確、かつ
高速にロボットを向けるようになった。
P 制御は、目標値と現在値の差（偏差）をモーターへのパワー（操作量）として与え、
偏差を０になるよう制御するものだ。だが、この制御だけでは偏差が小さくなったときに、
モーターへ供給するパワーが不足し、いつまでたっても目標値に到達できないという問
題がある。
そこで、目標値に正確に到達するために、I 制御を使う。偏差を時間ごとに足していき、
その合計値を操作量に加えることにより、目標値と現在の値が 0 になるまで正確に制御
する。これは、ロボットが回転するときに壁などにひっかかった時や段差がある地面を走
行するときなどに有効である。
最後に、D 制御である。環境などの変化により、現在値が目標値から急激にずれてし
まうことがある。PI 制御では目標値に修正するまでに時間がかかるが、微分制御では瞬
時に修正できる。目標地点でロボットが停止する制御を考える。PI 制御では、ロボットは
前進して目標地点で停止する場合に、目標地点を行きすぎないと後進にならない。ロボ
ットは慣性があるので急には止まれず、目標地点を通りすぎてしまう。一方、PID 制御で
は、目標地点に停止する前に後進の制御がかかり、目標地点で停止することができる。
この制御は大学レベルであるが、コンピュータにプログラミングすると、微分が減算、
積分が加算になるので中学生でも理解し、実装できる。これを C 言語でプログラムにし、
ロボットで実現するには２ヶ月くらいかかった。

15. 14
結 論
今まで 10 台以上もロボットを開発してきたが、そのたびに技術が上がり、ロボットづく
りの楽しさに気づくことができた。
また、大学のロボティクス学科の教授である父が 2011 年 5 月から石川の子供たちの
ためにロボットを教える DKT ロボットスクールをボランティアで開講した。私もスクール生
になるつもりだったのだが、アシスタントとしてスクールを手伝うよう言われ、教える側に
なった。そのため、自分で今まで以上にロボットやプログラミングを勉強するようになり、
教えることが楽しくなった。
これにより、ボランティア精神が生まれ、自分のことだけではなく石川の子供たちを強
くしたいという気持ちが生まれた。これからも石川のロボカップジュニアを、先輩である自
分が中心となって引っ張って行きたい。
参考文献
[1] ロボカップ日本委員会公式ウェブサイト, http://www.robocup.or.jp/
[2] ロボカップジュニアジャパン公式サイト, http://www.robocupjunior.jp/
[3] SICTIC (International School Student ICT Innovation Competition of
China), http://www.sictic.org/news/eng/index.html
[4] 国際学生信息科技大会 http://www.sictic.org/

16. 15
付 録： メイ ンプログラム
プログラムは多くのサブプログラムから構成されるが、ここでは最も中心となるメインプログラムのソースコードを
記す。
#include "Arduino.h"1
#include <SerialLCD.h> // シリアル LCD 用2
#include <SoftwareSerial.h> // シリアル LCD 用3
#include <Servo.h> // サーボモータ用4
#include <Wire.h> // I2C 用(地磁気センサ)5
#include <MsTimer2.h>6
#include "dktlib3.h"7
8
// ロボットプログラムの中心部分。この関数を繰り返し実行する。9
void loop()10
{11
static int step = 0; // 繰り返しの回数を数える変数12
checkLCD(step); // LCD のチェック13
Forward(); // フォワードロボット用関数ム14
step++; // １回実行されると step が 1 増加15
}16
17
// フォワードロボット用関数18
void Forward()19
{20
static int g_loop = 0;21
PID();22
if (g_diff > 60 || g_diff < -60 ) turn(g_pid);23
else24
{25
g_dist[0] = getPing(0);26

17. 16
g_dist[1] = getPing(2);27
g_dist[2] = getPing(1);28
g_dist[3] = getPing(3);29
g_integral = 0;30
getMaxball();31
if(g_no == 0)32
{33
if(g_ball[0] - g_debugBall[0] > 50)34
{35
if(g_dist[1] >= 35 && g_dist[3] >= 35)36
{37
moveAngle( 100 , g_pid , 0);38
}39
else if (g_dist[0] < 13)40
{41
if(g_dist[3] < 35)42
{43
turn(-80);44
delay(60);45
}46
else if (g_dist[1] < 35)47
{48
turn(80);49
delay(60);50
}51
}52
}53
else moveAngle( 100 , g_pid , 0 );54
}55

18. 17
else if(g_no == 1)56
{57
if (g_dist[1] < 15) moveAngle (100 , g_pid , 0);58
else if(g_ball[1] - g_debugBall[1] > 100)59
moveAngle( 80 , g_pid , 90 );60
else moveAngle( 80 , g_pid , 45);61
}62
else if(g_no == 2)63
{64
if(g_ball[2] - g_debugBall[2] > 100 )65
moveAngle( 100 , g_pid , 180);66
else moveAngle( 80 , g_pid , 90);67
}68
else if(g_no == 3 )69
moveAngle( 80 , g_pid , 225 );70
else if(g_no == 4 )71
{72
if (g_dist[1] > g_dist[3]) moveAngle( 80 , g_pid , 90 );73
else moveAngle(80 , g_pid , 270 );74
}75
else if(g_no == 5)76
{77
moveAngle( 80 , g_pid , 135 );78
}79
else if(g_no == 6)80
{81
if (g_ball[6] - g_debugBall[6] > 100)82
moveAngle( 100 , g_pid , 180 );83
else moveAngle(80 , g_pid , 270);84

19. 18
}85
else if(g_no == 7)86
{87
if (g_dist[3] < 15) moveAngle (100, g_pid , 0);88
else if (g_ball[7] - g_debugBall[7] > 100)89
moveAngle( 80 , g_pid , 270 );90
else moveAngle ( 80 , g_pid , 45);91
}92
else goHome();93
}94
g_loop++;95
}96
97
// 自陣待機98
void goHome()99
{100
if (g_diff > 45 || g_diff < -45) turn(g_pid);101
if(g_dist[2] > 15) // rear ping102
{103
moveAngle( 100, g_pid , 180 );104
printLCD("goHome: back");105
}106
else if (g_dist[1] < 60)107
{108
moveAngle( 18 , g_pid , 90 ); // g_dist[1]:right move left109
printLCD("goHome:left");110
}111
else if (g_dist[3] < 36)112
{113

20. 19
moveAngle( 18 , g_pid , 270); // g_dist[3]: left114
printLCD("goHome:right");115
}116
else117
{118
Stop();119
printLCD("goHome:stop");120
}121
}122
123
// ＰＩＤ制御124
void PID()125
{126
int last_diff = g_diff;127
static int target = getCompass();128
static float g_integral = 0;129
g_diff = target - getCompass();130
if(g_diff > 180) g_diff -= 360;131
else if(g_diff < - 180) g_diff += 360;132
float pro = g_diff * 0.2;133
if (last_diff + 2 <= g_diff && last_diff - 2 >= g_diff) g_integral += 0.5;134
if (g_diff < 0) g_integral *= -1;135
while (pro + g_integral > 70)136
{137
pro *= 0.9;138
g_integral *= 0.9;139
}140
g_pid = (int) (pro + g_integral);141
}142