2014a/Member

目次

メンバー

・tadanobo(私めでございます。只之某(ただのなにがし)ですがね)

・mame

・dai

・bgpat

課題

課題2.png

・上図のA B C 1 2 3 は紙コップを示し、上図の左半分の3つのカップ(青)の位置はくじによりランダムで決定される。右半分の3つのカップ(黄)の位置は固定されています。

・上図右上のstartから始めて、左半分エリアからカップを回収して右半分エリアにある同じ番号のカップにそれぞれ重ねていきます。

詳しい説明は2014a/Mission2を見てください。

全体の流れ

過程2’.png

・右上のstartから右に90°回転して左上の点線三角の位置に移動し、(右に90°向いた状態から)

  。姥通椶了、左に135°回転してカップを回収する

  ■恩通椶了、左に45°    〃

  3個目の時、左に90°回転しカップ手前まで前進してからカップを回収する

・カップを回収したら点線三角の位置に戻ってからstartに移動し、回収したカップの番号と同じ番号のカップのところへ置きに行く。

補足

・カップはマシンの正面についている超音波センサー(縦になってる目のようなやつ)によって探します。超音波センサーは物体との距離を図ることができるものです。 確実性を高めるため、カップに接近してからマシンを左右に旋回させて首を振り、センサーが計測した値の中で最小値の方向(センサーの計測した距離が最も短い方向=カップのある方向)に進むようにしています。 また、この動作を2回することでさらに正確にカップに正対できるようになりました。

・始めはライントレース(2014a/Member/tadanobo/Mission1の”ライントレース”の項目参照)によりマシンを移動させてカップを回収したりしようとしました。 しかし最終的には、上記のようにstart位置、となりのエリアの点線三角の位置を基準に直線的な動きをさせた方が単純明快で簡単、加えて確実性も増すと考えこのような流れになりました。

ロボット

本体

写真 2014-07-25 16 36 37.jpg

マシンはドライブベースの上に本体を横に並べて設置しています。その上にアームを接合してあります。本体2つとモーター1つが中心によっているので重心は安定しています。

正面から見て右が親機(master)、左が子機(slave)です。親機がドライブベース・超音波センサー・アームについている光センサーを動かし、子機がアームとつめを動かします。この親子機はBluetoothにより通信し連携をとります。

ドライブベース

写真 2014-08-07 16 25 37.jpg

後輪は始めキャスターのようにクルクル回るものでしたが、接合部が不安定で本体の重量が大きいこともあり、過度に負荷がかかっていたので写真のようにタイヤのゴムを外したものを使用しました。

このホイールにしたところ本体の重量を支えていても安定し、また車体が回転するときも摩擦が少ないのでスムーズにいきました。

アーム

写真 2014-08-07 16 24 01.jpg 写真 2014-08-07 16 24 43.jpg

アームは上から下ろしてカップをつかむようになっています。

つかむ1.png つかむ2.png つかむ4.png

カップをつかんでアームを上げた際、一度つめを上げてカップを離し右写真のようにし、再びつめを 下ろすことでカップを安定してつかむと同時にアームについている光センサーでカップを確実に識別できるようにしています。

プログラム

”()”はそのタイトル以下に書かれているプログラムの名前を示します。

ドライブベース

各モーター・センサーの定義(命名) ”robot.h” 

#define LEFT_WHEEL OUT_C //左のタイヤのモーター
#define RIGHT_WHEEL OUT_A //右のタイヤのモーター
#define BOTH_WHEEL OUT_AC //両タイヤのモーター

#define ARM_BASE OUT_A //本体側のアームのモーター
#define ARM_HAND OUT_B //アーム先のモーター

#define DIST_SENSOR S2 //距離測定用超音波センサー
#define TYPE_SENSOR S3 //紙コップ判別用光センサー

void InitSensor(){
    SetSensorLowspeed(DIST_SENSOR);
    SetSensorLight(TYPE_SENSOR);
}

各動作の定義 ”drivebase.h” [#p39e563b]

#define SPEED_STRAIGHT 50
#define SPEED_SPIN 40
#define SPEED_STRAIGHT_LOW 20
#define SPEED_SPIN_LOW 30

#define STAY_WAIT 50

#define WHEEL_STRAIGHT (80 / 1715)

void DebugDriveType(string type, int speed){
    DebugNumber(DEBUG_DRIVETYPE, type, speed);
}

void Stay(){
    Off(BOTH_WHEEL);
    DebugDriveType("Stay", 0);
}

void StayWait(){
    Stay();
    Wait(STAY_WAIT);
}

void Drive(int speed, int rate, string type){
    if(speed){
        if(speed > 0){
            OnFwdSync(BOTH_WHEEL, speed, rate);
        }else{
            OnRevSync(BOTH_WHEEL, -speed, rate);
        }
        DebugDriveType(type, speed);
    }else{
    }
}

void DriveStraight(int speed){
    Drive(speed, 0, "Straight");
}

void DriveLeft(int speed){
    Drive(speed, -100, "Left");
}

void DriveRight(int speed){
    Drive(speed, 100, "Right");
}

void Forward(){
    DriveStraight(SPEED_STRAIGHT);
}

void ForwardLow(){
    DriveStraight(SPEED_STRAIGHT_LOW);
}

void Back(){
    DriveStraight(-SPEED_STRAIGHT);
}

void BackLow(){
    DriveStraight(-SPEED_STRAIGHT_LOW);
}

void SpinLeft(){
    DriveLeft(SPEED_SPIN);
}

void SpinLeftLow(){
    DriveLeft(SPEED_SPIN_LOW);
}

void SpinRight(){
    DriveRight(SPEED_SPIN);
}

void SpinRightLow(){
    DriveRight(SPEED_SPIN_LOW);
}

void Spin(int degree){
    if(degree > 0){
        StartDirectionTimer();
        SpinLeft();
        until(StopDirectionTimer() >= degree);
    }else{
        StartDirectionTimer();
        SpinRight();
        until(StopDirectionTimer() <= degree);
    }
}

void SpinLow(int degree){
    if(degree > 0){
        StartDirectionTimer();
        SpinLeftLow();
        until(StopDirectionTimer() >= degree);
    }else{
        StartDirectionTimer();
        SpinRightLow();
        until(StopDirectionTimer() <= degree);
    }
}

void GoForward(int x){
    int d;
    float p = MotorRotationCount(LEFT_WHEEL);
    Forward();
    until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
        DebugDriveType("Forward", d);
    }
    Stay();
}

void GoForwardLow(int x){
    int d;
    float p = MotorRotationCount(LEFT_WHEEL);
    ForwardLow();
    until((d = x - (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
        DebugDriveType("ForwardLow", d);
    }
    Stay();
}

void GoBack(int x){
    int d;
    float p = MotorRotationCount(LEFT_WHEEL);
    Back();
    until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
        DebugDriveType("Back", d);
    }
    Stay();
}

void GoBackLow(int x){
    int d;
    float p = MotorRotationCount(LEFT_WHEEL);
    BackLow();
    until((d = x + (MotorRotationCount(LEFT_WHEEL) - p) * WHEEL_STRAIGHT) < 0){
        DebugDriveType("BackLow", d);
    }
    Stay();
}

void GoStraight(int x){
    if(x > 0){
        GoForward(x);
    }else{
        GoBack(-x);
    }
}

void GoStraightLow(int x){
    if(x > 0){
        GoForwardLow(x);
    }else{
        GoBackLow(-x);
    }
}

車体の向き(自身がフィールド上においてどの向きに向いているか)を計る ”direction.h”

#define BODY_WIDTH 12.6
#define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)

float direction = 0;
float leftMoterCount = 0;
float rightMoterCount = 0;
float directionTimer = 0;

float GetDirection(){
    float out = MotorRotationCount(RIGHT_WHEEL);
    float in = MotorRotationCount(LEFT_WHEEL);
    direction += (out - leftMoterCount - in + rightMoterCount) * DIRECTION_COE;
    leftMoterCount = out;
    rightMoterCount = in;
    DebugNumber(DIRECTION_DEBUG, "Direction", direction);
    return direction;
}

void StartDirectionTimer(){
    directionTimer = GetDirection();
}

float StopDirectionTimer(){
    return GetDirection() - directionTimer;
}

これはタイヤの半径・車体の旋回時における回転半径などを用いて自身が旋回したときに何度回ったかを計算するものです。

具体的には下図の赤い四角内の式で車体の角度が得られます。タイヤの回転角はモーターに搭載されている回転センサーによって分かります。

角度計算説明.png 説明2.png

※上図のαyとαxに関して・・・半径αの円が角度zだけ回ったとき、その回転した円周の長さは(半径)×(回転角度)=αz という公式が存在します。

※旋回の時は結果的に右上図にような式になります。厳密には左上図のrを-rに、x=-yと置き換えて計算すると右上図の赤四角の式が得られます。

このように自身の向きを計算させるとマシンの操作が容易になります。

最初startにまっすぐ置いたとして、それを0°とします。(因みに左回りが正で、右回りが負です。左に90°回ったときは+90°で、右に90°回ったときは-90°となります)

たとえばここから車体を90°右に向けたい時、マシンに「自身の向きが-90°になるまで右旋回する」というプログラムを入力すればマシンは右に90°だけ回ります。

また、向きを最初の状態にリセットしたい時は「自身の向きが0°になるよう旋回する」と入力すれば車体はスタート時のまっすぐの向きに直ります。

カップを探す ”search.h”

#define DIST_CATCH 12
#define DIST_STACK 11
#define DIST_SEARCH 6

#define SEARCH_DIRECTION1 90
#define SEARCH_DIRECTION2 120

#define DistSensor SensorUS(DIST_SENSOR)

void TowardCup(int degree){
    int min = DistSensor;
    float dir = GetDirection();
    SpinLow(-degree * 0.5);
    StartDirectionTimer();
    SpinLeftLow();
    while(StopDirectionTimer() < degree){
        if(DistSensor < min){
            min = DistSensor;
            dir = GetDirection();
        }
    }
    SpinRightLow();
    until(GetDirection() <= dir);
    Stay();
}

int PrepareCup(int dist){
    int x, y;
    TowardCup(SEARCH_DIRECTION1);
    x = DistSensor - DIST_SEARCH;
    GoForwardLow(x);
    StayWait();
    TowardCup(SEARCH_DIRECTION2);
    y = dist - DistSensor;
    GoBackLow(y);
    StayWait();
    return x - y;
}

int PrepareCatch(){
    return PrepareCup(DIST_CATCH);
}

int PrepareStack(){
    return PrepareCup(DIST_STACK);
}

まず最初に首を左右に振ってカップのある方向を計ってその方向に進み、ある程度接近したところでもう一度首を振ってカップを探します。

アームのプログラム ”arm.h”

定義文

#define DOWN_POWER 30
#define HOLD_DOWN_POWER 50
#define UP_POWER 90
#define HOLD_UP_POWER 50
#define HOLD_POWER 20
#define RELEASE_POWER 80

#define ARM1_DEGREE 90
#define ARM2_DEGREE 210

#define CUP1 55
#define CUP2 37
#define CUP3 23

#define TypeSensor SENSOR_3

サブルーチン

void DownArm(){
    Off(ARM_BASE);
    Wait(100);
    RotateMotor(ARM_BASE, DOWN_POWER, ARM1_DEGREE);
    OnFwd(ARM_BASE, HOLD_DOWN_POWER);
    Wait(1000);
}

void UpArm(){
    Off(ARM_BASE);
    Wait(100);
    RotateMotor(ARM_BASE, UP_POWER, -ARM1_DEGREE);
    OnRev(ARM_BASE, HOLD_UP_POWER);
    Wait(2000);
}

void HoldCup(){
    OnFwd(ARM_HAND, HOLD_POWER);
    while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE);
}

void ReleaseCup(){
    RotateMotor(ARM_HAND, RELEASE_POWER, -ARM2_DEGREE);
}

void CatchCup(){
    HoldCup();
    Wait(500);
    UpArm();
    Wait(500);
    ReleaseCup();
    HoldCup();
}

void StackCup(){
    DownArm();
    Wait(500);
    ReleaseCup();
    Wait(500);
}

int GetCupType(){
    int n = 0;
    if(TypeSensor > (CUP1 + CUP2) * 0.5){
        n = 1;
    }else if(TypeSensor > (CUP2 + CUP3) * 0.5){
        n = 2;
    }else{
        n = 3;
    }
    DebugNumber(DEBUG_CUPTYPE, "CupNumber", n);
    DebugNumber(DEBUG_CUPSENSOR, "CupSensor", TypeSensor);
    return n;
}

void MasterCatchCup(){
    BTSendMessage(BT_CATCH, true);
    BTReceiveMessage();
}

void MasterStackCup(){
    BTSendMessage(BT_STACK, true);
    BTReceiveMessage();
}

Bluetooth(2つの本体間での通信) ”bluetooth.h”

#define BT_CONN 1
#define BT_MAILBOX MAILBOX3

#define BT_NULL 0
#define BT_CATCH 1
#define BT_STACK 2
#define BT_FINISH 3

void BTStart(string file){
    until(BluetoothStatus(BT_CONN) == NO_ERR);
    RemoteStartProgram(BT_CONN, file);
}

int BTReceiveMessage(){
    int msg = BT_NULL;
    while(msg == BT_NULL){
        ReceiveRemoteNumber(BT_MAILBOX, true, msg);
        DebugNumber(5, "debug", msg);
    }
    return msg;
}

void BTSendMessage(int msg, bool isMaster){
    if(isMaster){
        SendRemoteNumber(BT_CONN, BT_MAILBOX, msg);
    }else{
        SendResponseNumber(BT_MAILBOX, msg);
    }
}

void BTStop(){
    BTSendMessage(BT_FINISH, true);
}

この通信では主に

/撞,子機にプログラムを起動させる(親機による子機内のプログラムの遠隔起動)

■欧弔遼楝隆屬妊瓮奪察璽犬里笋蠅箸

の2つをします。

たとえばマシンがカップをつかむときは、

/撞,子機にカップをつかむプログラムを起動させる。親機は子機からメッセージを受け取るまで待つ

∋匍,アームを動かしてカップを掴む

カップをつかみ終えたら、子機は親機に既定のメッセージを送る

た撞,六匍,ら既定のメッセージを受け取ると、それを合図として光センサーでカップを識別して から移動を開始する

ということをしています。親機はプログラムの遠隔起動により子機を動かすことができますが、その逆はできません。なので子機が親機を動かすにはメッセージのやり取りが必要になります

メインタスク slave― (子機のプログラム)

定義文

#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "arm.h"

このincludeとは、""内のプログラムを含む、という意味です。つまり、このmasterのプログラムはinclude"()"と書かれているプログラムをすべて内包していることになります。

このようにincludeを使うことでプログラムを短くまとめることができます。

メインタスク

task main(){
   int msg;
   while(msg != BT_FINISH){
       msg = BTReceiveMessage();
       switch(msg){
           case BT_CATCH:
               Debug(1, "Catch");
               DownArm();
               CatchCup();
               BTSendMessage(1, false);
               break;
           case BT_STACK:
               Debug(1, "Stack");
               StackCup();
               UpArm();
               BTSendMessage(1, false);
               break;
       }
       Debug(1, "Wait");
   }
   Debug(1, "Finish");
   Wait(5000);
}

メインタスク◆master― (親機のプログラム)

定義文

#include "debug.h"
#include "bluetooth.h"
#include "robot.h"
#include "direction.h"
#include "drivebase.h"
#include "search.h"
#include "arm.h"

サブルーチン

void GotoCatchCup(){
   GoForward(2);
   StayWait();
   Spin(-90);
   StayWait();
   GoForward(60);
   StayWait();
   Spin(90);
   StayWait();
}
void GotoStackCup(){
   GoForward(2);
   StayWait();
   Spin(-90);
   StayWait();
   GoBack(55);
   StayWait();
   Spin(90);
   StayWait();
}
void BeginCupA(){
   Spin(45);
   StayWait();
   GoForward(15);
   StayWait();
}
void BeginCupB(){
   Spin(-60);
   StayWait();
}
void BeginCupC(){
   GoForward(30);
   StayWait();
}
void EndCupA(){
   Spin(45 - GetDirection());
   StayWait();
   GoBack(10);
   StayWait();
}
void EndCupB(){
   Spin(-60 - GetDirection());
   StayWait();
}
void EndCupC(){
   Spin(-GetDirection());
   StayWait();
   GoBack(25);
   StayWait();
}
void BeginCup(int n){
   switch(n){
       case 1:
           DebugSound(TONE_C4);
           BeginCupA();
           break;
       case 2:
           DebugSound(TONE_C4);
           BeginCupB();
           break;
       case 3:
           DebugSound(TONE_E4);
           BeginCupC();
           break;
   }
}
void EndCup(int n){
   switch(n){
       case 1:
           EndCupA();
           break;
       case 2:
           EndCupB();
           break;
       case 3:
           EndCupC();
           break;
   }
}
void Cup(int phase){
   int x, n;
   GotoCatchCup();
   BeginCup(phase);
   x = PrepareCatch();
   MasterCatchCup();
   GoStraight(-x);
   EndCup(phase);
   Spin(-GetDirection());
   GotoStackCup();
   n = GetCupType();
   BeginCup(n);
   x = PrepareStack();
   MasterStackCup();
   GoStraight(-x);
   EndCup(n);
   Spin(-GetDirection());
} 

メインタスク

task main(){
   GoForward(4);
   StayWait();
   BTStart("slave.rxe");
   InitSensor();
   for(int i = 1; i <= 3; i++){
       Cup(i);
   }
   BTStop();
}

感想

課題1に比べ、チーム人数は4人に増えたことに加え準備の時間も比較的多く取られてはいたが、それでも本番直前まで調整を必要とした程度には時間がかかってしまった。 マシンは私の制作した試作機を皆で改良して完成させ、プログラミングは我らが誇る名プログラマー様様(bgpat)が大部分を手がけた。マシン制作からプログラミングまで、やることは多かったがチームの皆で協力してやれたのでよかった。

プログラムに関して、大部分を手がけた上、車体に自身の向きを計算させるというプログラムを考案するなど、bgpat氏には感謝御礼万々歳だ(このプログラムがなければ、左右のモーターをそれぞれ〇度ずつ回転させて車体を△度回転させ・・・などと逐一調整しなくてはならなかった。本当に画期的だ)。正直なところ、私は今回作成されたプログラムの詳細は把握しきれていない。とても複雑で、しかし高度なものだったとだけ言える。この授業でプログラミングは概ね習得したつもりでいたが、プログラミングはまだまだ奥が深いもので自分は未熟だと感じた。

反省点

・この度ロボコンでは2個のカップを重ねるに至ったものの、最後の一個を取る際ははさみきれないままアームを持ち上げ、結果カップを落としてしまった。このカップを取る際に落としてしまう現象は調整段階でも何度か見られた。カップを探しながら接近する段階で車体がうまくカップに正対できず、そのままアームを下したから、またカップを掴みきれていないままアームを上げたから、などが原因として考えられる。

・車体には自身の向きを計算させていたものの、移動しているうちに、特にカップを探す段階で徐々に誤差が生じ、隣のエリアに移るときに90°旋回させる時など角度指定で動作させているところでずれが生じてしまう(本来紙の辺に対し平行に移動するのに斜めになってしまう)など問題が生じた。これは本番直前まで調整していたが修正しきるには至らなかった。

良かった点

・本体とアーム部分はBluetoohによる通信で連携させたが、スムーズに機能していたのでカップをつかむ・重ねる動作がうまくいった。

・超音波センサーで探しながらのカップへの接近がうまくいっており、カップに対し適切な距離で正対できていた。


添付ファイル: file説明2.png 222件 [詳細] file角度計算説明.png 262件 [詳細] fileつかむ4.png 254件 [詳細] fileつかむ2.png 207件 [詳細] fileつかむ1.png 218件 [詳細] file写真 2014-08-07 16 24 01.jpg 228件 [詳細] file写真 2014-08-07 16 25 37.jpg 207件 [詳細] file写真 2014-08-07 16 24 43.jpg 254件 [詳細] file写真 2014-07-25 16 36 37.jpg 226件 [詳細] file過程2’.png 273件 [詳細] file課題2.png 264件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2014-09-09 (火) 02:12:40