- 追加された行はこの色です。
- 削除された行はこの色です。
[[2014a/Member]]
目次
#contents
*課題の説明 [#sa503f4f]
課題1で使用したコースを2枚用いて課題2を行う。1〜3と書かれた紙コップを2組作り、ランダムに配置された1組の紙コップを運び、もう1組の紙コップに重ねる。紙コップは色を塗ることで光センサーで識別できるようにする。
詳しい説明は[[2014a/Mission2]]を見てください。
*メンバー [#i813c647]
・tadanobo
・mame
・dai
・bgpat
*ロボット本体 [#d1181d24]
&ref(2014a/Member/dai/Mission2/写真 2014-07-25 16 36 37.jpg,15%);
ロボットの本体を2つ使用し、bluetoothを用いて連動させる。マスターの機器で走行とセンサーをコントロールし、スレーブの機器でアームの制御を行う。超音波センサーで紙コップを探し、アームを降ろし、掴み、持ち上げる。
&ref(2014a/Member/dai/Mission2/写真 2014-08-07 16 24 54.jpg,15%);
紙コップを落とさないために、持ち上げた後、持ち直す。そのとき、光センサーで紙コップの色を識別する。
紙コップを落とさないために、持ち上げた後、持ち直す。そのとき、光センサーで紙コップの色を識別する。そして、紙コップを配置された紙コップまで運び、重ねる。
&ref(2014a/Member/dai/Mission2/写真 2014-08-07 16 25 37.jpg,15%);
後輪の開発に苦労した。丈夫でかつ、滑りやすくするために工夫した。
* プログラム[#zec3fe48]
*工夫した点 [#m59c257d]
long timerStarted = 0; //タイマーをセット
まず、紙コップを持ち上げる機構を作ることに時間をかけた。複数の案が出て、全員で試行錯誤し作り上げた。出た案には、タイヤのゴムによる摩擦をつかったものや、アームを使うもの、また、モーターを1つ使うか、2つ使うかなど色々な案から今回の案を選んだ。多少のずれは修正でき、とても高い確率で持ち上げれる機構が完成した。
void StartTimer(){
timerStarted = CurrentTick(); //時間を数え始める
}
次に、ドライブベースだが、機器を2つ使うために、安定感のあるものを作った。また、各センサーをベストな場所に取り付けるために、ロボットを改良した。
long StopTimer(){
return CurrentTick() - timerStarted;
}
そして、プログラムは、bgpatさんを中心に作成し、時間をかけ何度も調整を繰り返し、より確実性を高めるようにした。
void Debug(int line, string text){
TextOut(0, (8 - line) * 8, StrCat(text, " "));
}
* プログラム[#zec3fe48]
void DebugText(int line, string name, string value){
Debug(line, StrCat(name, ": ", value));
}
**各センサーの設定 [#o848b692]
void DebugNumber(int line, string name, int num){
DebugText(line, name, NumToStr(num));
}
***定義 [#gea5dac9]
void DebugSound(int freq){
PlayTone(freq, 100);
}
#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 TRACE_SENSOR S1 //トレース用光センサー
#define DIST_SENSOR S2 //距離測定用超音波センサー
#define TYPE_SENSOR S3 //紙コップ判別用光センサー
***タスク [#ucfa76dd]
void InitSensor(){
SetSensorLight(TRACE_SENSOR);
SetSensorLowspeed(DIST_SENSOR);
SetSensorLight(TYPE_SENSOR);
SetSensorLowspeed(DIST_SENSOR);
SetSensorLight(TYPE_SENSOR);
}
**紙コップを探すプログラム [#la9d68ba]
***定義 [#j08cbcae]
#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)
***タスク [#e5e2ef4e]
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);
}
**方向調整のプログラム [#db9440c9]
***定義 [#m65c11e0]
#define BODY_WIDTH 12.6
#define DIRECTION_COE (0.044 / BODY_WIDTH * 360 / 2 / PI)
#define DIRECTION_DEBUG 1
***タスク [#id038b74]
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;
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();
directionTimer = GetDirection();
}
float StopDirectionTimer(){
return GetDirection() - directionTimer;
return GetDirection() - directionTimer;
}
#define THRESHOLD 43
#define SPEED_FORWARD 70
#define SPEED_TURN 80
#define SPEED_SPIN 60
#define SPEED_FORWARD_LOW 25
#define SPEED_SPIN_LOW 40
**コップキャッチまで微調整するプログラム [#c2b4ddee]
#define TraceSensor SENSOR_1
***定義 [#xe8737cb]
#define DEBUG_TRACETYPE 3
#define DEBUG_TRACESENSOR 2
#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)
#define WHEEL_STRAIGHT (80 / 1715)
***タスク [#j38d87aa]
int lastTrace = -1;
int forwardCount = 0;
float rightDirection = 0;
void DebugDriveType(string type, int speed){
DebugNumber(DEBUG_DRIVETYPE, type, speed);
}
void Stay(){
Off(BOTH_WHEEL);
DebugText(DEBUG_TRACETYPE, "State", "Stay");
Off(BOTH_WHEEL);
DebugDriveType("Stay", 0);
}
void GoForward(){
OnFwdSync(BOTH_WHEEL, SPEED_FORWARD, 0);
DebugText(DEBUG_TRACETYPE, "State", "GoForward");
void StayWait(){
Stay();
Wait(STAY_WAIT);
}
void GoBack(){
OnRevSync(BOTH_WHEEL, SPEED_FORWARD, 0);
DebugText(DEBUG_TRACETYPE, "State", "GoBack");
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 SpinLeft(){
OnFwdSync(BOTH_WHEEL, SPEED_SPIN, -100);
DebugText(DEBUG_TRACETYPE, "State", "SpinLeft");
}
void SpinRight(){
OnFwdSync(BOTH_WHEEL, SPEED_SPIN, 100);
DebugText(DEBUG_TRACETYPE, "State", "SpinRight");
void DriveStraight(int speed){
Drive(speed, 0, "Straight");
}
void GoForwardLow(){
OnFwdSync(BOTH_WHEEL, SPEED_FORWARD_LOW, 0);
DebugText(DEBUG_TRACETYPE, "State", "GoForwardLow");
}
void GoBackLow(){
OnRevSync(BOTH_WHEEL, SPEED_FORWARD_LOW, 0);
DebugText(DEBUG_TRACETYPE, "State", "GoBackLow");
void DriveLeft(int speed){
Drive(speed, -100, "Left");
}
void SpinLeftLow(){
OnFwdSync(BOTH_WHEEL, SPEED_SPIN_LOW, -100);
DebugText(DEBUG_TRACETYPE, "State", "SpinLeftLow");
void DriveRight(int speed){
Drive(speed, 100, "Right");
}
void SpinRightLow(){
OnFwdSync(BOTH_WHEEL, SPEED_SPIN_LOW, 100);
DebugText(DEBUG_TRACETYPE, "State", "SpinRightLow");
void Forward(){
DriveStraight(SPEED_STRAIGHT);
}
void TurnLeft(){
Stay();
OnFwd(RIGHT_WHEEL, SPEED_TURN);
DebugText(DEBUG_TRACETYPE, "State", "TurnLeft");
}
void TurnRight(){
Stay();
OnFwd(LEFT_WHEEL, SPEED_TURN);
DebugText(DEBUG_TRACETYPE, "State", "TurnRight");
void ForwardLow(){
DriveStraight(SPEED_STRAIGHT_LOW);
}
void UntilBlack(){
until(TraceSensor < THRESHOLD);
void Back(){
DriveStraight(-SPEED_STRAIGHT);
}
void UntilWhite(){
until(TraceSensor > THRESHOLD);
void BackLow(){
DriveStraight(-SPEED_STRAIGHT_LOW);
}
void UntilLine(){
UntilBlack();
UntilWhite();
void SpinLeft(){
DriveLeft(SPEED_SPIN);
}
int Trace(){
int n;
float d;
if(TraceSensor < THRESHOLD - 8){
SpinLeft();
n = 0;
}else if(TraceSensor < THRESHOLD - 2){
TurnLeft();
n = 1;
}else if(TraceSensor < THRESHOLD + 2){
GoForward();
n = 2;
}else if(TraceSensor < THRESHOLD + 8){
TurnRight();
n = 3;
}else{
SpinRight();
d = GetDirection();
if(lastTrace == 4){
if(d - rightDirection < -360){
PlayTone(440, 100);
GoBack();
UntilBlack();
}
}else{
rightDirection = d;
}
n = 4;
}
DebugNumber(DEBUG_TRACESENSOR, "State", TraceSensor);
if(n == 2){
forwardCount++;
}else{
forwardCount = 0;
}
lastTrace = n;
return n;
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);
}
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);
}
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 GoForwardTo(int x){
float a = MotorRotationCount(LEFT_WHEEL);
GoForward();
until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT >= x)DebugNumber(6, "fwd", x - (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT);
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 GoForwardToLow(int x){
float a = MotorRotationCount(LEFT_WHEEL);
GoForwardLow();
until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT >= x)DebugNumber(6, "fwd", x - (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT);
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 GoBackTo(int x){
float a = MotorRotationCount(LEFT_WHEEL);
GoBack();
until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT <= -x)DebugNumber(6, "back", (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT - x);
Stay();
void GoStraight(int x){
if(x > 0){
GoForward(x);
}else{
GoBack(-x);
}
}
void GoBackToLow(int x){
float a = MotorRotationCount(LEFT_WHEEL);
GoBackLow();
until((MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT <= -x)DebugNumber(6, "back", (MotorRotationCount(LEFT_WHEEL) - a) * WHEEL_STRAIGHT - x);
Stay();
void GoStraightLow(int x){
if(x > 0){
GoForwardLow(x);
}else{
GoBackLow(-x);
}
}
#define DIST_CATCH 13
#define DIST_STACK 13
#define DistSensor SensorUS(DIST_SENSOR)
void TowardCup(int degree){
int min = DistSensor;
float right = GetDirection();
SpinLow(-degree * 0.5);
StartDirectionTimer();
SpinLeftLow();
while(StopDirectionTimer() < degree){
if(DistSensor < min){
min = DistSensor;
right = GetDirection();
}
}
SpinRightLow();
until(GetDirection() <= right);
Stay();
}
**アームを動かすプログラム [#j1c77bb4]
int PrepareCatch(){
int l = 0;
TowardCup(90);
GoForwardToLow(DistSensor - 5);
l += DistSensor - 5;
Stay();
TowardCup(120);
GoBackToLow(DIST_CATCH - DistSensor);
l += DistSensor - DIST_CATCH;
Stay();
return l * WHEEL_STRAIGHT;
}
***定義 [#we2f8516]
int PrepareStack(){
int l = 0;
TowardCup(90);
GoForwardToLow(DistSensor - 5);
l += DistSensor - 5;
Stay();
TowardCup(120);
GoBackToLow(DIST_STACK - DistSensor);
l += DistSensor - DIST_STACK;
Stay();
return l * WHEEL_STRAIGHT;
}
#define DOWN_POWER 30
#define HOLD_DOWN_POWER 50
#define UP_POWER 90
#define POWER 20
#define HOLD_UP_POWER 50
#define HOLD_POWER 20
#define RELEASE_POWER 80
#define ARM1_DEGREE 90
#define ARM2_DEGREE 210
#define CUP1 45 //コップ1のしきい値
#define CUP2 30 //コップ2のしきい値
#define CUP3 20 //コップ3のしきい値
#define CUP1 55
#define CUP2 37
#define CUP3 23
#define TypeSensor SENSOR_3
void InitArm(){
SetSensorLight(DIST_SENSOR);
}
***タスク [#ffbd5461]
void DownArm(){
Off(ARM_BASE);
Wait(100);
RotateMotor(ARM_BASE, DOWN_POWER, ARM1_DEGREE);
OnFwd(ARM_BASE, 50);
Wait(1000);
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, 50);
Wait(2000);
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, POWER);
int s = 0;
while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE){
if(DistSensor > s){
s = DistSensor;
}
};
}
OnFwd(ARM_HAND, HOLD_POWER);
while(MotorRotationCount(ARM_HAND) < ARM2_DEGREE);
}
void ReleaseCup(){
RotateMotor(ARM_HAND, POWER, -ARM2_DEGREE);
RotateMotor(ARM_HAND, RELEASE_POWER, -ARM2_DEGREE);
}
void CatchCup(){
HoldCup();
Wait(500);
UpArm();
Wait(500);
ReleaseCup();
HoldCup();
HoldCup();
Wait(500);
UpArm();
Wait(500);
ReleaseCup();
HoldCup();
}
void StackCup(){
DownArm();
Wait(500);
ReleaseCup();
Wait(500);
DownArm();
Wait(500);
ReleaseCup();
Wait(500);
}
int GetCupType(){
int n = 0;
DebugNumber(2, "sensor", TypeSensor);
if(TypeSensor > CUP1){
n = 1;
DebugSound(TONE_C4);
}else if(TypeSensor > CUP2){
n = 2;
DebugSound(TONE_D4);
}else if(TypeSensor > CUP3){
n = 3;
DebugSound(TONE_E4);
}
return n;
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();
}
**コップの色を認識するプログラム [#sa224259]
***定義 [#b5fed68e]
#define DIRECTION_DEBUG 1
#define DEBUG_DRIVETYPE 2
#define DEBUG_CUPTYPE 3
#define DEBUG_CUPSENSOR 3
***タスク [#j547aede]
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のプログラム [#jf700c79]
***定義 [#a25e4aaf]
#define BT_CONN 1
#define BT_MAILBOX MAILBOX3
#define BT_NULL 0
#define BT_CATCH 1
#define BT_STACK 2
#define BT_FINISH 4
#define BT_FINISH 3
***タスク [#v7711bb2]
void BTStart(string file){
until(BluetoothStatus(BT_CONN) == NO_ERR);
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;
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);
if(isMaster){
SendRemoteNumber(BT_CONN, BT_MAILBOX, msg);
}else{
SendResponseNumber(BT_MAILBOX, msg);
}
SendResponseNumber(BT_MAILBOX, msg);
}
}
void GoCup1(){
InitSensor();
Spin(-90);
Stay();Wait(100);
GoForwardTo(60);
Stay();Wait(100);
Spin(135);
Stay();Wait(100);
GoForwardTo(10);
Stay();Wait(100);
void BTStop(){
BTSendMessage(BT_FINISH, true);
}
void BackCup1(int l){
GoBackTo(10 + l);
Stay();Wait(100);
Spin(-direction - 90);
Stay();Wait(100);
GoBackTo(50);
Stay();Wait(100);
Spin(90);
**マスターメインタスク [#v78a3d33]
#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 GoCup2(){
Spin(-90);
Stay();Wait(100);
GoForwardTo(60);
Stay();Wait(100);
Spin(45);
Stay();Wait(100);
void GotoStackCup(){
GoForward(2);
StayWait();
Spin(-90);
StayWait();
GoBack(55);
StayWait();
Spin(90);
StayWait();
}
void BeginCupA(){
Spin(45);
StayWait();
GoForward(15);
StayWait();
}
void BackCup2(int l){
GoBackTo(10 + l); //@
Stay();Wait(100);
Spin(-direction - 90);
Stay();Wait(100);
GoBackTo(50); //@
Stay();Wait(100);
Spin(90);
void BeginCupB(){
Spin(-60);
StayWait();
}
void GoCup3(){
InitSensor();
Spin(-90);
Stay();Wait(100);
GoForwardTo(60);
Stay();Wait(100);
Spin(90);
Stay();Wait(100);
GoForwardTo(30);
Stay();Wait(100);
void BeginCupC(){
GoForward(30);
StayWait();
}
void BackCup3(){
GoBackTo(35); //@
Stay();Wait(100);
Spin(-direction - 100); //@
Stay();Wait(100);
GoBackTo(60);
Stay();Wait(100);
Spin(90);
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(){
int l;
BTStart("slave3.rxe");
InitSensor();
GoCup1();
l = PrepareCatch();
//MasterCatchCup();
BTSendMessage(BT_CATCH, true);
BTReceiveMessage();
BackCup1(l);
int type = GetCupType();
if(type == 1){ //コップ1の場合
Spin(45); //45度回転
GoForwardTo(15);
}else if(type == 2){
Spin(-45);
GoForwardTo(15);
}else{
GoForwardTo(30);
GoForward(4);
StayWait();
BTStart("slave.rxe");
InitSensor();
for(int i = 1; i <= 3; i++){
Cup(i);
}
PrepareStack();
//MasterCatchCup();
BTSendMessage(BT_STACK, true);
BTReceiveMessage();
Stay();Wait(100);
if(type == 1){
Spin(45 - GetDirection());
GoBackTo(15);
}else if(type == 2){
Spin(-45 - GetDirection());
GoBackTo(15);
}else{
Spin(-GetDirection());
GoBackTo(30);
}
Spin(-GetDirection());
Stay();Wait(100);
GoCup2();
l = PrepareCatch();
//MasterCatchCup();
BTSendMessage(BT_CATCH, true);
BTReceiveMessage();
BackCup2(l);
type = GetCupType();
if(type == 1){
Spin(45);
GoForwardTo(15);
}else if(type == 2){
Spin(-45);
GoForwardTo(15);
}else{
GoForwardTo(30);
}
PrepareStack();
//MasterCatchCup();
BTSendMessage(BT_STACK, true);
BTReceiveMessage();
Stay();Wait(100);
if(type == 1){
Spin(45 - GetDirection());
GoBackTo(15);
}else if(type == 2){
Spin(-45 - GetDirection());
GoBackTo(15);
}else{
Spin(-GetDirection());
GoBackTo(30);
}
Spin(-GetDirection());
Stay();Wait(100);
GoCup3();
PrepareCatch();
//MasterCatchCup();
BTSendMessage(BT_CATCH, true);
BTReceiveMessage();
BackCup3();
type = GetCupType();
if(type == 1){
Spin(45);
GoForwardTo(15);
}else if(type == 2){
Spin(-45);
GoForwardTo(15);
}else{
GoForwardTo(30);
}
PrepareStack();
//MasterCatchCup();
BTSendMessage(BT_STACK, true);
BTReceiveMessage();
Stay();Wait(100);
if(type == 1){
Spin(45 - GetDirection());
GoBackTo(15);
}else if(type == 2){
Spin(-45 - GetDirection());
GoBackTo(15);
}else{
Spin(-GetDirection());
GoBackTo(30);
}
Spin(-GetDirection());
Stay();Wait(100);
BTSendMessage(BT_FINISH, true);
BTStop();
}
マスターのプログラムは上記のプログラムを含み、主にロボット本体を制御を行う。
**スレーブメインタスク [#q8ee98eb]
#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);
}
スレーブのプログラムは上記のいくつかのプログラムを含み、主にアームの制御を行う。
* 反省・感想[#ja007ef1]
・課題1に比べて、複雑なプログラミングが多く、製作に苦労した。
・より確実に紙コップを掴むため、アームの部分の部分の製作に時間がかかった。
・後輪の性能を高めるために、試行錯誤を繰り返した。
・プログラミングを部分的にテストすることで短時間での改善ができるようにした。
・プログラミングを主体として作ってくれた仲間に感謝したい。