![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
目次
正確なコースを作り、作業時間を短縮するために、 このような画像ファイルを作成しました。
これを何枚かのA4用紙に分けて印刷して台紙に貼り付け、 インクの薄い部分は黒いペンで塗りました。
しかし、貼り付ける作業に時間がかかってしまい、 あまいいい方法とは言えませんでした。
また、セロハンテープを使って貼り付けたため、テープの部分が少しだけ明るくなってしまいました。 しきい値の調整は難しかったですが、トレースにはほとんど問題ありませんでした。
説明書の見本をもとに組み立てました。
見本よりもタイヤの幅を狭くしたことで小回りが効き、 光センサーを前輪に近づけることでトレースがしやすくなりました。
パーツがたくさん余ったので補強をしっかりしておきました。
アームはNXT本体の右、車体の右肩にあたる部分に取り付けました。 モーターが車体からはみ出してしまうため、多めの補強を施しました。
アームを上から下ろすことでピンポン玉を確保し、坂に押し上げます。 このとき、ピンポン玉の位置がずれても確保できるように坂を3箇所作りました。
ピンポン玉を確保したままゴールまで運びます。 シュートするときは、アームを上げるとピンポン玉が坂を転がります。
車体の構成に関する部分です
// 入出力系 #define LEFT_MOTER OUT_C #define RIGHT_MOTER OUT_A #define LIGHT_SENSOR S3 // スピード #define SPEED 40 // 回転センサーの係数 #define MOTER_ROTATE (1 / 14.55) // 車体の幅 #define BODY_WIDTH 12.2
コードを簡略化するためのマクロです
// 回転センサーの値を取得する #define LeftRotation() MotorRotationCount(OUT_C) #define RightRotation() MotorRotationCount(OUT_A)
OnFwdとOnRevとOffをまとめた関数です。 スピードによって動作が変わるようになっています。
/*
* モーターを回転させる
* スピードが負のときは逆回転
* 0のときは停止
*/
void On(int leftSpeed, int rightSpeed){
if(leftSpeed > 0){
OnFwd(LEFT_MOTER, leftSpeed);
}else if(leftSpeed){
OnRev(LEFT_MOTER, -leftSpeed);
}else{
Off(LEFT_MOTER);
}
if(rightSpeed > 0){
OnFwd(RIGHT_MOTER, rightSpeed);
}else if(rightSpeed){
OnRev(RIGHT_MOTER, -rightSpeed);
}else{
Off(RIGHT_MOTER);
}
}
ライントレースに使います。
授業では触れていませんが、"switch"という構文を使うことを想定しています。 光センサーのしきい値はここに記入します。
/*
* 明るさを 0 〜 4 で返す
* 暗い < 0 1 2 3 4 > 明るい
*/
bool Light(){
if(SENSOR_3 < 40){
return 0;
}else if(SENSOR_3 < 45){
return 1;
}else if(SENSOR_3 < 50){
return 2;
}else if(SENSOR_3 < 55){
return 3;
}else{
return 4;
}
}
コード全体が見たい人以外は飛ばしてください。 解説には部分ごとにコードを載せています。
task main(){
//通過ポイントを数えるための変数
int corner = 0;
// 車体の向き
float rotation = 0;
// 光センサー初期化
SetSensorLight(LIGHT_SENSOR);
// スタートから出る
On(SPEED, SPEED);
until(Light() == 0);
until(Light() == 4);
// トレース開始
while(corner < 8){
Wait(1);
switch(Light()){
case 0: //左回転
On(-SPEED, SPEED);
break;
case 1: //左折
On(0, SPEED);
break;
case 2: //直進
On(SPEED, SPEED);
break;
case 3: //右折
On(SPEED, 0);
break;
case 4: //右回転
On(SPEED, -SPEED);
break;
}
// 回転センサーの値を取得する
int _tl = LeftRotation();
int _tr = RightRotation();
// 差から角度を求める
rotation += 360 * (_tr - tr - _tl + tl) * MOTER_ROTATE / (2 * BODY_WIDTH * PI);
tl = _tl;
tr = _tr;
// 通過ポイント
switch(corner){
case 0: // １回目のヘアピンカーブ
if(rotation > 90){
PlayTone(TONE_C4, 100);
corner++;
}
break;
case 1: // １回目の交差点
if(rotation < 10){
PlayTone(TONE_D4, 100);
On(SPEED, SPEED);
until(Light() == 0);
until(Light() >= 2);
corner++;
}
break;
case 2: // ピンポン玉を取る位置
if(rotation < -40){
PlayTone(TONE_E4, 100);
Wait(500);
On(SPEED, SPEED);
RotateMotor(OUT_B, 10, 90);
On(-SPEED, -SPEED);
Wait(500);
On(SPEED, 0);
until(Light() < 4);
corner++;
}
break;
case 3: // 大きいカーブ終了
if(rotation < -180){
PlayTone(TONE_F4, 100);
corner++;
}
break;
case 4: // ２回目の交差点
if(rotation > -180){
PlayTone(TONE_G4, 100);
On(SPEED, 0);
until(Light() == 4);
On(SPEED, SPEED);
until(Light() == 3);
corner++;
}
break;
case 5: // ３回目の交差点
if(rotation > 10){
PlayTone(TONE_A4, 100);
On(SPEED, 0);
until(Light() >= 3);
corner++;
}
break;
case 6: // ２回目のヘアピンカーブ
if(rotation > 90){
PlayTone(TONE_B4, 100);
corner++;
}
break;
case 7: // ゴール
if(rotation < -15){
PlayTone(TONE_C5, 100);
On(0, 0);
RotateMotor(OUT_B, 100, -90);
corner++;
}
break;
}
}
Wait(2000);
}
corner変数には通過ポイントの番号を、 rotation変数には車体の向きを格納します。
トレースに使用する光センサーを初期化します。
//通過ポイントを数えるための変数 int corner = 0; // 車体の向き float rotation = 0; // 光センサー初期化 SetSensorLight(LIGHT_SENSOR);
黒い線を跨ぐまで直進してスタートエリアから出ます。
On(SPEED, SPEED); until(Light() == 0); until(Light() == 4);
スタートエリアから出るとライントレースを開始します。 ゴールに到達してシュートするまで繰り返すループ関数です。
1ミリ秒で1ステップ進みます。
シュートをする前にプログラムが終了しないようにするために ループのあとにWaitを入れました。
while(corner < 8){
Wait(1);
/*
* ライントレース
*/
/*
* 車体の向きを求める
*/
/*
* 通過ポイントごとの処理(交差点を渡るなど)
*/
}
Wait(2000);
明るさを0から4の５段階に分けて、0と4のときはその場で回転、1と3のときは曲がりながら進む、2のときは直進します。
Light関数についてはこちらをご覧ください。
switch(Light()){
case 0: //左回転
On(-SPEED, SPEED);
break;
case 1: //左折
On(0, SPEED);
break;
case 2: //直進
On(SPEED, SPEED);
break;
case 3: //右折
On(SPEED, 0);
break;
case 4: //右回転
On(SPEED, -SPEED);
break;
}
この画像のように車体が進んだとき、 回転した角度θは方程式を解くと
θ = {(外側のタイヤが進んだ距離) - (内側のタイヤが進んだ距離)} / (車体の幅)
となります。 これを足していくと車体が向いている方向を求めることができます。
タイヤが進んだ距離はモーターの回転センサーを使って算出しています。 車体の幅は定規で測りました。 0°はスタートした向きとなります。
// 回転センサーの値を取得する int _tl = LeftRotation(); int _tr = RightRotation(); // 差から角度を求める rotation += 360 * (_tr - tr - _tl + tl) * MOTER_ROTATE / (2 * BODY_WIDTH * PI); tl = _tl; tr = _tr;
通過ポイントを設定して、車体の向きからコースのどのあたりにいるかを求めています。 デバッグ用に音を鳴らしています。
詳細は後述します。
switch(corner){
case 0:
/*
* あとで解説
*/
break;
}
車体の向きが90°を超えると通過ポイント1を超えたことにします。
if(rotation > 90){
PlayTone(TONE_C4, 100);
corner++;
}
break;
車体の向きが10°未満になると通過ポイント2を超えたことにして
交差点を超えます。
スタートと同じように線を超えるまで直進して交差点を超えます。
if(rotation < 10){
PlayTone(TONE_D4, 100);
On(SPEED, SPEED);
until(Light() == 0);
until(Light() >= 2);
corner++;
}
}
break;
１回目の交差点を超えて
車体の向きが-40°(320°)より小さくなるとると
ピンポン玉を拾います。
直進しながらアームを下げ、
ボールを確保すると元の位置までバックします。
右に進みながらライントレースに復帰します。
if(rotation < -40){
PlayTone(TONE_E4, 100);
Wait(500);
On(SPEED, SPEED);
RotateMotor(OUT_B, 10, 90);
On(-SPEED, -SPEED);
Wait(500);
On(SPEED, 0);
until(Light() < 4);
corner++;
}
break;
車体の向きがスタートしたときと反対になると大きいカーブが終了したとみなします。
if(rotation < -180){
PlayTone(TONE_F4, 100);
corner++;
}
break;
大きいカーブが終了して、車体が左に向くと２回目の交差点を越えます。
進み方や光センサーの条件を変えていますが、１回目の交差点を超えるときとほとんど同じです。
if(rotation > -180){
PlayTone(TONE_G4, 100);
On(SPEED, 0);
until(Light() == 4);
On(SPEED, SPEED);
until(Light() == 3);
corner++;
}
break;
車体の向きが10°を超えると３回目の交差点です。
本来は0°ですが、実際動かすと10°くらいが丁度よくなります。
この手前に直角カーブがありますが、ライントレースの処理だけで通過することができます。
方向を修正するように右に進み、交差点を越えていきます。
線から出るとライントレースに戻ります。
if(rotation > 10){
PlayTone(TONE_A4, 100);
On(SPEED, 0);
until(Light() >= 3);
corner++;
}
break;
１回目のヘアピンカーブと同じように車体の向きが90°を超えると７個目の通過ポイントです。
if(rotation > 90){
PlayTone(TONE_B4, 100);
corner++;
}
break;
車体の向きが15°より小さくなるとゴールです。
車体はゴールの方を向いているので、アームを上げてシュートします。
変数cornerが8になるのでループから抜けて、プログラムが終了します。
if(rotation < -15){
PlayTone(TONE_C5, 100);
On(0, 0);
RotateMotor(OUT_B, 100, -90);
corner++;
}
break;
課題のコースは決まっているので、 変曲点や交差点に通過ポイントを設定することで コース上のどのあたりを走行しているかを知ることができます。
例)１回目の交差点を超えるとき １回目交差点を普通にトレースすると左に曲がるので、0°より大きくなると交差点ということになります。 しかし、0°より大きくなる点は１回目の交差点までに存在しているので「スタートしてから車体の向きが90°になる点を通過すると」という条件を付け加えます。 このように通過ポイントを設定することで交差点を判定することができます。
はじめは、多くの人が使っている、カウンターの値が一定値を超えると直角(交差点と直角カーブ)と判定する方法を使おうと考えましたが、 実際試してみると、直角の前が直進の場合にうまく判定できなかったので 別の方法として向きを使う方法を考えました。
メリットは３つあります。 １つ目は判定の精度です。 向きの算出は、本来の向きと多少のズレがあるものの毎回同じ値になるので 判定に失敗することはほとんどありません。 トレースによる誤差も光センサーの位置やしきい値の調整のおかげで吸収できているようでした。 ２つ目は判定までの時間です。 交差点の判定がすぐにできるのでスムーズにトレースすることができます。 ３つ目は調整にかかる時間です。 通過ポイントの角度を指定するのですが、 コースを見ながら直感的に指定して、少し調整するだけで済み、 カウンターを使う方法よりも調整の時間を短縮できます。
デメリットは、スタートする向きがずれるとうまく判定できないことです。 スタートラインに坂を合わせることで克服しました。
NXTはモーターを接続する場所が3つしかなく、トレースに2個のモーターを使うのでピンポン玉の確保やシュートは1個のモーターだけで行う必要があります。
しかし、シュート機構が思いつかなかったので、車体の前に坂を付けてアームをあげるとピンポン玉が前に転がっていくようにしました。
ピンポン玉を確保するときはアームをおろしてピンポン玉を坂に押し上げます。坂は画像のように3箇所用意し、トレースが多少ズレてもピンポン玉を確保しシュートできるようにしました。坂を増やすことでシュートできる確率が格段に増えました。
ピンポン玉をゴールにシュートするところまでできたのでよかったです。
rotation.png 474件
[詳細]
saka.png 535件
[詳細]
DSC01846.jpg 1324件
[詳細]
body.JPG 1210件
[詳細]
point.png 1180件
[詳細]
course.png 1200件
[詳細]
