- 追加された行はこの色です。
- 削除された行はこの色です。
#contents
*課題について [#f729ca33]
*ロボットの説明 [#h1a6f92b]
**ロボット全体 [#f4876158]
**ロボットの改良 [#yaaaca9a]
*プログラムの説明 [#s30e530a]
**基本的な定義 [#y54b4306]
***定数の定義 [#mc5e2d5a]
#define CROSS_TIME 40
#define BALANCE_TIME 40
- CROSS_TIME:交差点判別時間
~ 黒部分がCROSS_TIMEより続いたら交差点として判別する。
- BALANCE_TIME:姿勢制御用時間
~ 境界部分がBALANCE_TIMEより続いたら機体がまっすぐとして判別する。
***マクロの定義 [#ca9f59fc]
基本動作のマクロです。
#define roll_left OnRev(OUT_A); OnFwd(OUT_C); //左回転
#define turn_left OnFwd(OUT_C); Off(OUT_A); //左旋回
#define go OnFwd(OUT_A); OnFwd(OUT_C); //前進
#define turn_right OnFwd(OUT_A); Off(OUT_C); //右旋回
#define roll_right OnFwd(OUT_A); OnRev(OUT_C); //右回転
#define back OnRev(OUT_A); OnRev(OUT_C); Wait(200); Off(OUT_AC); //後進
#define back OnRev(OUT_A); OnRev(OUT_C); Wait(100); Off(OUT_AC); //後進
交差点判別後のマクロです。
#define top OnRev(OUT_C); OnFwd(OUT_A); Wait(CROSS_TIME); OnFwd(OUT_AC); Wait(10);
#define left OnRev(OUT_A); OnFwd(OUT_C); Wait(20);
#define right OnRev(OUT_C); OnFwd(OUT_A); Wait(80); OnFwd(OUT_AC); Wait(20);
#define bottom OnRev(OUT_C); OnRev(OUT_AC); Wait(100); Off(OUT_AC);
- #define top:前進
#ref(2016a/Member/kurokouchi/Mission2/08.png,100%,01)
角度を戻してそのまま直進する。
- #define left:左折
#ref(2016a/Member/kurokouchi/Mission2/07.png,100%,01)
角度を戻してそのままライントレースさせる。
- #define right:右折
#ref(2016a/Member/kurokouchi/Mission2/09.png,100%,01)
角度を右寄りにして外側の線に機体を移動する。
**ライントレース [#y54b4306]
今回の課題ではライントレースでの移動をスムーズにするために、左右の境界部分のライントレースプログラムを作成した。
今回の課題では正確に各地点へ移動するために、左の境界部分のライントレースプログラムを作成した。
***光センサーでの色判別について [#l08c4391]
- 白部分・・・・・光センサーの値:53以降
- 弱白部分・・・光センサーの値:52以降〜53以下
- 境界部分・・・光センサーの値:44以降〜52以下
- 弱黒部分・・・光センサーの値:41以降〜44以下
- 黒部分・・・・・光センサーの値:41未満
***タイマーについて [#t320a82e]
- FastTimer(0):交差点判別用のタイマー
~ 光センサーが黒部分の判定をしている時にカウントをして、0.4秒以上続いたら交差点として判別する。また、黒部分以外の時はClearTimer(0)でFastTimer(0)を初期化する。
- FastTimer(1):交差点誤判別防止用のタイマー
~ 小さなカーブを交差点として誤判別しないために指定秒数間交差点判別を行わないようにする。ここで、指定秒数を変数としてl_timeをグローバル変数として定義する。また、ライントレース開始時にClearTimer(1)でFastTimer(1)を初期化する。
***左境界部分のトレース [#d1357496]
sub line_l(){
int ch=0; //ループ脱出用変数
ClearTimer(0); //交差点判別用のタイマー初期化
ClearTimer(1); //交差点誤判別防止用のタイマー初期化
while(ch==0) //chが0の時はループを続ける
{
if(SENSOR_2 > 53) //白部分 右回転
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
roll_right;
}
else if(SENSOR_2 > 52) //弱白部分 右旋回
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
turn_right;
}
else if(SENSOR_2 > 44) //境界部分 直進
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
go;
}
else if(SENSOR_2 > 41) //弱黒部分 左旋回
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
turn_left;
}
else{ //黒部分 左回転
roll_left;
}
if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time){ //交差点判別
Off(OUT_AC); //車輪を止める
PlaySound(SOUND_UP); //音を鳴らす
Wait(100); //1秒待つ
ch=1; //ループ脱出の為にchを1にする
}
Wait(1); //0.01秒待つ
}
}
***右境界部分のトレース [#o8e077a8]
sub line_r(){
int ch=0; //ループ脱出用変数
ClearTimer(0); //交差点判別用のタイマー初期化
ClearTimer(1); //交差点誤判別防止用のタイマー初期化
while(ch==0) //chが0の時はループを続ける
{
if(SENSOR_2 > 53) //白部分 左回転
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
roll_left;
}
else if(SENSOR_2 > 52) //弱白部分 左旋回
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
turn_left;
}
else if(SENSOR_2 > 44) //境界部分 直進
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
go;
}
else if(SENSOR_2 > 41) //弱黒部分 右旋回
{
ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
turn_right;
}
else{ //黒部分 右回転
roll_right;
}
if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time){ //交差点判別
Off(OUT_AC); //車輪を止める
PlaySound(SOUND_UP); //音を鳴らす
Wait(100); //1秒待つ
ch=1; //ループ脱出の為にchを1にする
}
Wait(1); //0.01秒待つ
}
}
**交差点の判別 [#s1bbda60]
上記のライントレース用プログラム内の交差点判別部分について詳しく説明する。
if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time){ //交差点判別
if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time) //交差点判別
{
Off(OUT_AC); //車輪を止める
PlaySound(SOUND_UP); //音を鳴らす
Wait(100); //1秒待つ
ch=1; //ループ脱出の為にchを1にする
}
交差点は黒部分が長く続いた時に交差点として判別するようにした。そこでタイマーを使い黒部分以外の時は以下の関数でタイマーを初期化するようにした。また交差点と判別したことが分かるように、判別したら音を鳴らすようにした。
ClearTimer(0);
タイマーがCROSS_TIMEより長い時間が経過した時に交差点として判別する。CROSS_TIMEは40とする。
if(FastTimer(0)>CROSS_TIME)
しかし、上記のプログラムではカーブを交差点として誤判別してしまう事があるので、それを防ぐためにタイマーをもう一つ追加して次の指定秒数をカウントしなければ交差点と判別しないようにした。
if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time)
l_timeを上記の指定秒数として、メインプログラムで指定する。
**姿勢制御プログラム [#a7ef2032]
C,Dに置いたコップを正確に取るために、ラインを使い機体の姿勢をまっすぐにするプログラムを作成しました。
***タイマーについて [#uae84032]
- FastTimer(0):姿勢判別用のタイマー
~ 光センサーが境界部分の判定をしている時にカウントをして、タイマーがBALANCE_TIME以上になったら機体の姿勢をまっすぐとして判別する。また、境界部分以外の時はClearTimer(0)でFastTimer(0)を初期化する。
- FastTimer(1):ライントレース時間判別用のタイマー
~ ライントレースをする時間を指定しなければ、姿勢がまっすぐにならない場合ライントレースをし続けてしまい、C,Dに置いてあるコップをとることが出来なくなってしまうので追加しました。また、姿勢制御プログラム開始時にClearTimer(1)でFastTimer(1)を初期化する。
~ ライントレースをする時間を指定しなければ、姿勢がまっすぐにならない場合ライントレースをし続けてしまい、C,D地点に置いてあるコップをとることが出来なくなってしまうので追加しました。また、姿勢制御プログラム開始時にClearTimer(1)でFastTimer(1)を初期化する。
***姿勢制御の方法 [#g5f4ea3c]
#ref(2016a/Member/kurokouchi/Mission3/01.gif,nolink,left,around);
姿勢制御プログラムは、交差点判別プログラムを応用して作成しました。
交差点判別プログラムとの違いは、黒部分の判定ではなく境界部分の判定であることです。
また、ラインの長さは限られているので、一定時間が経過しても姿勢がまっすぐにならなければ、一度処理を止めて後退してからまた姿勢制御プログラムを開始するようにしました。
このプログラムにより、C,D地点にますっぐ侵入することが出来るようになりました。
#clear
***左境界部分での姿勢制御 [#la2de7ff]
sub balance_l(){
int ch=0; //ループ脱出用変数
ClearTimer(0); //姿勢判別用のタイマー初期化
while(ch==0) //chが0の時はループを続ける
{
ClearTimer(1); //ライントレース時間判別用のタイマー初期化
while(FastTimer(1)<200&&ch==0)
{
if(SENSOR_2 > 53) //白部分 右回転
{
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
roll_right;
}
else if(SENSOR_2 > 52) //弱白部分 右旋回
{
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
turn_right;
}
else if(SENSOR_2 > 44) //境界部分 直進
{
go;
}
else if(SENSOR_2 > 41) //弱黒部分 左旋回
{
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
turn_left;
}
else{ //黒部分 左回転
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
roll_left;
}
if(FastTimer(0)>BALANCE_TIME) //交差点判別
if(FastTimer(0)>BALANCE_TIME) //姿勢判別
{
Off(OUT_AC); //車輪を止める
PlaySound(SOUND_UP); //音を鳴らす
Wait(100); //1秒待つ
ch=1; //ループ脱出の為にchを1にする
}
Wait(1); //0.01秒待つ
}
if(ch==0)
{
back; //後退
back; //約2秒分の後退
back;
}
}
}
***右境界部分での姿勢制御 [#oa1e2b0b]
sub balance_r(){
int ch=0; //ループ脱出用変数
ClearTimer(0); //姿勢判別用のタイマー初期化
while(ch==0) //chが0の時はループを続ける
{
ClearTimer(1); //ライントレース時間判別用のタイマー初期化
while(FastTimer(1)<200&&ch==0)
{
if(SENSOR_2 > 53) //白部分 左回転
{
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
roll_left;
}
else if(SENSOR_2 > 52) //弱白部分 左旋回
{
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
turn_left;
}
else if(SENSOR_2 > 44) //境界部分 直進
{
go;
}
else if(SENSOR_2 > 41) //弱黒部分 右旋回
{
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
turn_right;
}
else{ //黒部分 右回転
ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
roll_right;
}
if(FastTimer(0)>BALANCE_TIME) //交差点判別
if(FastTimer(0)>BALANCE_TIME) //姿勢判別
{
Off(OUT_AC); //車輪を止める
PlaySound(SOUND_UP); //音を鳴らす
Wait(100); //1秒待つ
ch=1; //ループ脱出の為にchを1にする
}
Wait(1); //0.01秒待つ
}
if(ch==0)
{
back; //後退
back; //約2秒分の後退
back;
}
}
}
***姿勢判別について [#i9445880]
上記の姿勢制御プログラム内の姿勢判別部分について詳しく説明する。
if(FastTimer(0)>BALANCE_TIME) //姿勢判別
{
Off(OUT_AC); //車輪を止める
PlaySound(SOUND_UP); //音を鳴らす
Wait(100); //1秒待つ
ch=1; //ループ脱出の為にchを1にする
}
このプログラムでは、境界部分が長く続いた時に機体がますっぐであると判別するようにした。
そこでタイマーを使い境界部分以外の時は以下の関数でタイマーを初期化するようにした。また機体がますっぐであると判別したことが分かるように、判別したら音を鳴らすようにした。
ClearTimer(0);
タイマーがBALANCE_TIME(0.4秒)より長い時間が経過した時に機体がますっぐであると判別する。
if(FastTimer(0)>BALANCE_TIME)
また、機体が一定時間(2秒)経過してもまっすぐにならない場合、一度後退してから姿勢制御プログラムを再開する。
if(ch==0) //姿勢がまっすぐと判別された場合は後退しない。
{
back; //約2秒分の後退
back;
}
**コップ取得について [#o0a89cab]
今回の課題で最も重要であるコップの取得プログラムです。
***コップ取得方法 [#cd850069]
#ref(2016a/Member/kurokouchi/Mission3/02.gif);
コップを二つのアームの間に挟んでからアームを上げることによってコップを持ち上げて機体の手前に置くことが出来ます。同じ動作で最大8つまでのコップを重ねることが出来ます。
光センサーを使いコップを認識したら前進を停止します。
コップを二つのアームの間に入れなくてはいけないので、上記の姿勢制御プログラムが重要です。
#ref(2016a/Member/kurokouchi/Mission3/02.gif,nolink,left);
***コップ取得プログラム [#u348530d]
sub cup(){
OnRev(OUT_B); //アームを降ろす
OnRev(OUT_B); //アームを下す
Wait(50);
Off(OUT_B);
OnFwd(OUT_AC); //前進
while(SENSOR_1<42); //光センサーがコップを感知するまで前進する。
while(SENSOR_1<42); //光センサーがコップを認識するまで前進する
Off(OUT_AC); //停止
OnFwd(OUT_B); //アームを挙げる
OnFwd(OUT_B); //アームを上げる
Wait(50);
Off(OUT_B);
}
***C地点からD地点への移動 [#mabbd401]
**最後の交差点の判別 [#v23d196e]
ゴール地点の四角の中に入るために、最後の交差点を判別したら以下の処理を実行する。
C地点のコップを取得後、左回転し右境界部分で姿勢制御を開始する。
#ref(2016a/Member/kurokouchi/Mission3/03.gif,nolink,left,around);
赤い点:光センサー
#clear
***D地点からC地点への移動 [#y048567e]
D地点のコップを取得後、後退をし交差点を超えてから左回転し右境界部分で姿勢制御を開始する。
#ref(2016a/Member/kurokouchi/Mission3/04.gif,nolink,left,around);
赤い点:光センサー
#clear
また右回転ではなく左回転をする理由は、左側にはアームだけしかストッパーがなくコップが転倒する可能性が上がるたので、左回転にした。
***左回転プログラム [#gf5e92ce]
sub turn(){
OnFwd(OUT_C); //左回転
OnRev(OUT_A);
Wait(100); //黒部分から白部分に出るための時間(D地点からC地点への移動時に特に必要)
while(SENSOR_2>41); //白部分である場合は回転し続ける
Off(OUT_AC); //停止
}
光センサーで判別して回転するので、電池の消費による回転不足をなくすことができた。
**Q地点へのコップの設置 [#v23d196e]
***Q地点での動き [#c03fa70a]
設置場所の輪の中へコップを置くために、最後の交差点を判別したら以下の処理を実行する。
#ref(2016a/Member/kurokouchi/Mission3/05.gif,nolink,left,around);
1:交差点判別後、機体の姿勢を戻さずにそのまま前進する。
2:前進停止後、アームを下して左側のストッパーをなくす。
3:右回転させ、コップを外に出す。
4:後進させ、コップが機体に触れないようにする。
#clear
***設置プログラム [#zbc8d2e8]
sub last(){
OnRev(OUT_C); //姿勢をまっすぐにする
OnFwd(OUT_A);
Wait(40);
OnFwd(OUT_AC); //前進
Wait(200); //2秒間待つ
Off(OUT_AC); //停止
ClearTimer(0); //交差点判別用のタイマー初期化(初期化しなくてもよいが最後なので一応初期化)
ClearTimer(1); //交差点誤判別防止用のタイマー初期化(初期化しなくてもよいが最後なので一応初期化)
OnFwd(OUT_AC); //前進
Wait(50); //約0.5秒待つ
Off(OUT_AC);
OnRev(OUT_B); //アームを下す
Wait(50); //約0.5秒待つ
Off(OUT_B);
OnFwd(OUT_A); //右回転
OnRev(OUT_C);
Wait(100); //約1秒待つ
back; //約2秒間後進する(1秒ではアームがコップに触れている時があるため)
back;
}
**メインプログラム [#r51b8f1f]
task main ()
{
SetSensor(SENSOR_1, SENSOR_LIGHT); //光センサー1の初期設定(紙コップ認識用)
SetSensor(SENSOR_2, SENSOR_LIGHT); //光センサー2の初期設定(ライントレース用)
OnFwd(OUT_AC); //スタート地点の枠から脱出
Wait(60);
SetSensor(SENSOR_2, SENSOR_LIGHT);
//C地点までの移動
line_l(); //左側境界部分をトレース
left; //P地点を左折
l_time=400; //次の交差点までの秒数を4秒に設定
line_l(); //左側境界部分をトレース
right; //S地点を右折
balance_l(); //左境界部分での姿勢制御
l_time=300; //次の交差点までの秒数を3秒に設定
line_l(); //左側境界部分をトレース
left(); //S地点を左折
line_l();
left(); //P地点を左折
line_l();
top(); //Q地点を直進
l_time=700; //交差点誤判別防止用秒数を7秒に設定
line_l();
top(); //Q地点を直進
l_time=300; //交差点誤判別防止用秒数を3秒に設定
line_l();
right(); //R地点を右折
line_r(); //右側境界部分をトレース
left(); //P地点を左折
l_time=700; //交差点誤判別防止用秒数を7秒に設定
line_l();
last();
//紙コップ回収作業
cup(); //1つ目の紙コップ取得
back; //後進
//D地点までの移動
turn(); //左回転
balance_r(); //右境界部分での姿勢制御
//紙コップ回収作業
cup(); //2つ目の紙コップ取得
back; //後進
//C地点までの移動
turn(); //左回転
OnFwd(OUT_AC); //前進
Wait(20);
balance_r(); //右境界部分での姿勢制御
//紙コップ回収作業
cup(); //3つ目の紙コップ取得
back; //後進
//D地点までの移動
turn(); //左回転
OnFwd(OUT_AC); //前進
Wait(20);
balance_r(); //右境界部分での姿勢制御
//紙コップ回収作業
cup(); //4つ目の紙コップ取得
back; //約4秒間後進
back;
back;
back;
l_time=200; //次の交差点までの秒数を2秒に設定
//左境界部分へ移動
OnFwd(OUT_C); //左回転
OnRev(OUT_A);
Wait(50);
OnFwd(OUT_AC); //前進
Wait(30);
//Q地点までの移動
line_l(); //左側境界部分をトレース
left; //S地点を左折
line_l(); //左側境界部分をトレース
left; //P地点を左折
line_l(); //左側境界部分をトレース
//紙コップの設置
last();
}
*課題結果 [#i72ce040]
課題コースはC地点 → S左折 → P左折 → Q直進 → Q直進 → R右折 → P左折 → A地点のコースです。
#ref(2016a/Member/kurokouchi/Mission2/05.png,100%,01)
C地点からA地点までのタイムは1分10秒でした。
#ref(2016a/Member/kurokouchi/Mission2/05.png,50%,01)
*反省・感想 [#af5aefdb]
C地点からA地点までのタイムは1分10秒で到達タイムが1分切れなかったことが残念でした。しかし、到達率がほぼ10割で、安定性のあるプログラムが書けたことはよかったです。
今回の課題から光センサーを使い始めたので一つ一つの動作を時間指定しなくてよくなった部分があり電池の残量などによる動作の不安定さがなくなった分、プログラムが複雑になったと感じました。
また、機体の都合で増やした処理もあったのでロボット本体にもう少し工夫した方が良かったと思いました。次回の課題ではプログラムだけではなくロボットにも工夫したいです。