ロボティクス入門ゼミ
- このロボットは、左の班の課題3で使ったドライブベースを使って、缶を運ぶためのアームを取り付けました。
- このアームを動かすために使用したモーターの軸がアームのギアにうまく絡まなかったために、その間に中継するためのギアを組み込んで見ました。
- アームのところのギアを空回りができるものにしたので、余分な力がかかってもその場で空回りをしてくれるので、アームを上げたままの状態が保てるようにしてあります。
- アームの上の部分に小さなタイヤをつけることで缶をキャッチしたときの滑り止めにしました。(これは松本先生のアイデアをもとに取り付けました。)
- ゴールをしたかを判定するために、下の部分にタッチセンサーをつけました。
- これを階段状にしたことで、缶がゴールと機体にはさまれて、階段状のセンサーによって持ち上げられてゴールの中に入るようにしました。
- なお、タッチセンサーは缶には反応しない高さに取り付けてあります。ライトセンサーは、ライントレースのプログラムの都合上、ラインをはさむ形式にしました。
プログラム†
int t; //変数tを指定
//------サブルーチンの指定-----
// (1)直進トレース
sub line_straight()
{
PlaySound(SOUND_DOUBLE_BEEP) ;
OnRev(OUT_A+OUT_C);
Wait(50);
OnRev(OUT_A+OUT_C);
while(SENSOR_1>35||SENSOR_2>45)
{
if(SENSOR_1>35&&SENSOR_2>45)
{
OnRev(OUT_A+OUT_C);
}
if(SENSOR_1<35&&SENSOR_2>45)
{
OnRev(OUT_C);Off(OUT_A);
}
if(SENSOR_1>35&&SENSOR_2<45)
{
OnRev(OUT_A);Off(OUT_C);
}
}
Off(OUT_A+OUT_C);
Wait(50);
}
// (2)バック
sub line_back()
{
PlaySound(SOUND_DOUBLE_BEEP) ;
OnFwd(OUT_A+OUT_C);
Wait(260);
Off(OUT_A+OUT_C);
Wait(50);
}
sub go_straight()
{
PlaySound(SOUND_LOW_BEEP) ;
OnFwd(OUT_C);OnRev(OUT_A);Wait(10);
OnFwd(OUT_A);OnRev(OUT_C);Wait(10);
OnRev(OUT_A+OUT_C);Wait(50);
OnRev(OUT_A+OUT_C);
until(SENSOR_1<35&&SENSOR_2<45);
Off(OUT_A+OUT_C);
Wait(50);
}
// (4)ゴール
sub goal()
{
PlaySound(SOUND_FAST_UP) ;
OnRev(OUT_A+OUT_B+OUT_C);
Wait(50);
OnRev(OUT_A+OUT_B+OUT_C);
while(SENSOR_3==0)
{
if(SENSOR_1>35&&SENSOR_2>45)
{
OnRev(OUT_A+OUT_B+OUT_C);
}
if(SENSOR_1<35&&SENSOR_2>45)
{
OnRev(OUT_B+OUT_C);Off(OUT_A);
}
if(SENSOR_1>35&&SENSOR_2<45)
{
OnRev(OUT_A+OUT_B);Off(OUT_C);
}
}
Off(OUT_A+OUT_B+OUT_C);
}
//------マクロの指定(改行してある)-------------
// (5)左回転
#define turn_left(t)
PlaySound(SOUND_UP);
OnRev (OUT_A+OUT_C);Wait(50);
OnFwd(OUT_A);OnRev(OUT_C);Wait(t);
Off(OUT_A+OUT_C);
OnFwd(OUT_C);OnRev(OUT_A);Wait(10);
OnFwd(OUT_A);OnRev(OUT_C);Wait(10);
Off(OUT_A+OUT_C);Wait(50);
// (6)右回転
#define turn_right(t)
PlaySound(SOUND_UP) ;
OnRev(OUT_A+OUT_C);Wait(50);
OnFwd(OUT_C);OnRev(OUT_A);Wait(t);
Off(OUT_A+OUT_C);
OnFwd(OUT_C);OnRev(OUT_A);Wait(10);
OnFwd(OUT_A);OnRev(OUT_C);Wait(10);
Off(OUT_A+OUT_C);Wait(50);
//===================================
//=============メイン================
//===================================
task main()
{
SetPower(OUT_A,10);
SetPower(OUT_C,10);
SetSensor (SENSOR_1, SENSOR_LIGHT);
SetSensor (SENSOR_3, SENSOR_TOUCH);
SetSensor (SENSOR_2, SENSOR_LIGHT);
PlaySound(SOUND_LOW_BEEP) ;
Wait(1000);
OnRev(OUT_A+OUT_C);
until(SENSOR_1<35&&SENSOR_2<45);
Off(OUT_A+OUT_C);
Wait(50);
repeat (3)
{
line_straight ();
turn_right(100);
line_straight();
line_straight();
turn_right(100);
OnRev(OUT_B);
line_straight();
OnRev(OUT_A+OUT_C);
Wait(50);
Off(OUT_A+OUT_B+OUT_C);
Wait(50);
OnFwd(OUT_B);
line_back ();
Off(OUT_B);
turn_right(170);
line_straight();
line_straight();
turn_left(150);
OnFwd(OUT_A+OUT_B+OUT_C);
Wait(150);
Off(OUT_A+OUT_B+OUT_C);
OnRev(OUT_C);OnFwd(OUT_A);Wait(30);
OnRev(OUT_A);OnFwd(OUT_C);Wait(30);
Off(OUT_A+OUT_C);
line_straight();
line_straight();
go_straight();
line_straight();
turn_left(160);
line_straight();
line_straight();
turn_left(160);
goal();
OnFwd(OUT_A+OUT_C);
Wait(200);
Off(OUT_A+OUT_C);
turn_left(100);
line_straight();
line_straight();
turn_right (100);
OnFwd(OUT_A+OUT_C);
Wait(120);
Off(OUT_A+OUT_C);
line_straight();
line_straight();
go_straight();
}
}
プログラムの解説†
マクロ、サブルーチン編 〜首をふれ、首を!〜†
(1)直進トレース†
sub line_straight()
{
PlaySound(SOUND_DOUBLE_BEEP) ; //ピーピーと音を鳴らす
OnRev(OUT_A+OUT_C); //少しだけ直進
Wait(50);
OnRev(OUT_A+OUT_C);
while(SENSOR_1>35||SENSOR_2>45) //センサー1,2のどちらかが黒線に乗っていないとき
{
if(SENSOR_1>35&&SENSOR_2>45) //1,2の両方とも黒線に乗っていないとき
{
OnRev(OUT_A+OUT_C); //そのまま直進
}
if(SENSOR_1<35&&SENSOR_2>45) //1が黒線に乗ったとき
{
OnRev(OUT_C);Off(OUT_A); //Cを回転して軌道修正
}
if(SENSOR_1>35&&SENSOR_2<45) //3が黒線に乗ったとき
{
OnRev(OUT_A);Off(OUT_C); //Aを回転して軌道修正
}
}
Off(OUT_A+OUT_C); //1,2共に黒線に乗ったとき、停止
Wait(50);
}
- プログラムの中心になる直進ライントレースのプログラム
- センサーを左右に2つつけ、黒線を挟んだ状態で進む
- コースの1ブロックのみ進む
- 十字路にきたときは、センサー1,2共に黒線に乗るので、そこで停止する
(2)バック†
sub line_back()
{
PlaySound(SOUND_DOUBLE_BEEP) ;
OnFwd(OUT_A+OUT_C);
Wait(260);
Off(OUT_A+OUT_C);
Wait(50);
}
- バックのプログラム
- 本来はライントレース機能をつけるつもりだったが、時間の都合カット
- ライントレースしなくても、セットパワーによりちゃんと直進するので特に問題はなかった
(3)直進トレースなし†
sub go_straight()
{
PlaySound(SOUND_LOW_BEEP) ;
OnFwd(OUT_C);OnRev(OUT_A);Wait(10); // 首
OnFwd(OUT_A);OnRev(OUT_C);Wait(10); // 振り
OnRev(OUT_A+OUT_C);Wait(50);
OnRev(OUT_A+OUT_C);
until(SENSOR_1<35&&SENSOR_2<45);
Off(OUT_A+OUT_C);
Wait(50);
}
- 缶の間を通り抜けるためのプログラム
- 缶を運ぶ上で最も重要な、「首振り」が使われている
- 0,1秒ずつ左右に回転することで、缶が片方のアームにだけ強く押し付けられるのを防ぐ
- 首振りを使わない場合、片方のアームにだけ缶が押し付けられ、それによってうまく直進しない
(4)ゴール†
sub goal()
{
PlaySound(SOUND_FAST_UP) ;
OnRev(OUT_A+OUT_B+OUT_C);
Wait(50);
OnRev(OUT_A+OUT_B+OUT_C);
while(SENSOR_3==0)
{
if(SENSOR_1>35&&SENSOR_2>45)
{
OnRev(OUT_A+OUT_B+OUT_C);
}
if(SENSOR_1<35&&SENSOR_2>45)
{
OnRev(OUT_B+OUT_C);Off(OUT_A);
}
if(SENSOR_1>35&&SENSOR_2<45)
{
OnRev(OUT_A+OUT_B);Off(OUT_C);
}
}
Off(OUT_A+OUT_B+OUT_C);
}
- 缶をゴールに入れるプログラム
- 缶がゴールに入り、本体前部につけられたタッチセンサーがonになるまで前進する
(5)左回転 †
#define turn_left(t)
PlaySound(SOUND_UP);
OnRev (OUT_A+OUT_C);Wait(50);
OnFwd(OUT_A);OnRev(OUT_C);Wait(t);
Off(OUT_A+OUT_C);
OnFwd(OUT_C);OnRev(OUT_A);Wait(10); //
OnFwd(OUT_A);OnRev(OUT_C);Wait(10); //首振り
Off(OUT_A+OUT_C);Wait(50); //
- 左に回転するプログラム
- ここでも「首振り」を使用している
- 左に回転した後は、右のアームに缶が押し付けられるため、それを防ぐ
(6)右回転†
#define turn_right(t)
PlaySound(SOUND_UP) ;
OnRev(OUT_A+OUT_C);Wait(50);
OnFwd(OUT_C);OnRev(OUT_A);Wait(t);
Off(OUT_A+OUT_C);
OnFwd(OUT_C);OnRev(OUT_A);Wait(10);
OnFwd(OUT_A);OnRev(OUT_C);Wait(10);
Off(OUT_A+OUT_C);Wait(50);
- 右回転のプログラム
- ここでも同様に「首振り」を使用
メインタスク編 〜先生!腰が限界です〜†
task main()
{
SetPower(OUT_A,10);
SetPower(OUT_C,10);
SetSensor (SENSOR_1, SENSOR_LIGHT);
SetSensor (SENSOR_3, SENSOR_TOUCH);
SetSensor (SENSOR_2, SENSOR_LIGHT);
- ここでのポイントはセットパワー
- モータの力の差による曲進を防ぐ
- 本番直前まで時間の限り調整を繰り返した
PlaySound(SOUND_LOW_BEEP) ;
Wait(1000);
OnRev(OUT_A+OUT_C);
until(SENSOR_1<35&&SENSOR_2<45);
Off(OUT_A+OUT_C);
Wait(50);
repeat (3) //以下を3回繰り返す
{
- 最初の線にたどり着くまで直進
- このとき低い音が鳴る
line_straight (); // (1)
turn_right(100); // (6)
line_straight(); // (1)
line_straight(); // (1)
turn_right(100); // (6)
OnRev(OUT_B); //アーム上げ
line_straight(); // (1)
OnRev(OUT_A+OUT_C);
Wait(50);
Off(OUT_A+OUT_B+OUT_C);
Wait(50);
OnFwd(OUT_B); //アーム下げ
- 直進、右回転を繰り返し缶の前の線まで移動
- 途中でBを回し、アームを上に上げる
- アームを上げたまま直進し、缶にぶつかるところでアームを下ろす
- 図中の数字はマクロ、サブルーチンの番号
line_back (); //(2)
Off(OUT_B);
turn_right(170); //(6)
line_straight(); //(1)
line_straight(); //(1)
turn_left(150); //(5)
OnFwd(OUT_A+OUT_B+OUT_C);
Wait(150);
Off(OUT_A+OUT_B+OUT_C);
- 缶をキャッチしたら、前の線までバック
- 右回転後、二つ先の線まで直進
- 左回転し、一度バックする
- 次のライントレースをを長くして、まっすぐ缶の間を通らせるため
OnRev(OUT_C);OnFwd(OUT_A);Wait(30);
OnRev(OUT_A);OnFwd(OUT_C);Wait(30);
Off(OUT_A+OUT_C);
- 「首振り」
- マクロ、サブルーチン編で解説
line_straight(); //(1)
line_straight(); //(1)
go_straight(); //(3)
line_straight(); //(1)
turn_left(160); //(5)
line_straight(); //(1)
line_straight(); //(1)
turn_left(160); //(5)
goal(); //(4)
OnFwd(OUT_A+OUT_C); //
Wait(200); // バック
Off(OUT_A+OUT_C); //
turn_left(100); //(5)
line_straight(); //(1)
line_straight(); //(1)
turn_right (100); //(6)
OnFwd(OUT_A+OUT_C); //
Wait(120); //バック
Off(OUT_A+OUT_C); //
line_straight(); //(1)
line_straight(); //(1)
go_straight(); //(3)
} //ここまでを3回繰り返す
}