2014a/Member/mame/Mission2 の変更点 - ロボティクス入門ゼミ

追加された行はこの色です。
削除された行はこの色です。
2014a/Member/mame/Mission2 へ行く。
2014a/Member/mame/Mission2 の差分を削除
[[2014a/Member]]

目次

#contents

*課題の説明 [#r13fce05]

詳しくは[[2014a/Mission2]]を見てください。

*メンバー [#na188e61]

tadanobo

mame

dai

bgpad

*ロボット本体 [#bdd03821]

&ref(2014a/Member/mame/Mission2/S__2424847.jpg,30%);

アーム、胴体、ドライブベースと、パーツごとに分けたので組み立て、取り外しが容易である

**アーム [#y6f68c61]

&ref(2014a/Member/mame/Mission2/写真 2014-07-25 16 36 37(1).jpg,25%);
①コップをつかむ
&ref(2014a/Member/mame/Mission2/写真 2014-08-07 16 24 50.jpg,7%);
②アームが下りる
&ref(2014a/Member/mame/Mission2/aiueo.jpg,35%);
③他のコップに重ねる

**ドライブベース [#je9abe0b]

&ref(2014a/Member/mame/Mission2/写真 2014-08-07 16 25 32.jpg,10%);
機体が重いのでベースは頑丈にした。また、後輪はスムーズに動くように大きなものにした


*作戦 [#b8f08a37]

コップを確実にとるために二回コップを判断するプログラムを入れた。

超音波センサーでコップの位置を確認し、光センサーでコップの番号を見分けるようにした。

また、少しでも角度がずれないようにコップをとる毎に角度を修正するプログラムを入れた。

主に行動するための親機、主にコップをとるための子機にプログラムを分けた。


*プログラム [#z80241ef]

**基本のプログラム [#yb10803f]
***定義 [#ua28c278]
 #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 //紙コップ判別用光センサー

***メインプログラム [#k87d7c7e]
 
 void InitSensor(){
    SetSensorLowspeed(DIST_SENSOR);
    SetSensorLight(TYPE_SENSOR);
 }

**機体の行動するプログラム [#edcd34a9]
***定義 [#w23ba2df]
 #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)

***メインプログラム [#h5000208]
 
 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);
    }
 }

**コップを探すためのプログラム [#w225d5ad]
 
***定義 [#p8f79d0c]
 #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)

***メインプログラム [#ie6d8cf2]
 
 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);
 } 

**コップを判断するためのプログラム [#h13b91a8]
***定義 [#uac0f5e7]
 #define DIRECTION_DEBUG 1
 #define DEBUG_DRIVETYPE 2
 #define DEBUG_CUPTYPE 3　
 #define DEBUG_CUPSENSOR 3

***メインプログラム [#c68e677d]
 
 void Debug(int line, string text){
   TextOut(0, (8 - line) * 8, StrCat(text, "                    "));
 } 
 
 void DebugText(int line, string name, string value){
    Debug(line, StrCat(name, ": ", value));
 }
 
 void DebugNumber(int line, string name, int num){
    DebugText(line, name, NumToStr(num));
 }
 
 void DebugSound(int freq){
    PlayTone(freq, 100);
 }

**Bluetoothのプログラム [#e5c11d2e]
***定義 [#pea36e5f]
 #define BT_CONN 1
 #define BT_MAILBOX MAILBOX3
 
 #define BT_NULL 0
 #define BT_CATCH 1
 #define BT_STACK 2
 #define BT_FINISH 3

***メインプログラム [#m0f1915a]
 
 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);
 }

**アームのプログラム [#n48ddf38]
***定義 [#x6c230d3]
 #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

***メインプログラム [#o58a78f8]
 
 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();
 }

**角度調整のプログラム [#b5f9e30c]
***定義 [#xe4a73ec]
 #define BODY_WIDTH 12.6
 #define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)

***メインプログラム [#d9b4c61d]
 
 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;
 }

**親機のプログラム [#w0bdc2b7]
***定義 [#p7c1d702]
 #include "debug.h"
 #include "bluetooth.h"
 #include "robot.h"
 #include "direction.h"
 #include "drivebase.h"
 #include "search.h"
 #include "arm.h"

***メインプログラム [#reaffc66]

 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());
 } 

***メインタスク [#i8a27cb2]

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

**子機のプログラム [#r0bb1a13]
***定義 [#g2a189fc]
 #include "debug.h"
 #include "bluetooth.h"
 #include "robot.h"
 #include "arm.h"

***メインタスク [#i1fb1f38]

 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);
 }


*反省と感想 [#dc1c78a7]

成功率を上げるために何度も調整し直した。

本番のロボコンで最後のコップをつかみ損ねてしまった。

かなり安定性の高い機体とプログラムになった。

機体の核を作ってくれたメンバー、共に調整してくれたメンバー、
プログラムを中心に書いてくれたメンバーに感謝である。