More than 5 years have passed since last update.

【昔話】ETロボコン 2007年向けシミュレーター開発の記録【アーカイブ】

Last updated at 2019-03-10Posted at 2019-03-10

本投稿は10年以上前のブログ記事が元になっているため、技術の情報が古いです。ご注意ください

概要

ETロボコン 2007用のシミュレーターを開発したときの記録です（参戦記を兼ねる）
まだ気軽なUnityも、気軽な物理シミュレーターも、気軽なDeep Learningも無い時代に、若さ！？とパッションだけでシミュレーターをフルスクラッチ開発しました＾＾

ETロボコンシミュレーターとは

作ったETロボコンシミュレーターが動いている様子はこれです。(注意：BGMつきの動画が開きます）

12年前の自分がもっているスキルをフル動員してフルスクラッチでETロボコン(RCX時代)のPCシミュレーターを作りました。

【技術】

Visual C++ 2005 →走行ロジック
Visual C# 2005 →シミュレーション計算、GUIパート
Direct3D →3Dレンダリング
H8 C++(RCX)

【シミュレータースペック】

運動学計算：2輪モデル
BRICK：LEGO Mindstorm(RCX)
緒元値：RCXベースのパスファインダーから実測、2D表示モード・3D表示モード、実時間シミュレーション可能、ETロボコン 2007対応

プロローグ

UMLでロボットのソフトウェアを設計して戦うロボコンがあるという話を、同じプロジェクトの仲間のN女史から聞いた。
彼女の所属会社でETロボコンに参加するチームがあるということで、そのチームの仲間に入れてもらうことになった。

ROBO-ONEなど、ロボコンには何度か参加したことはあったがUMLでロボットのソフト設計を競うというのは個人的に思いもよらなかった面白い視点。

私が出場していたロボコンではたいていメカとか回路まわりに苦戦して、ソフトももちろん書くけど、ソフトの中身のつくりで勝負しようとは思っていませんでした。

でも、このロボコンはソフトウェアの設計にフォーカスしているらしい。

ロボットもソフトも好きな人間としてはとっても興味がわく。

ETロボコン 2007 とは

ロボコン名称

ロボコンの名前は「ETロボコン」と呼ばれている。古くは、ETソフトウェアデザインロボコンと呼んでいたそう。

このロボコンの面白いところは、本来ロボコンの主役となるはずのロボットがあらかじめ規定されているということ。

Webサイト http://www.etrobo.jp/

パスファインダー

その規定されているロボットというのがLEGO Mindstormで作ったパスファインダーと呼ばれる走行ロボット。

LEGOでできた機構部分だけでなく、モーター、センサー、からマイコン、それらをつなぐ回路などまで、出場者みんなが同じ機体を使う。

「何で勝敗・優劣を決めるのか？」

機体が一緒なんだから、

「当然、勝敗はソフトウェアで決まります。」

ということだそう。

コンテストの内容

勝敗、優劣の判定基準は大きく分けると、ソフトウェアのモデルで競う、レースのタイムで競う、の２つの側面がある。

コンセプト

参加にあたり、こんなコンセプトをたてた

■モデル評価重視型アプローチ

(1)アーキテクチャ方針
　・内部および外部品質特性を明示的にアピールできるようにする。→ISO9126の１つ１つに
すべてロジカルに答えられるようにする。

　・アーキテクチャと品質特性の因果を明示したい。→○○を達成したいから、ここはこう
なっているという説明ができるように。

(2)分析・設計
　・プロダクトライン（コアと変動性の層別）分析設計をやってみたい。→コアと変動部分の分析、例えば次回ルールが変わりそうなところはどこ？

　・カタリシスによるコンポ分析をやってみたい。

■タイムアタック重視型アプローチ

(3)制御アルゴリズム
　・PID制御をきちっとSWで実装する。＆それをOOで設計する。
　・自機の相対ポジションを保持する。　→モータ電圧とステアリングと経過時間から計算
可能？
　・フィードフォワード制御をする。　→あらかじめすべての通路情報をインプットしてお
く。
　・シャトル多数決システムの採用　→同一アルゴリズムを意味的に異なる設計で多重につ
くり、多数決値を制御につかうスペースシャトル方式を使う。
　・学習システム搭載　→GA,強化学習,ニューラルネットとか。

(4)シミュレーション
　・シミュレータを作成し、最適アルゴリズムを探す。

追加アイデア

(1)外乱に強いつくりにする
　　→光量の情報なども瞬間値でなく積分値、加重平均でとるなどの工夫。

(2)実機の操作には薄いラッパーをはさむ。直接APIをたたかない。
　　→シミュレータも同じインタフェースで実装しておけば実機とシミュで同じ制御ロジックを使いまわせるでしょ。

(3)タッチセンサーを活用する。

スケジュール

    | 公式            | 内部予定
-----+-----------------+---------------
4/20 |                 | ▲コンセプトの決定
     |                 | ▲アーキテクチャ＆担当の決定
4/25 |                 | ▲LimitedFunction 1
4/27 | モデリング教育  | 教育２：Yさん参加
5/11 |                 | ▲LimitedFunction 2
5/18 |                 | ▲FullFunction
5/25 |                 | ▲RC
5/27 | 試走会Ⅰ        | おそらく土曜に参加
6/08 | 試走会Ⅱ        | 未定
6/30 | 関東地区予選    | 未定
7/14 | 本戦            | 未定

チーム

チームは私ふくめて4人。

チームといっても、それぞれのプロジェクトがあるので、ほとんど集まれないので
主担当をきめて、それぞれが自宅で各自進めるということになった。

私の担当

他のメンバーは所属会社のお金で実機を買ってもらっていた。彼らは業務としてロボコンに参加できるのだが、
私は所属会社もプロジェクトも違うので実機（パスファインダー）を持っていなかった。

もちろん業務としての参加ではなかったので、
実機のアルゴを試せるシミュレーターを開発する役割を買って出た。

うまくいくか不明だが、　最適アルゴリズムを探す　というミッションのもとシミュレーターを開発することにした。

シミュレータのアイデア

シミュレータの実現ステップ

こんなステップでシミュレータをつくる。期間内にベストエフォートで。

目的にあわせて、どのレベルの正確さを求めるか、が物理シミュレーター作りのポイント。

STEP1:通称：LV1
最低限の動作をシミュレーションする。2次元、3自由度（X,Y,ヨー）の車両のシミュレーション。必要な各種パラメータは実機あわせ。実時間シミュレーションの離散化はシンプルなオイラー法。2週間くらいで作るというのはこれくらいが限界かな

STEP2:通称：LV2
少し動力学、運動学をいれてみる。各種パラメータ（たとえば慣性モーメントとか、テンソルとか）は物性値から計算してみる。2次元、3自由度（X,Y,yaw）の車両のシミュレーション。実時間シミュレーションの離散化はルンゲクッタで。
STEP3:通称：LV3
3次元、6自由度の運動シミュレーション。ぜんぶ自作プログラミングだと厳しいかも。まっとうな物理エンジンとか、最低でもMATLABくらいは使いたい。

実装よりの整理

実装よりにシミュレータへの要望を整理してみると、こんな感じ

Windowsプラットフォームで動作すること
みんなのPCがWindowsだから。昔つくった3Dライブラリあるので、DirectXつかう。
今後のSTEP2,STEP3への物理計算拡張を考慮した設計とすること
物理計算処理も階層化したほうがよさそう。ソルバー単位になるかな。
物理計算エンジン部分を取替え可能に設計すること
物理計算と描画の境界線は意識しましょう。学生の頃、物理計算用のPG組んだとき、これを混ぜて痛い目を見た。後から描画処理を追加するときに、計算処理に手をいれたくなるもの。
パスファインダーをパーツのアセンブリとして表現すること
将来パスファインダーを構成する部品が変わってもいいように。
3Dグラフィックスへの対応を考慮した設計とすること
いずれ3Dにも対応させたいので、スケール感をもって、2Dの計算をさせたほうが良い。
複数台のパスファインダーの同時シミュレーションに対応すること
複数でアルゴリズム対決みたいなのをシミュレータ上でできたら楽しそう。
C++で記述されたパスファインダー制御コードと連携できること
パスファインダーの制御コードはC++なので、薄いWrapperをかませるような使い方を想定します。

実装イメージ

もう少しイメージをふくらませて

シミュレータの能力

パスファインダー（実機）写真

シミュレーションする現象

とりあえず、STEP1では以下のようなものを再現してみる。

動きの再現
直進、バック
ステアリングによる旋回（円旋回）
動きの原因の再現
トラクションモーター
ステアリングモーター

物理計算の制約

STEP1では、

計算は2次元、3自由度
車両は4輪等価な2輪モデル

急激な運動は考慮しない。急加速、減速、急ステアリングなど。
過渡現象は考えない→車両速度は一定
みたいな感じを想定

いよいよ実機で計測

実機のサイズ

パスファインダーの寸法をマニュアルで測る。
それにしても資料をさがしてもどこにも寸法が書いていないというのは不思議

こんな計測方法なので精度がまったく保証されない。
しかし、物理シミュレーションのアバウトさ加減を考えると1mm,2mmずれてもどうってことは無い。

実機走行実験

①速度測定実験

実機の速度を測定する実験を行う。

目的
プログラム内でのトラクションモータ制御値とパスファインダー（実機）の速度の関係を調査する。
実験方法
一定距離の直線を走行させて、走破時間をストップウォッチで計測する。

実験条件
593mmの直線コースを、モーター制御値 255 128 64 32 でそれぞれ走行させる。

実験結果を以下に示す

考察

実験1,2,3は制御値(電圧)とほぼ速度と比例の関係を確認した。
実験4は低電圧のため、かかる負荷トルクにモータトルクが影響を受けていると思われる。
電池はフル
ストップウォッチは人間の目視のため不正確

利用できそうな計算式の導出

計算式の導出
速度と制御値Xの関係を1次式で近似してみた

y=2.0552x+51.445

負荷トルクを無視できる、制御量がある程度大きいときには上式で近似する。

②定常円旋回実験

ステアリング性能をみるため、ある角度にステアリングをきったまま同一速度で旋回運動をさせる実験をする。

目的
舵角δと旋回半径Rの関係を調査すること
実験方法
舵角と速度を固定し、定常円旋回運動をさせ、パスファインダーの走行軌跡を調べる

実験条件

一定速度V=325(mm/s^2)\\
一定舵角δ=0.698(rad)・・・40°\\

定常円旋回実験結果

走行させたものを固定カメラで録画し画像解析して定常円旋回性能をしることができた。
下の図はステアリングを40°に固定したときの軌跡。きれいな円を描いている。

実験結果

**走行軌道より旋回半径

旋回半径が判明

R=111 (mm)

定常円旋回計算式確認

定常円旋回計算式
パスファインダーを四輪と等価な二輪車モデルとすると、一定速度V(mm/s2)、一定舵角δ(°)の定常円旋回運動は以下の式で表すことができる

R=\frac{(1+AV^2)L}{δ} ・・・（１）

\begin{align}
&R:旋回半径(mm)\\
&L:ホイールベース（:車軸間距離） (mm)\\
&δ:舵角(rad)\\ 
&V:速度(mm/s^2)\\ 
&A:スタビリティファクタ\\
\end{align}

上式より

A=\frac{Rδ}{L – 1}\frac{1}{V^2} ・・・（２）

Aはスタビリティファクタとなる。

スタビリティファクタAの導出

実験結果を(2)式に代入

A=-1.22×10^{-6}

ここで

\begin{align}
&A >0 アンダーステア(US)・・・速度増加とともにRが増加する。\\
&A =0 ニュートラルステア(NS)　\\
&A <0 オーバーステア(OS)・・・速度増加とともにRが減少する。\\
\end{align}

であるので

パスファインダーは オーバーステア（OS) の特性をもった車両ということになる。

③ステアリングモーター動作実験

目的
プログラムから指示する制御値に対するステアリングの回転数（角速度）の導出。
LEGOモータの定格とステアリングに力を伝達するギア比から計算してもいいが、結局のところ慣性モーメントその他の影響を考慮すると実際にタイアを地面につけたまま、制御の末端であるステアリングの回転数をはかったほうがより現実的だろうという判断をした。
実験
ステアリングが90°回転するまでの所用時間を計測した

実験結果

実験データを対数近似でフィットさせた以下の式をステアリング角速度の計算に用いる

Y=103.29Ln(x)-209.04

x:制御値 (17～255)

制御値<16の場合、
まったく動作しない(モータ＋ギアのトルクが摩擦にまけて動かない）

可動・寸法計測

ステアリングの可動範囲

以下の２つについて計測する

運動限界舵角
勝手に命名(本当は何て言うんだっけ？)
これ以上ステアリングを切りすぎるとまともな動きをしなくなる（つかえる、倒れる、壊れる等）限界の角度を計測する

運動限界舵角＝±80°

可動限界舵角
メカ的に動くことのできる角度を計測

可動限界舵角＝±120°

計算用のモデル化

以前計測したものを、計算用の簡易寸法にした

20070428 シミュレータ用計算式まとめ

これまで導出してきたパスファインダー自身の動作に関する計算式の類を備忘録もかねてまとめておく

速度 Vs


Ctr<64 のとき\\
Vs=0

64≦Ctr≦255 のとき\\
Vs=2.0552Ctr+51.445

Ctr:トラクションモータ制御値\\
Vs:速度 (mm/s)

ステアリングの角速度ω

Cst<16のとき\\
ω=0

16≦Cst≦255 のとき\\
ω=103.29Ln(Cst)-209.04\\

定常円旋回半径 R

定常円旋回半径の一般式

R=\frac{(1+AV^2)L}{δ} ・・・（１）

\begin{align}
&R:旋回半径(mm)\\
&L:ホイールベース（:車軸間距離） (mm)\\
&δ:舵角(rad)\\ 
&V:速度(mm/s^2)\\ 
&A:スタビリティファクタ\\
\end{align}

パスファインダーの定常円旋回半径計算式

L=88(mm)\\
A= -1.22×10'-6'\\

より

R=\frac{(1+(-1.22×10^{-6}Vs^2))×88}{δ}

定常円旋回半径R計算の補足

ある時間t1における舵角（ステアリングの角度)をδ1とすると、そこからΔt秒進んだ時間t2(=t1+Δt)における舵角δ2は以下のようになる

δ2=δ1+ωΔt

ここで

ω=103.29Ln(Cst)-209.04

Vs=2.0552Ctr+51.445

なので、これを

R= \frac{(1+(-1.22×10^{-6}Vs^2))×88}{δ}

に代入すると

R= \frac{(1+(-1.22×10^{-6}(2.0552Ctr+51.445)^2))×88}{(δ1+(103.29Ln(Cst)-209.04)Δt)}

となり、定常円旋回半径RはCstとCtrと経過時間Δtの関数として計算することができる

シミュレータ構想

設計の構想

ここでは設計指針、機能要求、非機能要求、を要求と呼ぶことにして、再度やりたいことをまとめた。

以下のような感じ

1.Windowsプラットフォームで動作すること

2.今後のSTEP2,STEP3への物理計算拡張を考慮した設計とすること

3.物理計算エンジン部分を取替え可能に設計すること

4.パスファインダーをパーツのアセンブリとして表現すること

5.3Dグラフィックスへの対応を考慮した設計とすること

6.複数台のパスファインダーの同時シミュレーションに対応すること

7.C++で記述されたパスファインダー制御コードと連携できること

8.新しい技術ネタを取り入れること
やるからには単なるソフトの設計作業でなくて、何か新しいことSomething Newが欲しい。

9.とにかく動くこと
2007年のゴールデンウィーク中に動くものを作る

10.見た目（も）重視
シミュレータでインパクトを与えるのも目的。見た目もダサくならないように注意。

アーキテクチャを考える

アーキテクチャとはソフトウェア構造のラフスケッチ、要求からラフスケッチをおこす

スケッチの修正

責務

Algorithm
パスファインダーのアルゴリズムを担当。シミュレータとは物理的にも完全に分離する。シミュレータに対するクライアントになる。シミュレータをCOMで作ってしまうのもありだ。
Wrapper
スクラッチで作るので、実質インタフェースを定義して、インタフェースに対して実装しようねと言っている。
Simulator
COMではなくて、.NETライブラリで実装する。実装レイヤーに近いほど覚えた知識があっというまに使えなくなる、時代の流れは早い。
- RCX Simulator
  パスファインダーと、パスファインダーの外的環境、その２者の相互作用など。
- Kinetics
  動力学。学生のときには「ｘｘｘにおける動力学」みたいなレポートをよく作ったが今回は時間的におそらくまともな実装はできない。
- Kinematics
  運動学。こちらは、「モドキ」レベルで最低限実装する。
Draw
画面への描画を担当。RCX Simulatorと密接に関わる。

要求に答える

要求とアーキテクチャ

要求から、アーキテクチャをすこし詳細化する。実装に入りたいので、もうパッケージを意識する

□内の数字はアーキテクチャに対応する、要求番号となる

2.今後のSTEP2,STEP3への物理計算拡張を考慮した設計とする
3.物理計算エンジン部分を取替え可能に設計する
2と3はパスファインダーと物理計算の分離および、物理計算と外部環境（たとえばコースとのインタファクション）との分離を意識して設計せよ、ということになる
4.パスファインダーをパーツのアセンブリとして表現すること
パスファインダーの形状は毎年変わるかもしれないので、パーツ（たとえば１つのLEGOブロック）を組んでパスファインダーの形状を再現できるように設計しておく必要がある。ルールが変わってもいいように、違う形状のパスファインダーを同時に走らせるなんてこともやりたい。
5.3Dグラフィックスへの対応を考慮した設計とすること
パスファインダーを描画するグラフィックはとりあえず2Dで作るが、3Dでも使えるように設計しておく。つまり2Dでも3Dでも同じ操作体系で使えるように設計せよ、との方針になる。Factory MethodとかTemplate Methodとか「サブクラスさんおまかせ」系のデザパタがつかえる。
6.複数台のパスファインダーの同時シミュレーションに対応すること
PathFinderクラスをnewしてaddして大量生産できるよに設計する。OOP的にはあたりまえかもしれないが、最初に考えておかないと苦労する「あるある」ポイント。二十歳そこらの頃、robo-one用のシミュレータを作ったときは、ここで泣いた（笑）

要求と開発環境

もういちど考えを整理

	Algorithm	Wrapper	Simulator
プラットフォーム	Windows .NET Framework2.0
(理由)	MFCがタダで使える環境が無いので。
開発環境	VC++ 2005 Express		VC# 2005 Express
（理由）	ロジックはパスファインダー側と同じにしたいのでC++。C++/CLIをさわってみたい。		VC#のりファクタリング機能が使いたい(VC++には無いの）＆新しいことを覚えたい。
構成管理	SubVersion
（理由）	VSSが有料でSubVersionもTortoiseSVNも無料だから
C++とC#の連携	.NETのコンポーネントにするので問題無いはず。Event/Delegateまわりには注意。

要求の整理
- 1.Windowsプラットフォームで動作すること
- 7.C++で記述されたパスファインダー制御コードと連携できること
  Windowsプラットフォームの選択肢を考える。パスファインダー用のマイコン(H8)コードはC言語 or C++言語を使うので、そっちと連携するにはWinでもC++を使うのがよさげ。
- 8.新しい技術ネタを取り入れること
  (1).NET Framework 2.0ってどんな感じか。
  (2)C++/CLIってどんな感じか。
  (3)C#の新しい仕様（パーシャルクラスとか）ってどんな感じか。
  (4)新しい開発環境
- 9.とにかく動くこと
  RADする
- 10.見た目（も）重視

設計・実装開始

開発環境インストール

Visual C# ExpressとVisual C++ Expressをダウンロード・インストールする。
http://www.microsoft.com/japan/msdn/vstudio/express/default.aspx

SQL Server Express サーバは不要。Webからインストールするが、数時間かかった。

クラス設計

無償で使えるUMLツールJUDEを使わせていただいた。

パッケージGraphic.Misawa2D.Parts

とっかかりはグラフィックから。１つ１つのグラフィックパーツとなる。 2Dのパッケージ名は数字からはじめられないのでMisawa2Dで。。。
.NETのSystem.Drawingと同じクラス名だとまぎらわしいのでPrefix、M2Dを使う。

.NET Frameworkに依存しないように書いてみる。C#はJavaに少し似ているので、後でAppletにも移植してみようという野心あり。モデルベースで書いておけば、苦労せずにJavaに持っていけそう。

パッケージGraphic.Misawa2D.Model

複数パーツを組み合わせてPathFinderを構成するクラスとなる。PathFinderの形状は今のところ１つだけ。

実装

3日かけてGraphicパッケージ（とその周辺）の0版実装が終わった。

今回の教訓その１：マトリックス演算は
Utilクラスの座標変換にはマトリックスを使うが、これを.NETが持っているものを使うか、自分でマトリックス演算を書くか最後まで悩んだ末、自分で書くことに。Javan等、他プラットフォームに移植したいので特定言語にしかない便利系ライブラリをつかうと移植が面倒そうという理由。2Dの場合は2x2のいわゆる行列演算なのでまだ楽。

こんな感じ

//位置ベクトルの回転
public static M2DVector RotateVector(M2DVector srcVector, double degree)
{
    double radian = GetRadianByDegree(degree);

    M2DVector destVector = new M2DVector();
    destVector.U = (srcVector.U) * Math.Cos(radian) - (srcVector.V) * Math.Sin(radian);
    destVector.V = (srcVector.U) * Math.Sin(radian) + (srcVector.V) * Math.Cos(radian);

    return destVector;
}

今回の教訓その2：プロパティを使うかどうか
C#にはCOMの時代よろしくプロパティという概念がある。アクセッサをメソッドを合理的に扱うというものだが、これを使うと便利だけど、移植性ややきになる。

モデルベースといいつつも、コードそのまま流用ももくろむ。

こんな感じ。


public double U
{
    set { this._relativeU = value; }
    get { return this._relativeU; }
}
public double V
{
    set { this._relativeV = value; }
    get { return this._relativeV; }
}

実行イメージ

デフォルメされたパスファインダーが動いた

Genericは便利

C#でもGenericタイプが使えるようになった。Javaよりやや遅れて。コレクションではわかりやすくその恩恵にあやかることができる。

private List<M2DVertex> _lstVertex;

型パラメータとしてM2DVertexを指定するので、このListはM2DVertex用のリストということになる。いちいちキャストしなくて済むのでとっても便利。

クラス設計

パッケージEtrobo.Simulator

パスファインダー、コース、論理的なモーターなどを設計する

と最初はこうしたが、無意味に一般化してもしょうがないので、下のような現実案とすることにする。またSensorは分け合って独立事象にした。

あとでC++用にさらにWrapperをかぶせる予定なので、あまりしつこい設計にしたくないというのが今回の現実案の理由となる。

パッケージEtrobo.Kinematics

さらに、パスファインダーの動作を計算する運動学用（切り替え可能）のクラスを追加した。切り替え可能だが現状ではAggregateではなくNavigate扱いにしておく。設計をややこしくしたくないので、物理計算を切り替えたいときはこのクラスのサブクラスをつくっていって、PathFinderからつかうときは、クラス名そのものを書き換えることにする。
ModelBaseのModelは物理モデルを意識している。

実装

親のコンストラクタでdelegate使う

delegateを使ってトリッキーな継承をやってみた。
従来のC++ではオブジェクトを作ってから、メソッドの関数ポインタを取得してきて、それを使用側であてがって使ったりしたところが、delegateと親クラスをあらわすbaseの柔軟性により１つにまとめて書いた。

Motor.cs


using System;
using System.Collections.Generic;
using System.Text;

namespace Misawat.Etrobo.Simulator
{
    public class Motor
    {
        //SetMotorSpeedメソッドのデリゲート変数
        SetMotorSpeed _func_SetMotorSpeed;

        //回転方向 1:正転 -1:逆転
        int _direction = 1;

        //回転速度
        int _speed = 0;
        
        public Motor(SetMotorSpeed func_SetMotorSpeed)
        {
            _func_SetMotorSpeed = func_SetMotorSpeed;
        }
        public void Foward(int speed)
        {
            _speed = speed;
            _direction = 1;
            _func_SetMotorSpeed((double)(_direction * _speed));
        }
        public void Reverse(int speed)
        {
            _speed = speed;
            _direction = -1;
            _func_SetMotorSpeed((double)(_direction * _speed));
        }
        public void Speed(int speed)
        {
            _speed = speed;
            _func_SetMotorSpeed((double)(_direction * _speed));
        }

        public void Brake()
        {
            //TBD
        }
        public void Off()
        {
            //TBD
        }
    }
}

SteeringMotor.cs


using System;
using System.Collections.Generic;
using System.Text;

namespace Misawat.Etrobo.Simulator
{
    public class SteeringMotor : Motor
    {
        public SteeringMotor(PathFinder pathFinder)
            : base(pathFinder.SetSteeringMotorSpeed)
        {
        }

    }
}

Graphic.Misawa2D.Partsを振り返る

実装したところの修正をかけていて、クラス図の修正をした

グラフィックパーツはそれぞれ子要素をもっており１つのモデルをくみ上げるときには複数のグラフィックパーツの親子関係を設定してツリーを作っていく。

つまり、再帰的にツリーを構成していく部分(build())をcompositeパターンとして実現しているので、クラス図に少し補足をいれた。

実行イメージ

赤い点が旋回中心、円が定常円旋回軌道となる。

ステアリングの角度に対して定常円旋回計算式は正常に動作した模様。
最後はステアリングの限界舵角を超えているので、旋回円は無効となる。

離散化にともなうΔtの取り扱い

物理シミュレーションを行うときには、必ず物理方程式の離散化を考えるが、今回は軽量シミュレータなので、運動方程式といっても一階微分方程式くらいのもの。微分方程式は、漸化式による差分方程式を作ってシミュレーションするが、ここで注意しなくてはならないのは、位置を更新する際の経過時間Δtとなる。

一般にΔtは小さいほど精度良くシミュレーションできるがこのシミュレータでは、パスファインダーに入るセンサー情報のサンプリング周期とΔtを同期させている為、なるべくΔtはシミュレータのサンプリング周期にあわせたい。

Δtが100msecとか、あんまり大きいとまずいなと思い実機でサンプリング周期をはかってみたところ5msecであった。
最高速度500mm/s程度のスケール感なら、まあいいかということで、Δtを5msecとした。あとはこの状態でシミュレーションをして実機との挙動があまりに違っていたらΔtの長さの変更や、Δtとシミュレータのサンプリング周期の同期をやめるなどの判断をしていきたい。最後は現物あわせ

コースとのインタラクション

コースは1mmにつき1pixelとして取り扱う。つまり1m×1mだと1000pixel x 1000pixelのコース画像を用意する。

コースの色情報の抽出

パスファインダーの先端部分にある光センサーの座標を計算し、その座標に対応するコース画像上の1pixel x 1pixelの色情報を取得して、LightSensorの値にセットするというシンプルなロジック。

シミュレータ部分のコーディングはひとまず終わり。
リバースかけてみると。

実行イメージ

シミュレータを使ったコーディング

C++でシミュレータプログラムを作る

C++でパスファインダーに搭載するプログラムを意識して、プログラミングをしていく。
C++といっても.NET2.0のC++はちょっと違うので「H8用のコテコテのC++とVisual C++で同じコード使いたい」ときには色々と注しないといけない。

こういうところが面白い。

.NET Framework 2.0 のC++はC++/CLIに準拠していて、今までのC++とは文法の記法が違うところもあり注意が必要。.NET1.1から.NET2.0（C++/CLI)でポインタが*から^になったのにはちょっと驚いた。

ほかにも、Visual C++ 6.0時代はnmakeでmakefileベースでビルドできたのに、VC2005みてたらどうやらMSBuildというツールに変わった模様。Apache ANTっぽい感じになっている。

Managed C++とOldSyntax

.NET時代のc++はUnmanagedとManagedの２つがあるが、C#でつくってきたシミュレータライブラリ(SimEtrLib.dllという名前にした)がこれをManagedなライブラリとしたので、C++もManaged C++を使う。また、C++/cliで導入された構文でなくて、C++の構文を使わないとパスファインダーのコードと連携ができなさそうなので、構文はC++を使う

.NET Framework 2.0のManagedなコンパイラでは、デフォルトでC++/CLIとなっているので、オプションで古い構文(OldSyntax)を指定してやる必要がある。

ライブラリを用いたコーディング

コード例は、イメージをつかむために掲載する。ヘッダ読めばわかるところなどは省略。

走行アルゴリズム部分(RunningLogic)のコード
これはH8用のBrickOS APIのシグネチャを真似て作ったもので、これを使ってH8側とWindows側でアルゴリズム部分のソースコードを同一のものにすることができる。

RunningLogic


-------RunningLogic.h----------------------------------------
# pragma once
using namespace Misawat::Etrobo::Simulator;

public __gc class RunningLogic
{
public:
	RunningLogic(PathFinder* pathFinder);
	~RunningLogic(void);
	void RunningLogic::Run(void);

private:
	RunningLogic(void);	
	PathFinder* pathFinder;
	LightSensor* lightSensor;
	TouchSensor* touchSensor;
	Motor* tractionMotor;
	Motor* steeringMotor;
};

-------RunningLogic.cpp----------------------------------------

# include "StdAfx.h"
# include "RunningLogic.h"

RunningLogic::RunningLogic(PathFinder* pathFinder)
{
	lightSensor = pathFinder->GetLightSensor();
	touchSensor = pathFinder->GetTouchSensor();
	tractionMotor = pathFinder->GetTractionMotor();
	steeringMotor = pathFinder->GetSteeringMotor();
}

//↓ここからが走行アルゴリズムそのもの
void RunningLogic::Run(void)
{
	tractionMotor->Foward(255);

	if (lightSensor->Get() < 50)
	{
		steeringMotor->Foward(255);
	}
	else
	{
		steeringMotor->Reverse(255);
	}
}

※クラスをnewするときの_gcキーワードは、ガベージコレクションの対象とするオブジェクトですよということを宣言している。そのため、オブジェクトを破棄するときに明示的にdeleteする必要は無い。

シミュレータ部分のコード
PathFinderオブジェクトをCreateして、SimulatorPlatformにAddすると、コース上にパスファインダーが出現する。
何台でもCreateしてAddできる。
RunningLogicクラスでは、LightSensorの値（コースの白黒値）を読んで、TractionMotorやSteeringMotorを制御する。

Simulation.cpp

-------Simulation.h----------------------------------------
# pragma once

using namespace System;
using namespace System::Text;
using namespace Misawat::Etrobo::Simulator;

public __gc class Simulation
	{
	public:
		Simulation(System::Drawing::Graphics* myGraphics);
		virtual ~Simulation(void);
		void Simulation::Start();
		void Simulation::Stop(void);
	private:
		Simulation(void);
		bool running;
		SimulatorPlatform* _simulatorPlatform;
		System::Drawing::Graphics* _myGraphics;
	};
 -------Simulation.cpp----------------------------------------
# include "StdAfx.h"
# include "Simulation.h"
# include "RunningLogic.h"

Simulation::Simulation(System::Drawing::Graphics* myGraphics)
{
	_myGraphics=myGraphics;
}

void Simulation::Start()
{
	//SimulatorPlatform(コースの画像データ,グラフィックコンテクスト,描画時の倍率)
	_simulatorPlatform = __gc new SimulatorPlatform(".\\test_course01.png", _myGraphics,0.6);

	PathFinder* pathFinder = _simulatorPlatform->CreatePathFinder(2400, 500, 90);
	pathFinder->SetBodyColor(System::Drawing::Color::FromArgb(255,255,0,0));

	RunningLogic* pathFinderLogic=__gc new RunningLogic(pathFinder);

	running=true;

	while(running)
	{
		System::Windows::Forms::Application::DoEvents();
		_simulatorPlatform->Update(0.01);
		pathFinderLogic->Run();
	}
}

いろいろなシミュレーション条件

情報を表示しながら

2台同時走行

方向を変えて２台で。

4台同時走行

カラフルに4台で。にぎやか。

これでシミュレータV1.0は完成
あとは、本番コースの画像で、ちゃんとしたシミュレーションロジックでシミュレーションをまわしてみる。

コース画像の準備

コースの大きさ

5460mm×3640mm

コースのダウンロード

ETロボコンのサイトからコースのＰＤＦをダウンロードする

ETロボコン事務局から配布されるPDFをみてみると、

なので、これは実際の縦横それぞれ1/2スケールになっている。

コース画像の変換

PDFからPNGへ

シミュレータでは1mmあたり1pixelのコース画像を使う（ことにしました）のでこのＰＤＦ(2730mm x 1820mm)を5460pixel x 3640pixel　の画像に変換する。

PostScriptオペレーションの定番GhostScriptをつかうと、PDFをPNG形式の画像に変換することができる。


gswin32c.exe -dSAFER -dBATCH -dNOPAUSE -sDEVICE=png16m
	-r96 -sOutputFile=.\honban_course.png  .\robocon_course20070423.pdf

PNGファイルはサイズが10318 x 6879 ある。これは96DPIを指定したからなので、こうなっているが、これはGIMPで補完圧縮をし、さらに簡単なスクリプトを書いて変な色になっていた部分を変換して使えそうなコース画像を作った。

最終的に 5260 x 3640 の画像となる。

メンバー向けにシミュレーターリリース

2007 ゴールデンウィークを使って開発したシミュレータ＋サンプルコードを同じチームのメンバーに初公開しましました。

評判はまずまずで、最低限の役目は果たせたような気がする。

本番コースでシミュレーション

本番コースでシミュレーションしてみた。2台で併走。

全体像

ちゃんと動いているようでひと安心。

ダメだし

友人(工学博士、物理シミュレーション、運動学)にこのページ+ソースコードを見せて運動学の計算について読んでもらいましたが、当然いろいろとダメだしが。方程式、離散化、剛体か質点か、厳密にやろうとするといろいろ問題が。アカデミック軸ではなく、「モドキ」軸ということで貴重な意見を今後の改善に生かすとして今回はこのまま行こうということに。

3D化対応設計

パッケージ

もともと3Dへの対応を考慮して設計していたので、特に今までの設計と変更はなし。Misawa2Dというのに加えてMisawa3Dというパッケージにする。

クラスの設計

3Dの実現には、Windowsのゲーム用APIのDirect3Dを使用する。
OpenGLでも良かったが昔作ったコードを流用したかったのでDirect3Dを使うことに。

設計の構想は2Dのときとほぼ同じ。

3Dのモデルは3Dモデラー(LightWave)を使ってモデリングする。

3Dのパスファインダー

3Dプラットフォームの実装

Direct3Dを用いて実装を開始。

地面ポリゴンを配置する。マウスでグリグリするインタフェースを搭載。

コース画像のテクスチャを地面ポリゴンに貼る

3D車両データ

LDRAWの3Dデータを組み合わせてログラムから操作できる3Dデータにする。

3D実装つづき

3Dプログラミングは純粋にたのしい

3Dのプログラミングは純粋に楽しい。
過去のETロボコンのサイトを見ていたら、コース上にモアイのような置物がおいてあったのでLightWave3Dをつかってモアイ君をモデリングした。

モアイ君をSimulator3D上に配置してグリグリ動かしてみた。

せっかくモデリングしたのでどこかでこのモアイ君を使いたいなぁ。

2007年5月23日プレ試走会

某社の某会議室で実機を走らせてみた

本番用のコースを印刷したものを会議室にならべてこれから実機を走行させてみた。

数十枚を並べる作業はかなり手間がかかる。
1枚ならべると隣がずれたり、ずれを修正しているとまたほかがずれたり。やはり現実の世界は何かと”リアル”な問題が起こる。
これが楽しい。

↓ようやくしきつめた、でもパスファインダーが走行するたびにズレる(笑)

シミュレータで基本ロジックは確認したものの、結局現実世界へのあわせこみは実機を使ってTry&Errorしかない。実機にプログラムを焼いちゃ消し、焼いちゃ消しという作業に明け暮れた。

組み込み界隈ではよく見る光景。

実機が動くと楽しいので、深夜までTry&Errorは続く。

物理計算モジュールとの連携

連携

計算モジュール(Misawat.Etrobo.Kinematics)と3Dの表示モジュール(Misawat.Graphics.Misawa3D)を連携させた。

コースを疾走するモアイ君

裏で物理演算モジュールが、コースの計算をしている。
3D上では、パスファインダーの代わりにモアイ君を走行させてみた。

モアイがんばれ

3Dモデルの生成成功

3Dのパスファインダーモデルの生成に成功

簡単に書くと、LDRAWにあるパスファインダーは頂点数が多すぎて使い物にならない(HAL使っても超ノロノロ描画)ことが判明。

LightWaveでブーリアンを駆使して足し算引き算、その後、オリジナルスクリプトで面の向きをすべて外向きにし、そとから見えないところを削除し、頂点を減らして、くっつけて、構成しているパーツから重なっている面をとりのぞいて、、、と全部で３０ステップくらいフィルターをかましてやっと、プログラムからリアルタイムでレンダリングできる3Dモデルに変換できた

モアイ君->タイア

3Dモデルを作ったので、モアイ君を卒業して、3Dモデル化したパスファインダーのタイアに差し替える。
タイアが転がった～。

3Dの親子関係

3Dモデル

3Dモデルは以下のような親子のツリー構造になっている。

パスファインダーはBody(本体部分＝ＲＣＸと機構部）,FrontWheel（前輪）,RearWheel(後輪),LightSensor(光センサーと機構部)をそれぞれ独立したパーツとした。

2Dのときは、とても簡単なモデルだったので、頂点座標を計測して、それらを結んで多角形をつくってパスファインダーの外形にしたが、3Dでは構造がとても複雑なので、もっと大きな単位（たとえばBodyだけでも数千のポリゴンがある)で扱った。

↑FrontWheelはLightSensorの回転と連動するので、LightSensorの子になっている。

パーツごとの動作

親子関係をつけた状態でパーツごとに適当に動かす。ちゃんとそれぞれ連携して動いている。

タイヤの回転

前輪と後輪を速度から計算して回転させる、ここのロジックは単純で、

\begin{align}
&v=rω\\
&v:速さ(mm)\\
&r:タイヤの半径\\
&ω:角速度(rad/s)\\
\end{align}

なので、

ω=v/r

より計算する

パスファインダー疾走！

パスファインダーを、本番コースをセットしたシミュレータ上で実際に走らせて見た。
やはり、2Dよりも、モアイ君よりリアリティある

カメラアングル

3Dシミュレータのよい点は、簡単に好きなところにカメラを持ってこられる点

これでカメラワーク演出が可能となる

ズームIN/OUTや固定視点、可動視点などいろいろと実装した。

移動追跡カメラ

固定カメラ

車載カメラ搭載

パスファインダーに乗車

車載カメラを搭載した

どこに載せてもいいが(パラメータを変えるだけなので）、リアルでは不可能そうな場所ということで、光センターの後方の本体部につけてみた。

もし実際ここにカメラがついていたら、こんなに左右にゆさぶられるものだった。めがまわりそー。

でも走るレゴに乗った気分は楽しめる

ソフトウェア設計変更

設計書はメンテしよう

3Dで遊びすぎていてソフトウェアの設計そのものをないがしろにしつつあったが、、設計変更した。というか、実装してみてしっくりこなかったので、修正をかけてみた。

よくありがちな、設計書のメンテナンス放棄ということに陥らないように常に設計書(JUDE)をふりかえるようにしたい

Before After

クラスの意味も説明せずにやぶからぼうにBefore Afterだけを掲載しておくと、、、

設計変更前

設計変更後

設計変更したのに

設計とソースコードは、連動しないもの。。。

リバースしたものがこちら。

レビュー不足。

M3DFormは専用3Dウィンドウを出そうと思いつきで作ったが、これも使わないことにしたんだった、、、ということも忘れていた。

リバースをして確認というのは、このレベルでも役に立つなぁ。

Gadgetの追加

なんだかものさびしいので

ネットで本番のETロボコンの写真などをみていると、コース上にオブジェ（障害物？）がおいてある模様。非機能要件ということで、3Dシミュレータ上に、オブジェを配置できるようにしようかと。

オブジェの場所

3Dモデリングした

スケールをあわせながら3Dモデラーでモデリングした

スタート、中間ゲートにつかうポール

スフィンクス

ピサの斜塔（テクスチャあり）

ピラミッド（テクスチャあり）

凱旋門（テクスチャあり）

モアイ君（テクスチャあり）

テクスチャ（3Dモデルの表面に貼り付ける画像）があると、質感がでてリアルになる。

オブジェを配置

また設計変更

オブジェを配置できるようにするために、設計変更をしました。

SimulatorPlatform3D#AddGadgetメソッドでオブジェを追加できるようになった。

実行結果

コース上にオブジェを配置した。

ここまでプログラムが出来てくると箱庭をつくっているような感覚で楽しい。

モアイ君も自分の居場所をみつけた。
モアイ君に石模様のテクスチャをつけたらちょっと怖かったので、テクスチャはつけないことにした。

車載カメラのブレ防止

ローパス

車載カメラのブレが激しいため、カメラのlooking計算にローパスフィルターを導入。だいぶ揺れがおさまり、乗車感が向上。
車載カメラの位置を前にして景色が良くなるよう改善。

完全3Dプログラミング遊びになってきたが楽しいのでやめられない。

箱庭コースの中を疾走！

車載カメラ視点（BGMあり)

車載カメラ２つ

カメラ視点２つ。

それぞれのパスファインダーの車載カメラ視点で走行させてみた。

本番前日

本番とは

いよいよ明日が本番。
本番とは関東地区予選大会第1日目のこと。まだ予選。

シミュレーターを使ったアルゴリズム確認は各メンバーで行ってだいぶいい感じの動きをしていたとのフィードバックをもらう。

2007年6月30日 ETロボコン当日レポート

ETロボコンの関東地区予選会第１日目に参加した

開催概要

日時 2007年6月30日(土) 8:50-
場所工学院大学（新宿)

当日のタイムテーブル

9:00	参加者受付
9:30 ～ 11:30	試走・車検
11:30	モデル評価 (※ここまでは参加者のみ)
12:00	開会式
12:10 ～ 13:40	競技１回目
	休憩
14:00 ～ 15:20	競技２回目
15:30	表彰式
16:20 ～ 17:10	モデルワークショップ

受付～試走会

8:50に開催場所である工学院大学に集合。工学院大学は都庁至近の都会のなかの大学という感じ

受付でチームごとのゼッケンと名札をもらう

会場に入ると、目の前に本番用コースが。これはシミュレータのネタ収集のチャンス！

チームにはピットと呼ばれる、チーム用の机（会議室なんかにある折りたたみ机のようなもの）がある。２チームで１個の机を共有して、実機の調整等を行う。

ピットで早速工作、光センサーの遮光のための遮光スカートをつくる作業を行った。スカート部分をのりづけ。

遮光スカート工作終了

試走会がはじまった。一度に数チームがコース上でパスファインダーを試走させる

我々のチームの試走の順番がまわってきた

試走会～予選走行

いよいよ本番コースでの、試走となる
別チームのパスファインダーが高速に追い抜いていたりするが、うちはうちの走りができればOK
試走終了後、車検を行う。車検では、パスファインダーが設計図通りくみ上げられているか、不正改造が無いかなどがチェックされる。
車検に合格すると、競技時に使用する電池(Panasonic製)をもらうことができる。

競技用電池をパスファインダーにセットして、RCXのふたを閉じた後に封をしてもらう。「よくできました」

モデル審査開始。各チームが他チームのモデルを審査して、投票する。これがうちのモデルシート。

お昼12:00に、一般見学のお客さんたちもそろって、開会式がはじまる

開会式後から予選会スタート。予選会の本番走行の第１回目として、２チーム同時にインコース、アウトコースといった具合に走行していく。

われらがパスファインダー走り

予選走行～レース結果

トラブル発生
車検のときに、大会で使用するためのオフィシャル電池に取り替えた。その都合でRCXに再度ソフトウェアを書き込まなければいけないんのだが、書き込み用ソフトが起動しない、、、本番前になると、きまってこうトラブルが発生する。組み込みあるある。

なんとか復活
IRタワーのUSBを抜いて、PCを再起動して、、を何回か繰り返しているうちに、起動した。

またもや、あるあるな解決法

一安心してソフトウェアを書き込み

そして、ついに、うちのゼッケン番号が呼ばれた。いよいよ、順番が回ってきた。本番コースで走行前のキャリブレーションを行う。

いよいよ、スタート。手前がうちのパスファインダー、奥が一緒にはしるチームのパスファインダー。
司会の方のReady GOでスタート！

序盤、順調な走りをみせる

なかなか快調。一緒にはしるチームはパスファインダーの起動がうまくいかなかった模様

そして落下・・・

1週目後半、悪夢が。。。突然のコースアウト。その後、道無き道を迷走して、谷底に落下という末路をたどることに。
司会の方は「あららー落下しちゃいましたね～」
パスファインダーを持って唖然としている仲間達。

ということで、わがチームはリタイアとなりました。

無念！

後で分析したところ、ざっくりいうと、電池が強すぎたみたい。
(われわれが使っていた電池が弱かったみたい）

ふりかえり

Keep
- シミュレーターを使ったアルゴリズム調整
  描いた設計思想に忠実な実装
Problem
- 実機を走らせる時間（と空間）を十分確保できなかった
  電池まで本番と完全に同じものを使うべきだった

使い方

コース画像を準備する。
走行ロジックを書く。ラッパー(インタフェース)をたたくだけでOK
CPU Core2Duo以上、DirectX9～対応グラボなら3D表示しても実時間実行可能
　離散化⊿tの値を大きくすれば、計算は荒くなるが遅いPCでもサクサク動くはず。
コース上の指定した座標にPathfinderを配置してシミュレーションスタート

シミュレーターコードについて

インタフェース

BrickOSWrapper.h
実機とシミュレータでこのインタフェースを共通化してつかう

BrickOSWrapper.h

# pragma once
# include <vcclr.h>
namespace BrickOSWrapper{
class PathFinder{
private:
        static gcroot<Misawat::Etrobo::Simulator::PathFinder*> _pathFinder;
public:
        static void Set( Misawat::Etrobo::Simulator::PathFinder* pathFinder ){
                _pathFinder = pathFinder;
        }
        static Misawat::Etrobo::Simulator::PathFinder* pathFinder(){
                return _pathFinder;
        }
};
class LightSensorWrapper{
private:
        gcroot<Misawat::Etrobo::Simulator::LightSensor*> _lightSensor;
public:
        LightSensorWrapper(){
                _lightSensor = PathFinder::pathFinder()->GetLightSensor();
        }
        unsigned int get() const{
                return _lightSensor->Get();
        }
};
class TractionMotorWrapper{
private:
        gcroot<Misawat::Etrobo::Simulator::Motor*> _tractionMotor;
public:
        TractionMotorWrapper(){
                _tractionMotor = PathFinder::pathFinder()->GetTractionMotor();
        }
        void Forward( unsigned char s ) const{
                _tractionMotor->Foward(s);
        }
        void Reverse( unsigned char s ) const{
                _tractionMotor->Reverse(s);
        }
};
class SteeringMotorWrapper{
private:
        gcroot<Misawat::Etrobo::Simulator::Motor*> _steeringMotor;
public:
        SteeringMotorWrapper(){
                _steeringMotor = PathFinder::pathFinder()->GetSteeringMotor();
        }
        void Forward( unsigned char s ) const{
                _steeringMotor->Foward(s);
        }
        void Reverse( unsigned char s ) const{
                _steeringMotor->Reverse(s);
        }
};
};

走行ロジック（アルゴリズム）クラスの例

RunningMethodManager.cpp
走行ロジックのマネージャ

# include "StdAfx.h"
 #include "RunningMethodManager.h"
 // Normal
 #include "RunningMethod_Normal.h"
 RunningMethod::Normal* iRunningMethodNormal;
 // TODO-start: ここに各自のRunngingMethodクラスを追加
 // #include "RunningMethod_XXX.h"
 // iRunningMethodXXX* iRunningMethodXXX;
 // TODO-end:
 RunningMethodManager::RunningMethodManager()
 {
         // 光センサー値履歴管理モジュール生成
         iLightValueHistory   = new Common::LightValueHistory();
         iRunningMethodNormal = new RunningMethod::Normal();
         
         // TODO-start: ここに各自のRunngingMethodクラスを追加
         // iRunningMethodXXX = new iRunningMethodXXX();
         // TODO-end:
         
         // 閾値設定
         Common::LightValueHistory::setThreshold( 100,0, 50 );
 }
 RunningMethodManager::~RunningMethodManager()
 {
 }
 int RunningMethodManager::StartRunning()
 {
         bool         courseDetection = false;
         unsigned int latestValue = 0;
         // 光センサー値更新
         iLightValueHistory->UpdateLightValueHistory(&latestValue);
         // コース判別
         // TODO-start: ここに各自のRunngingMethodクラスを追加
         // courseDetection = iRunningMethodXXX->DetectCourse(iLightValueHistory);
         // if(true == courseDetection){
         //      iRunningMethodXXX->Run();
         // }
         // TODO-end:
         courseDetection = iRunningMethodNormal->DetectCourse(iLightValueHistory);
         if(true == courseDetection){
                 iRunningMethodNormal->Run();
         }
         return 1;
 }
 int RunningMethodManager::StopRunning()
 {
         return 1;
 }

RunningMethod_Normal.cpp*
状況ごとの走行ロジック（複数用意しても、１つでもＯＫ）

RunningMethod_Normal.cpp

 //==============================================================================
 // - 名前: 通常走行
 // - 機能: 難所などを含まない通常のカーブ・直線を走るロジックを持つ。
 //         - 灰色検出のためコースの外側をエッジ走行する。
 //           つまり、黒：右、白：左にステアリングを切る。
 //         - 一定角度以上ステアリングをきらないようにリミットを設ける。
 //==============================================================================
 #include "stdafx.h"
 #include "RunningMethod_Normal.h"
 using namespace Common;
 using namespace BrickOSWrapper;
 using namespace RunningMethod;
 #define ABS(x) (0<(x)?(x):-(x))
 Normal::Normal() :
         _currentDirection( DirectionRight ),
         _counter( 0 )
 {
         COUNTER_MAX    = 100;
         STEERING_SPEED = 255 / (LIGHT_HISTORY_MAX / 2);
 }
 Normal::~Normal()
 {
 }
 int Normal::Run()
 {
         _tractionMotor.Forward(100);
         if( _lightValueHistory->isWhite( _lightSensor.get() ) ){
                 Left();
         }else{
                 Right();
         }
         return 0;
 }
 void Normal::Right()
 {
         Count( DirectionRight );
         _steeringMotor.Forward( GetSpeed() );
 }
 void Normal::Left()
 {
         Count( DirectionLeft );
         _steeringMotor.Reverse( GetSpeed() );
 }
 void Normal::Count( Direction direction )
 {
         if( _currentDirection!=direction ){
                 _counter = 0;
         }
         else{
                 _counter ++;
         }
         _currentDirection = direction;
 }
 unsigned char Normal::GetSpeed()
 {
         if( _counter < COUNTER_MAX ){
                 return GetSpeedFromHistory();
         }
         else{
                 // ステアリングきりすぎ防止
                 return 0;
         }
 }
 unsigned char Normal::GetSpeedFromHistory()
 {
         // 白黒の割合によって、ステアリングスピードの変更
         return ABS(_lightValueHistory->GetWhiteRate()-LIGHT_HISTORY_MAX/2) * STEERING_SPEED;
 }
 bool Normal::DetectCourse(Common::LightValueHistory* lightValueHistory)
 {
         // runで使用するのでポインタを保持
         _lightValueHistory = lightValueHistory;
         // 通常走行なので必ずtrueを返す
         return true;
 }

シミュレータメイン

Simulation.cpp


# include "StdAfx.h"
# include "Simulation.h"
# include "BrickOSWrapper.h"
# include "RunningMethodManager.h"

Simulation::Simulation(System::Drawing::Graphics* myGraphics)
{
	_myGraphics=myGraphics;

}

Simulation::Simulation(System::Windows::Forms::Form* form)
{
	_myForm=form;
	_myGraphics=form->CreateGraphics();
}

Simulation::Simulation(void)
{
	//UNAVAILABLE
}

Simulation::~Simulation(void)
{
	//DO NOTHING
}

void Simulation::Stop(void)
{
	running=false;
}

void Simulation::Start()
{
	//[NOTICE]
	//Windows標準は、左上を(0,0)として右方向にX軸,下方向にY軸をとった座標系
	//であるので回転角度0°は下向き、回転は反時計回り

	//[HINT]
	//SimulatorPlatform(コースの画像データ,グラフィックコンテクスト,描画時の倍率)
	//単位は1pixel=1mm換算
	//コースの画像データの横幅が1000pxで倍率が0.6なら600pixelで描画されます

	//コース
	String* courseImageFile=".\\honban_course.png";
	
	_simulatorPlatform = __gc new SimulatorPlatform(courseImageFile, _myGraphics,0.001);
	PathFinder* pathFinder = _simulatorPlatform->CreatePathFinder(2400,500,90);
	PathFinder* pathFinder2 = _simulatorPlatform->CreatePathFinder(2400,300,90);


//	Misawat::Graphic::Misawa3D::M3DDeviceContext::SetHALMode(false);
	Misawat::Graphic::Misawa3D::M3DDeviceContext::SetDataDirectory(".");
	
	SimulatorPlatform3D* simulator3D = new SimulatorPlatform3D(courseImageFile, false,_myForm->Handle.ToInt32(),_myForm->ClientRectangle.Width,_myForm->ClientRectangle.Height);
	

	_myForm->MouseDown+=new System::Windows::Forms::MouseEventHandler(simulator3D, &Misawat::Etrobo::Simulator::SimulatorPlatform3D::MouseDown);
	_myForm->MouseUp+=new System::Windows::Forms::MouseEventHandler(simulator3D, &Misawat::Etrobo::Simulator::SimulatorPlatform3D::MouseUp);
	_myForm->MouseMove+=new System::Windows::Forms::MouseEventHandler(simulator3D, &Misawat::Etrobo::Simulator::SimulatorPlatform3D::MouseMove);

	simulator3D->AddPathFinder(pathFinder);
	simulator3D->AddPathFinder(pathFinder2);

	//パスファインダーに乗車した視点でシミュレータを実行する場合
	//simulator3D->SetOnVehicleMode(pathFinder);


	//ガジェットを配置する。
	simulator3D->AddGadget("etrobo_moai.x", NULL, 3750, 1, 2040, 0);
	simulator3D->AddGadget("etrobo_gate.x", "marble.bmp", 3280, 1, 2600, 83);


	//パスファインダーのボディの色を指定する。
	pathFinder->SetBodyColor(System::Drawing::Color::FromArgb(200,255,0,0));

	// Wrapper用にパスファインダをセットする
	BrickOSWrapper::PathFinder::Set( pathFinder );

	//pathFinder用走行ロジックオブジェクトを生成する
	RunningMethodManager* runningMethodManager=__gc new RunningMethodManager();

	//画面全体を描画する
	//_simulatorPlatform->PaintAll();
	_simulatorPlatform->DisableDraw(true);
	running=true;

	//パスファインダーの制御ループ
	while(running)
	{
		System::Windows::Forms::Application::DoEvents();

		//シミュレータの時間を0.03sec(30msec)を進める
		//ここは実機の制御周期(1ループにかかる時間）に即した値を入れる
		_simulatorPlatform->Update(0.03);
		simulator3D->Update();
		//走行計算を行う
		runningMethodManager->StartRunning();
	}
}

(以前はフルソースコードを公開していましたが、10年以上前でビルドも通らない可能性があるので、需要があればまた公開検討します^^)

まとめ

2007年に参加したETロボコンと、そのために作ったETロボコンシミュレーター開発の記録をアーカイブとして掲載しました
2019年現在とはだいぶ異なる手法でシミュレータを開発しましたが、とてもエキサイティングな経験となりました。

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up