2013b/Member/michaegon/Mission2

課題2内容
メンバー
ロボット概要
プログラム概要
- ライントレース部分の仕様
  - 交差点・急カーブ判定
  - TIRE_main.nxc
- 缶を動かす部分の仕様
  - ARM_main.nxc
実際に動かしてみた...
反省点・改善点
- ロボットについて
- プログラムについて

課題2内容†

2台のロボットで空き缶を収集し、ゴールに空き缶になるべく高く積み上げる

基本ルール

競技時間は審判が続行不能と判断するまで、あるいはリタイアするまで。

図の左下の隅からスタートする。ロボットが紙からはみ出ない範囲で可能な限りに隅に配置する。

空き缶を集めて図の「ゴール」と書かれた円弧と直線で囲まれる領域に空き缶を運ぶ

なるべく高く空き缶を積み上げる

運び終わった後、あるいは積み上げた後はロボットはゴールの領域から外に出ること。また空き缶と接触していたはいけない。

開始の合図から５秒以内にスタートボタンを押す作業を完了すること。

競技が終了するまで、ロボットに触ったり人間が遠隔で操作してはならない。

途中でうまく動かなくなった場合、１回限り再スタートすることができる（再スタートの際に別プログラムで起動してよい）。

以上2013b/Mission2より引用

コース図

↑

メンバー†

dyuma
ロボット製作（+ロボット管理）・プログラミングとどちらもこなしている中間管理職。

pepo
ロボットを組ませたら彼の右に出るものはいない。（ただし情報工学科）

michaegon
このページの著者
プログラムの方をやってます。

↑

ロボット概要†

今回使用したロボットです。胴体がかなり長いです。

胴体中央のレールに缶が４本まで入るようになっています。

缶をこの腕で殴ってレールに入れる仕組みです。

缶を感知するのにはこの超音波センサーを使います。

レールの後ろから缶が漏れないようにストッパー君がついています。

缶をレールに詰め込んだら、缶を抑えるための腕で缶を抑えながらレール自身を立ち上げます。

↑

プログラム概要†

↑

ライントレース部分の仕様†

今回もPID制御を利用したライントレースプログラムを組んでみました。
前回の反省を生かしてより安定して成功できるものに改良しました。 PID制御については前回のページをご覧ください。

↑

交差点・急カーブ判定†

基本的なライントレースのプログラムは以下の通りです。

#define SPEED 32
#define KP 1.6
#define KI 0.00002
#define KD 0.8
#define THRESHOLD 50
#define THRESHOLD2 32
#define RIGHT 1
#define LEFT -1
#define DT 1
#define CT 250

int dev[2]; // 偏差
float integral;

// 操作量を返す関数
inline int ope_amount()
{
 float pid;
 int result;

 dev[0]=dev[1]; // 過去の偏差
 dev[1]=SENSOR_1-THRESHOLD; // 現在の偏差

 if (dev[0]==dev[1]) { // 不要になったらintegralリセット
    integral=0;
 }
 integral+=(dev[1]+dev[0])*DT/2; // 積分計算

 pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT; // PID

 result=pid; // int型に変換
 /*シンクロ率の範囲におさめる*/
 if (result>100) {
    result=100;
 } else if (result<-100) {
    result=-100;
 }

 return result;
}

/*
 黒線の、とある側を走行する関数（車線変更つき）
*/
inline void go_xside(int sgn)
{
 while (true) {
       OnFwdSync(OUT_BC,SPEED,ope_amount()*sgn);
       Wait(DT);
       if (SENSOR_1<THRESHOLD2) {
          OnFwdSync(OUT_BC,SPEED,-ope_amount()*sgn);
          Wait(CT);
          break;

       }
 }
}

task main()
{
 while (true) {
       go_xside(RIGHT);
       go_xside(LEFT);
 }
}

このプログラムは前回同様、PID制御を使ってライントレースをしています。
前回は、コースの形状を利用して力技で交差点を突破するプログラムだったせいで、ずいぶんコース１周回りきれる確率が低いのが決定的な欠点でした。
そこで、今回はもっと成功率が高い交差点通過方法を考えました。

今回は閾値をラインの境界上とライン中央付近の２段階に設定しました（ラインの中央に行くほど光センサーの値は小さくなります）。
直線やカーブの外側、緩やかなカーブの内側を走っているときは、ラインの境界上付近を走行していますが、急カーブの内側や交差点に差し掛かると、ロボットがコースの変化に追いつけずにライン中央付近の２つ目の閾値を越えてきます。
すると、go_xside内のif文が実行されます。

if (SENSOR_1<THRESHOLD2) {
    OnFwdSync(OUT_BC,SPEED,-ope_amount()*sgn); // 通常とは逆向きへ曲がる
    Wait(CT);
    break;
}

このif文では通常ライン内にロボットが入ったとき、ライン外へ向けて曲がりだすところをCTミリ秒間だけラインの内側に向けて走ります。
そうすることで、ライン中央を斜めに横切ってラインの反対側（カーブ中ならカーブの外側、交差点なら交差点中央）へ移動します。
このように、交差点を斜めに横切ってわたりきることができるようになりました。
この方法では、前回の力技通過よりもKIなどの値や電池残量などに成功率が左右されずに安定した走行を実現できるうえ、どっち周りにコースを回っても１周することができます。

↓以上を利用して、今回のために作ったプログラムです。

↑

TIRE_main.nxc†

タイヤを制御してライントレースをする方のメイン部分です。

#define SPEED 33
#define SPEED2 20
#define KP 7
#define KI 0.002
#define KD 1.8
#define THRESHOLD 20
#define THRESHOLD2 10
#define RIGHT 1
#define LEFT -1
#define DT 1
#define CT 1200
#define ARM 1
#define FIRST_T 5000

void turn_right()
{
 OnFwd(OUT_C,SPEED2);
 OnFwd(OUT_B,-SPEED2);
}

void turn_left()
{
 OnFwd(OUT_B,SPEED2);
 OnFwd(OUT_C,-SPEED2);
} 

void start_action()
{
 // Bluetooth接続の確認
 until (BluetoothStatus(ARM)==NO_ERR);
 PlaySound(SOUND_UP); 

 // スタート地点からコースに乗るまで直進
 OnFwdSync(OUT_BC,SPEED,0);
 until (SENSOR_1<THRESHOLD);
 Off(OUT_BC);
} 

void can_build2()
{
 // 図の右からゴール前の交差点に入るとする
 turn_right();
 Wait(CT);
 Off(OUT_BC);
 // 積み上げ指令を送る
 SendRemoteNumber(ARM,MAILBOX3,1);
 // 積み上げ完了の合図を待つ
 int msg;
 do {
    msg=0;
    ReceiveRemoteNumber(MAILBOX4,true,msg);
 } while (!msg);
} 

void can_build1()
{
 // 図の右からゴール前の交差点に入るとする
 turn_left();
 Wait(CT*4);
 Off(OUT_BC);
 // ゴール奥へ移動
 OnFwd(OUT_BC,SPEED);
 Wait(CT*4);
 Off(OUT_BC);
 // 積み上げ指令を送る
 SendRemoteNumber(ARM,MAILBOX3,1);
 // 積み上げ完了の合図を待つ
 int msg;
 do {
    msg=0;
    ReceiveRemoteNumber(MAILBOX4,true,msg);
 } while (!msg);
 // バックで交差点まで戻って止まる
 OnFwd(OUT_BC,-SPEED);
 Wait(CT*5);
 Off(OUT_BC);
 SendRemoteNumber(ARM,MAILBOX5,1);
}

void end_action()
{
 // かなり後ろに下がる
 OnFwd(OUT_BC,-SPEED);
 Wait(FIRST_T);
 turn_right();
 Wait(CT);
 // コース外へ離脱
 OnFwd(OUT_BC,100);
 Wait(FIRST_T*2);
 PlaySound(SOUND_UP);
}

// 缶があるかどうか判定する関数
int iscan()
{
 if (SensorUS(S2)<=10) { // 超音波センサーが10cm以内を感知
    return 1;
 } else {
    return 0;
 }
}

// 缶の蹴り上げを命令する関数
void kick()
{
 OnFwdSync(OUT_BC,SPEED,0);
 Wait(CT);
 Float(OUT_BC);
 // Bluetoothで通信して蹴り上げを実行
 SendRemoteNumber(ARM,MAILBOX1,1);
 int msg=0;
 do {
    ReceiveRemoteNumber(MAILBOX2,true,msg);
 } while (!msg);
}
 
int dev[2];
int flag,count;
float integral;

inline int ope_amount()
{
 float pid;
 int result;

 dev[0]=dev[1]; // 過去の偏差
 dev[1]=SENSOR_1-THRESHOLD; // 現在の偏差

 if (dev[0]==dev[1]) { // 不要になったらintegralリセット
    integral=0;
 }
 integral+=(dev[1]+dev[0])*DT/2; // 積分計算

 pid=KP*dev[1]+KI*integral+KD*(dev[1]-dev[0])/DT; // PID

 result=pid; // int型に変換
 /*シンクロ率の範囲におさめる*/
 if (result>100) {
    result=100;
 } else if (result<-100) {
    result=-100;
 }

 return result;
}

inline void go_xside(int sgn,int change)
{
 while (true) {
       OnFwdSync(OUT_BC,SPEED,ope_amount()*sgn);
       Wait(DT);
       if (SENSOR_1<THRESHOLD2) {
          OnFwdSync(OUT_BC,SPEED,-ope_amount()*sgn);
          Wait(CT);
          PlaySound(SOUND_UP);
          break;
       }
       if (iscan()) {
          kick();
          count++;
          if (count==change) {
             flag++;
             break;
          }
       }
 }
}

inline void go_xside2(int sgn)
{
 while (true) {
       OnFwdSync(OUT_BC,SPEED,ope_amount()*sgn);
       Wait(DT);
       if (SENSOR_1<THRESHOLD2) {
          count++;
          OnFwdSync(OUT_BC,SPEED,-ope_amount()*sgn);
          Wait(CT);
          PlaySound(SOUND_UP);
          break;
       }
 }
}

inline void go_xside3(int sgn)
{
 while (true) {
       OnFwdSync(OUT_BC,SPEED2,ope_amount()*sgn);
       Wait(DT);
       if (SENSOR_1<THRESHOLD2) {
          count++;
          OnFwdSync(OUT_BC,SPEED2,-ope_amount()*sgn);
          Wait(CT);
          PlaySound(SOUND_UP);
          break;
       }
 }
}

// １回目
// 缶を4つ集めてゴール前の交差点に到達するまで
void phase1()
{
 count=0;
 flag=0;
 while (true) {
       go_xside(LEFT,4);
       if (flag) {
          break;
       }
       go_xside(RIGHT,4);
       if (flag) {
          break;
       }
 }
 count=0;
 flag=0;
 while (true) {
       go_xside2(LEFT);
       if (count==3){
          break;
       }
       go_xside2(RIGHT);
       if (count==3){
          break;
       }
 }
 Off(OUT_BC);
}

// ２回目
// ゴール前の交差点からスタートし、
// 缶を4つ集めてゴール前の交差点まで戻ってくるところまで
void phase2()
{
 turn_right();
 Wait(CT*4);
 count=0;
 // ヘアピンまで
 while (true) {
       go_xside2(LEFT);
       if (count==2) {
          break;
       }
       go_xside2(RIGHT);
       if (count==2) {
          break;
       }
 }
 // 向こう岸へ移動
 Wait(CT*4);
 OnFwd(OUT_BC,SPEED);
 Wait(FIRST_T);
 count=0;
 flag=0;
 // 缶集め
 while (true) {
       go_xside(LEFT,4);
       if (flag) {
          break;
       }
       go_xside(RIGHT,4);
       if (flag) {
          break;
       }
 }
 count=0;
 // マップ移動
 while (true) {
       go_xside3(LEFT);
       if (count==2) {
          break;
       }
       go_xside3(RIGHT);
       if (count==2) {
          break;
       }
 }
 Wait(CT);
 count=0;
 while (true) {
       go_xside2(LEFT);
       if (count==3) {
          break;
       }
       go_xside2(RIGHT);
       if (count==3) {
          break;
       }
 }
 Off(OUT_BC);
}
// ライントレース用タイヤ制御側プログラム(MASTER)
//main部分

task main()
{
 SetSensorLight(S1);
 SetSensorLowspeed(S2);
 
 // スタート地点からコースに乗るまでの行動
 start_action();

 // 缶集めと缶建てそれぞれ2回ずつ
 phase1();
 can_build1();
 phase2();
 can_build2();

 // コース外への離脱を定義
 end_action();

}

このプログラムでは下のようにコースを回って缶を４つ集めてはゴールに向かい、建て終わればまた４つ集めてゴールに向かうようにしました。

まずphase1()では上図の①（赤）、②（緑）のように進んでいきます。ライントレースのためにインライン関数go_xsideが働いています。この最中、缶を感知するiscan関数が動いていて、缶があればアームが動いて缶をけり上げてレールに入れる動作を指示するようになっています。缶を入れる動作の方はARM_main.nxcの方に書かれていて、スレーブ側が行います。
缶を４つ集め終わるとgo_xside2に切り替わります。この関数では光センサーがTHRESHOLD2以下、つまり交差点、急カーブ判定をした回数が合計３回（４つ目の缶を見つけた後にゴールまでに認知する数）になると缶を建てるための移動作業can_build1が始まります。

can_build1ではまずゴール前の交差点で４５度右に旋回してその後直進してゴール中奥に移動して缶を建てる作業をするようにスレーブに指示を送ります。
建て終わったという指示を受け取ると、バックしてゴール前の交差点に戻ります。
このあと、２回目の缶集めのためにphase2に移ります。

phase2では上図の③（青）のように進んでいきます。
go_xside関数が働いていて、ゴールから出発した後、ヘアピンカーブのところから直進して隣のコースへ移動して、缶を集めてきます。集め終わったらgo_xside3が働いて２つのコースのつなぎ目を移動します。その後、go_xside2を使ってゴール前の交差点まで移動、その後、缶を建てるためのcan_build2に移ります。

can_build2では、交差点で右４５度に旋回したあと、その場で缶を建てるようにスレーブ側に指示を出します。缶を建て終わったという指示を受け取ると、コースを離脱して終了するためのend_action関数に移ります。

end_actionでは、建てた缶にぶつからないように、十分下がったあと、旋回後、直進しコースから離脱して終了します。

↑

缶を動かす部分の仕様†

↑

ARM_main.nxc†

アーム制御をする方のメイン部分です。
マスターである上のライントレースをするプログラムであるTIRE_main.nxcからの指示を受け取って缶を回収したり、缶を建てたりといった行動をします。

#define DT 1
#define CT 250

// 蹴り上げ
void hit()
{
 // 蹴り
 RotateMotor(OUT_A,100,270);
 Wait(3000);
 Off(OUT_A);
 Wait(DT);
 // 戻し
 RotateMotor(OUT_A,-20,270);
 Wait(3000);
 Off(OUT_A);
 Wait(DT);
 SendResponseNumber(MAILBOX2,1);
 PlaySound(SOUND_UP);
}

void build()
{
 // 抑えアームを動かす
 RotateMotor(OUT_C,20,120);
 Wait(CT);
 Off(OUT_C);
 // 上げる
 ResetTachoCount(OUT_B);
 while (MotorTachoCount(OUT_B)>-20) {
       OnFwdReg(OUT_B,-15,OUT_REGMODE_SPEED);
       PlaySound(SOUND_UP);
 }
 Off(OUT_B);
 // 抑え外す
 RotateMotor(OUT_C,-20,120);
 Wait(CT);
 Off(OUT_C);
 // 建て終わりの合図
 SendResponseNumber(MAILBOX4,1);

 // 収納合図を待つ
 int ord=0;
 do {
    ReceiveRemoteNumber(MAILBOX5,true,ord);
 } while (!ord);

 // 収納
 ResetTachoCount(OUT_B);
 while (MotorTachoCount(OUT_B)<30) {
       OnFwdReg(OUT_B,1,OUT_REGMODE_SPEED);
       PlaySound(SOUND_DOWN);
 }
 Off(OUT_B);
 Wait(DT);
}

// アーム部main
task main()
{
 int msg;
 while (true) {
       msg=0;
       // 蹴り上げを指令を確認
       ReceiveRemoteNumber(MAILBOX1,true,msg);
       if (msg) {
          msg=0;
          hit();
       }
       // 缶建ての指令を確認
       ReceiveRemoteNumber(MAILBOX3,true,msg);
       if (msg) {
          msg=0;
          build();
       }
 }
}

缶を集める段階では、マスターの方が缶を見つけると、缶をレールに蹴りいれるhit関数を動かすように指示が送られてきます。関数hitではアームで缶を倒した後そのままレールに蹴りいれます。

缶を建てる段階になると、マスターの方から指示が送られてきて、缶を建てる作業をする関数buildを動かします。
関数buildでは、建てるときにタワーの下の方を支えるための腕を動かして缶が倒れないようにした後、レール自体を建てて缶を建てます。

缶を建て終えたらマスターの方に指示を送ります。この指示を受け取ったマスター側は、レールやアームを格納したときに建てた缶にぶつからない程度に下がった後、スレーブ側にアームなどの格納の支持を送ってきます。この指示を受けたスレーブ側は格納作業をします。
格納作業が終わると、マスター側に指示を出して、再び缶集めの作業などに戻ります。

↑

実際に動かしてみた...†

上ではずいぶんスムーズに館を建てる作業までいけるようなことを書いてますが、実際やってみるとまったくうまくいきませんでした。

缶を蹴りいれて缶を集める作業は予想以上にうまくいきました。ただし、本体がやたらと長いので、ライントレースの最中にカーブを曲がったりしていると、その長い本体でコース上の缶をなぎ倒してしまい缶が取れない状況になってしまいました。

また、本体が重すぎて後輪が滑らかに動かずにそもそもうまく進みませんでした。このせいでライントレースが不安定になってしまい、ゴールにたどり着けませんでした。

缶を建てる作業についても、缶を倒さないようにゆっくりとレールを建てる予定でしたが、ゆっくり上げるためにモーターのパワーをかなり低めにしてレールを建てようとすると、レールと缶の重さでうまく上がりませんでした。だからといって、あまりパワーを上げると勢いがつきすぎて缶が倒れてしまう心配があったのでパワーを上げることはできませんでした。

↑

反省点・改善点†

↑

ロボットについて†

実際に動かしてみると全然うまくいきませんでした。
これを改善するには、まずロボットをもっと余裕のある設計にするべきだと思いました。
たとえば、後輪をもっと強化して、ライントレースに支障が出ないように出来ればよかったです。
また、缶を建てるときにレールを上げるため動力としてモーターを使っていましたが、レールとモーターの間にもっとギアを付けてモーターをたくさん回しても少しずつしかレールが動かないようにすれば、缶が倒れたり、レールが持ち上がらないといった心配が解消できたと思います。

↑

プログラムについて†

ライントレース部分については、前回の課題の時よりもより安定した仕組みにしたつもりでしたが、大きい胴体のロボット用に調整がうまくいきませんでした。やはりPID制御は各種ゲインの値の良い調整方法を確立しないと難しかったです。

缶を建てる部分についても、結構大雑把な動作になっていたと思います。缶を蹴りいれる部分はロボットの方がうまく作れていたのでうまくいきましたが、缶を建てる部分はプログラムの方でも改善すべきだと思いました。レールを上げる作業では、パワーを下げすぎても上がらないし、パワーを上げすぎても缶が倒れてしまうので、プログラムの方でも適切なパワー調整をするべきだと思いました。