1. ETロボコンシミュレーションの環境構築
まず、ETロボコンシミュレーションの環境の構築を行います。
こちらを参考にしてください。
2. mryby-on-ev3rt+tecsプラットフォーム
以下のリンクからダウンロードできます。
mruby-on-ev3rt+tecs
3 TECS CDL記述
次はathrill側での操作の説明です。
まず新しくsdk2というディレクトリを作成し、sdkの中身をコピーしておきます。
3.1 tMotor.cdl,tColorSensor.cdlのコピー
ロボットを動かすために用いるcdlファイルです。
これらをmryby-on-ev3rt+tecsからathrill側に持ってきます。
詳しく説明すると
mruby-on-ev3rt+tecs2.2.0\hr-tecs\tecs_lib
にあるtMotor.cdlおよびtColorSensor.cdlを
ev3rt-athrill-v850e2m/sdk2/common/ev3api/src
にコピーします。
3.2 tMotor.cdl
コピーしてきたtMotor.cdlを書き加えます。
import_C("ev3api_motor.h");
signature sMotor{
ER setPower([in]int power);
void initializePort([in]int32_t type);
};
signature sControlMotors
{
ER steer([in]int power, [in]int turn_ratio);
};
celltype tMotor
{
entry sMotor eMotor;
attr{
int32_t port;
};
};
[singleton]
celltype tControlMotors
{
call sMotor cLeftMotor;
call sMotor cRightMotor;
entry sControlMotors eControlMotors;
};
まず、Left・RightMotorそれぞれのパワーを決める関数setPowerとポートのタイプを決める関数initializePortのみ有効にします。
ControlMotorsのシグニチャ記述
signature sControlMotors{
ER steer([in]int power, [in]int turn_ratio);
void initializePort([in]int32_t type);
};
ロボットのステアリングを制御する関数steerを定義し、引数をpowerとturn_ratioの2つだけにします。
セルタイプtControlMotorsのセルタイプ記述
[singleton]
celltype tControlMotors
{
call sMotor cLeftMotor;
call sMotor cRightMotor;
entry sControlMotors eControlMotors;
};
今までと同様に受け口と呼び口を定義します。
[singleton]でなくても構いません。
3.3 athrill_common.cdl
TECS CDL記述をsdk2/common/athrill_common.cdl
に追加します。
今回は以下のコンポーネント図でロボットを動かしていきます。
+--------------+
| tColorSensor |
+----------------------->| ColorSensor |
| +--------------+
|
| +-----------+
+----------+ +--------------+ | +----------------+ | tMotor |
| tTask | | tControlMain |- | tControlMotors |--->| LeftMoter |
| |--->| |--->| | +-----------+
| Task | | ControlMain | | ControlMotors |--->+-----------+
+----------+ +--------------+ +----------------+ | tMotor |
| RightMotor|
+-----------+
追加するコードがこちらです。
cell tTask Task
{
cTaskBody = ControlMain.eBody;
stackSize = 81920;
priority = 10;
attribute = C_EXP("TA_ACT");
};
celltype tControlMain
{
entry sTaskBody eBody;
call sControlMotors cControlMotors;
call sColorSensor cColorSensor;
};
cell tControlMain ControlMain
{
cControlMotors = ControlMotors.eControlMotors;
cColorSensor = ColorSensor.eColorSensor;
};
cell tControlMotors ControlMotors
{
cLeftMotor = LeftMotor.eMotor;
cRightMotor = RightMotor.eMotor;
};
cell tMotor LeftMotor
{
port = C_EXP("EV3_PORT_A");
};
cell tMotor RightMotor
{
port = C_EXP("EV3_PORT_B");
};
cell tColorSensor ColorSensor
{
port = C_EXP("EV3_PORT_1");
};
この時、tMotor.cdl, tColorSensor.cdlへのパスも加えます。
import("sdk2/common/ev3api/src/tMotor.cdl");
import("sdk2/common/ev3api/src/tColorSensor.cdl");
セルTaskの組み上げ記述
cell tTask Task
{
cTaskBody = ControlMain.eBody;
stackSize = 81920;
priority = 10;
attribute = C_EXP("TA_ACT");
};
組み上げ記述はセル同士の結合関係を定義し、アプリケーションを構築するために用います。
今回だと、セルTaskの呼び口cTaskBodyがセルControlMainの受け口Bodyと結合しているという意味です。
**attribute = C_EXP("TA_ACT")**はOS起動時にタスクを起動状態にしています。
セルTaskは以下にあります。
import(<kernel.cdl>)
セルタイプtControlMainのセルタイプ記述
celltype tControlMain
{
entry sTaskBody eBody;
call sControlMotors cControlMotors;
call sColorSensor cColorSensor;
};
セルタイプ記述は呼び口、受け口、属性、変数を用いてセルタイプを定義します。
今回は、シグニチャsTaskBodyの受け口をeBody、
シグニチャsControlMotors, sColorSensor
の呼び口をそれぞれcControlMotors、cColorSensorと定義している。
セルControlMainの組み上げ記述
cell tControlMain ControlMain
{
cControlMotors = ControlMotors.eControlMotors;
cColorSensor = ColorSensor.eColorSensor;
};
セルControlMainの呼び口cControlMotorsがセルControlMotorsの受け口eControlMotorsと結合している。呼び口cColorSensorも同様です。
セルControlMotorsの組み上げ記述####
cell tControlMotors ControlMotors
{
cLeftMotor = LeftMotor.eMotor;
cRightMotor = RightMotor.eMotor;
};
上記と同様に呼び口と受け口の結合を表しています。
セルLeftMotor,RightMotor,ColorSensorの組み上げ記述
cell tMotor LeftMotor
{
port = C_EXP("EV3_PORT_A");
};
cell tMotor RightMotor
{
port = C_EXP("EV3_PORT_B");
};
cell tColorSensor ColorSensor
{
port = C_EXP("EV3_PORT_1");
}
ここで、**C_EXP("~")**はTECSジェネレータ上では~がただの文字列として扱われるように命令している。
今回の場合では、athrill側のCファイルでEV3_PORT_A,B,1を定義する記述に出力させ、ジェネレータでは処理せずC言語のコンパイラに任せている。(コンパイルの際にはしっかり有効になる)
また、C_EXPを用いる理由としては、コードの見栄えや定義を再度理解させるのが面倒だからである。
C_EXPRESSIONの略です。
4 Makefile.app
common/Makefile.appの34, 42, 56, 61行目のappをtControlMainに書き換えます。
5 app.cfg
workspace/single-robot/line_trace/app.cfgの
INCLUDE("app_common.cfg");
INCLUDE("tecsgen.cfg");
この2つ以外のコードを削除します。(削除したコードはC++用なので)
削除後、ファイル名をtControlMain,cfgとしてathrillsampleにコピー。
6 Makefile.img
common/Makefile.imgの236~241行目を#で無効にする。
APPL_ASMOBJS :=
ifdef USE_CXX
APPL_CXXOBJS := @(APPLOBJS)
APPL_COBJS +=
else
APPL_COBJS += @(APPLOBJS)
endif
この操作はControMain.cの2重定義を解消しています。
7 Makefile
workspace/Makefileの12行目に
-include ../OBJ/gen/Makefile.tecsgen
を加えます。
8 TECSジェネレータ
TECSジェネレータを実行してテンプレートコードを生成します。
TECSジェネレータの詳しい説明はこちらから。
一旦、こちらを参考にETロボコン制御プログラムをビルドしてください。
このビルド過程の中でTECSジェネレータが実行されるので、
aev3rt-athrill-v850e2m/sdk2/common/OBJ/gen
にテンプレートコードが生成されます。
また、今回のビルドでは以下のエラーが出てしまいますが、この後に説明するTECSマージによって解消できます。
9 TECSマージ
TECSジェネレータの生成したテンプレートコードからセルタイプコードを生成します。
TECSマージの詳しい説明はこちらから。
テンプレートコードのtControlMain_templ.cを
ev3rt-athrill-v850e2m/sdk2/workspace/アプリケーションフォルダ名に
tControlMotors_templ.c,tMotor_templ.cおよびtColorSensor_templ.cを
ev3rt-athrill-v850e2m/sdk2/common/ev3api/src
にセルタイプコードとして生成させます。
$ruby tecsmerge.rb 対象のテンプレートファイル セルタイプコード生成場所
10 テンプレートコード
生成されたセルタイプコードにコンポーネントの振る舞いを記述します。
tMotor.c
src/ev3api_motor.cからev3rt_motor_set_powerおよびev3_motor_configをコピー。
戻り値として追加し、引数を属性に合わせ変更します。
ER
eMotor_setPower(CELLIDX idx, int power)
{
ER ercd = E_OK;
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
return ev3_motor_set_power(ATTR_port, power);
}
void
eMotor_initializePort(CELLIDX idx, int32_t type)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
else {
}
return ev3_motor_config(ATTR_port,type);
}
tControlMotors.c
src/ev3api_motor.cから関数ev3_motor_steerの中身をコピー。
ER
eControlMotors_steer(int power, int turn_ratio)
{
cLeftMotor_initializePort( LARGE_MOTOR );
cRightMotor_initializePort( LARGE_MOTOR );
int left_power;
int right_power;
int abs_turn_ratio = (turn_ratio < 0) ? -turn_ratio : turn_ratio;
int abs_power = (power < 0) ? -power : power;
if (abs_turn_ratio > 100) {
abs_turn_ratio = 100;
}
if (abs_power > 100) {
abs_power = 100;
}
left_power = abs_power;
right_power = abs_power;
if (turn_ratio > 0) {
right_power = (abs_power * (100 - abs_turn_ratio)) / 100 ;
}
else {
left_power = (abs_power * (100 - abs_turn_ratio)) / 100 ;
}
if (power < 0) {
left_power = left_power * (-1);
right_power = right_power * (-1);
}
cLeftMotor_setPower(left_power);
cRightMotor_setPower(right_power);
return E_OK;
}
cLeftMotor_initializePort( LARGE_MOTOR ); cRightMotor_initializePort( LARGE_MOTOR );
を追加し、モータのタイプをLARGE_MOTORに。
引数がpowerおよびturn_ratioのみになったので、CHECK_から始まる関数は削除。
ev3_Motor_set_power名をTECS CDL記述に合わせ、引数をpowerだけに変更。
関数の名前ですが、呼び口cLeftMotorがセルLeftMotorのsetPowerを利用するようなイメージ。
tColorSensor.c
colorid_t
eColorSensor_getColor(CELLIDX idx)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
return ev3_color_sensor_get_color(ATTR_port);
}
uint8_t
eColorSensor_getReflect(CELLIDX idx)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
return ev3_color_sensor_get_reflect(ATTR_port);
}
uint8_t
eColorSensor_getAmbient(CELLIDX idx)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
return ev3_color_sensor_get_ambient(ATTR_port);
}
void
eColorSensor_initializePort(CELLIDX idx)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
ev3_sensor_config(ATTR_port, COLOR_SENSOR);
}
void
eColorSensor_getRGBRaw(CELLIDX idx, uint16_t* r, uint16_t* g, uint16_t* b)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
rgb_raw_t val;
ev3_color_sensor_get_rgb_raw(ATTR_port, &val);
(*r) = val.r; (*g) = val.g; (*b) = val.b;
}
src/ev3api_sensor.cからtColorSensor.cにある関数に対応する関数をコピーし、引数を属性に合わせて変更。
tControlMain.c
void
eBody_main(CELLIDX idx)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) {
p_cellcb = GET_CELLCB(idx);
}
else {
}
cColorSensor_initializePort( );
while(1) {
# define LIGHT_BRIGHT
# ifdef LIGHT_DARK
# define white 78
# define black 20
# else
# define white 100
# define black 50
# endif
static float lasterror = 0, integral = 0;
static float midpoint = (white - black) / 2 + black;
{
float error = midpoint - cColorSensor_getReflect();
# ifdef LIGHT_BRIGHT
integral = error + integral * 0.01;
float steer = 0.9 * error + 0.1 * integral + 1 * (error - lasterror);
# else
integral = error + integral * 0.05;
float steer = 0.7 * error + 0.1 * integral + 1 * (error - lasterror);
# endif
cControlMotors_steer(10, steer);
lasterror = error;
}
tslp_tsk(100000); /* 100msec */
}
workspace/athrillsample/app.cから
main_taskの中身をコピーして、main_task内の1行~syslogは必要ないので削除します。
cColorSensor_initializePort( )を代わりに置きます。
次に、ev3_color_sensor_get_reflectをTECS CDL記述に合わせて書き直し、引数を無くします。
さらに、ev3_motor_steerもTECS CDL記述に合わせて書き直し、引数をpowerとturn_ratioのみにします。
11 Unityのシミュレーションの開始
10まで終わればもう一度$make img=<アプリケーションフォルダ名>して終了です。
シミュレーションはこちらを参考にしてください。
参考にしているWebサイトのUnityパッケージではなくこちらのバージョンを使用してください。(参考サイトで説明されているScriptが見つからないため)
— 下村遼太 (@mlBIlXK8fUiDlGv) July 26, 2020