2014a/Member

目次

課題の説明

課題1で使用したコースを2枚用いて課題2を行う。1〜3と書かれた紙コップを2組作り、ランダムに配置された1組の紙コップを運び、もう1組の紙コップに重ねる。紙コップは色を塗ることで光センサーで識別できるようにする。

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

メンバー

・tadanobo

・mame

・dai

・bgpat

ロボット本体

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

ロボットの本体を2つ使用し、bluetoothを用いて連動させる。マスターの機器で走行とセンサーをコントロールし、スレーブの機器でアームの制御を行う。超音波センサーで紙コップを探し、アームを降ろし、掴み、持ち上げる。

写真 2014-08-07 16 24 54.jpg

紙コップを落とさないために、持ち上げた後、持ち直す。そのとき、光センサーで紙コップの色を識別する。そして、紙コップを配置された紙コップまで運び、重ねる。

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

後輪の開発に苦労した。丈夫でかつ、滑りやすくするために工夫した。

工夫した点

まず、紙コップを持ち上げる機構を作ることに時間をかけた。複数の案が出て、全員で試行錯誤し作り上げた。出た案には、タイヤのゴムによる摩擦をつかったものや、アームを使うもの、また、モーターを1つ使うか、2つ使うかなど色々な案から今回の案を選んだ。多少のずれは修正でき、とても高い確率で持ち上げれる機構が完成した。

次に、ドライブベースだが、機器を2つ使うために、安定感のあるものを作った。また、各センサーをベストな場所に取り付けるために、ロボットを改良した。

そして、プログラムは、bgpatさんを中心に作成し、時間をかけ何度も調整を繰り返し、より確実性を高めるようにした。

プログラム

各センサーの設定

定義

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

紙コップを探すプログラム

定義

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

方向調整のプログラム

定義

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

コップキャッチまで微調整するプログラム

定義

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

アームを動かすプログラム

定義

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

コップの色を認識するプログラム

定義

#define DIRECTION_DEBUG 1
#define DEBUG_DRIVETYPE 2
#define DEBUG_CUPTYPE 3
#define DEBUG_CUPSENSOR 3

タスク

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のプログラム

定義

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

マスターメインタスク

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

マスターのプログラムは上記のプログラムを含み、主にロボット本体を制御を行う。

スレーブメインタスク

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

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

スレーブのプログラムは上記のいくつかのプログラムを含み、主にアームの制御を行う。

反省・感想

・課題1に比べて、複雑なプログラミングが多く、製作に苦労した。

・より確実に紙コップを掴むため、アームの部分の部分の製作に時間がかかった。

・後輪の性能を高めるために、試行錯誤を繰り返した。

・プログラミングを部分的にテストすることで短時間での改善ができるようにした。

・プログラミングを主体として作ってくれた仲間に感謝したい。


添付ファイル: file写真 2014-08-07 16 24 54.jpg 126件 [詳細] file写真 2014-08-07 16 25 37.jpg 136件 [詳細] file写真 2014-07-25 16 36 37.jpg 149件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2014-08-11 (月) 23:58:37