2014b/Member/arisyka/Mission3 の履歴の現在との差分(No.2)

履歴一覧
差分を表示
ソースを表示
2014b/Member/arisyka/Mission3 へ行く。
- 1 (2015-02-06 (金) 02:38:47)
- 2 (2015-02-08 (日) 10:15:47)
- 3 (2015-02-08 (日) 14:41:29)
- 4 (2015-02-08 (日) 20:06:05)
- 5 (2015-02-10 (火) 15:56:25)
- 6 (2015-02-11 (水) 14:21:01)
追加された行はこの色です。
削除された行はこの色です。
[[2014b/Member]]

#contents

*はじめに [#o4d8a879]
課題3はスタート地点からロボットを進め、2つの紙コップにそれぞれ入っている2個のボールを入れ替えるというものである。
([[2014年後期/課題3:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMission3]]参照)

*マスター型 [#hacfc6a7]
　私たちの班のメンバーは、
-[[edaeda:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMember%2Fedaeda%2FMission3]]
-[[knee:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMember%2Fknee%2FMission3]]
- kuryu

#define CONN 1 
と私の4人である。

#define SPEED 50
#define SPEED_SLOW 40
私たちの班の考案は、アームとレールを作り、アームを使って４つのボールをレールにのせ、レールを傾かせることで紙コップにボールを入れるというものである。

#define turn_right_here OnFwd(OUT_B,SPEED_SLOW);OnFwd(OUT_C,-SPEED_SLOW);
*ロボット、またはその動作の説明 [#h9193c48]
**全体 [#ne8db113]
#ref(2014b/Member/arisyka/Mission3/IMG_3870.JPG,20%)
写真のような構造である。

 float GetAngle(float d) {
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0; return ang;

}
右と左のタイヤの負荷がそれぞれ異なるため、プログラムで左右のタイヤの出力を変えることで直進や回転、旋回をスムーズに行わせる。

**アーム [#y4bd3434]
#ref(2014b/Member/arisyka/Mission3/IMG_3869.gif,30%)
上の写真の赤で囲まれた部分。
1のモーターでアーム全体を上げ下げし、2のモーターでアームの挟む動きを操作する。

const float diameter = 5.45; const float track = 15; const float pi = 3.1415;
アームを動かすために使った定義は以下のようである。
 #define SPEED1 45
 #define SPEED2 60
 #define SPEED3 10
 #define SPEED4 30
 #define SPEED5 50
 #define SPEED6 25

void fwdDist(float d) {
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);

}

void turnAng(long ang) {
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);

}

int searchDirection(long ang) {
long angle,tacho_min=0,tacho_corr;
int d_min;
d_min=300;
angle = (track/diameter)*ang;
turnAng(ang/2);
ResetTachoCount(OUT_BC);
OnFwdSync(OUT_BC,SPEED_SLOW,-100);
while(MotorTachoCount(OUT_B)<=angle){
 if(SensorUS(S1)<d_min){
  d_min=SensorUS(S1);
  PlaySound(SOUND_UP);
  tacho_min=MotorTachoCount(OUT_B);
 float GetAngle(float d) {　　　　　　　　　　//進む距離を計算する
 const float diameter=5.45;　　　　　　　　　　//タイヤの直径
 const float pi=3.1415;　　　　　　　　　　　　//円周率
 float ang = d/(diameter*pi)*360.0; return ang; //角度を計算する
 }
}
OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
Wait(14);
Off(OUT_BC);Wait(500);
return d_min;

}

task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
ここの定義はアーム全体の動きを定義している。(上の写真の①のモーターによる動き)
 #define DOWN RotateMotor(OUT_A,SPEED3,105);　　//アーム全体を下げる
 #define UP RotateMotor(OUT_A,-SPEED5,105);  　//アーム全体を上げる
 #define LDown RotateMotor(OUT_A,SPEED3,25);　　
 #define LUp RotateMotor(OUT_A,-SPEED2,25);
 #define Little LDown;Wait(2000);LUp;          //ボールをレールに確実にのせるための動き

RotateMotor(OUT_BC,50,GetAngle(50));
OnFwd(OUT_B,30);
OnRev(OUT_C,50);
Wait(1500);
RotateMotor(OUT_BC,50,GetAngle(30));
#ref(2014b/Member/arisyka/Mission3/armループ2.gif,45%)

int d = searchDirection(120);

if(d > 15){
 fwdDist(d-15.0);}
else {fwdDist(d-15);}
PlaySound(SOUND_UP);
ここの定義はアームのハサミ部分の動きを定義している。(上の写真の②のモーターによる動き)
 #define Catch RotateMotor(OUT_B,-SPEED4,105);　　//アームでつかむ
 #define Separate RotateMotor(OUT_B,SPEED4,105);  //アームを離す

turn_right_here;
Wait(200);
#ref(2014b/Member/arisyka/Mission3/catch&separate.gif,40%)

RemoteStartProgram(CONN, "arm time1.rxe");
}

**レール [#k920454a]
#ref(2014b/Member/arisyka/Mission3/IMG_3873.gif,30%)
上の写真の赤で囲まれた部分。また、橙色で囲まれた部分はボールを紙コップに入れる際に、紙コップが倒れないようにするためのストッパーである。倒す角度を調整することで紙コップに入れるボールの数を変える。

**angle move1 [#d59f8a17]
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
レールを動かすために使った動作は上記に書いたアームの定義と同じものを使用した。

RotateMotor(OUT_BC,50,GetAngle(50));
OnFwd(OUT_B,30);
OnRev(OUT_C,50);
Wait(1500);
RotateMotor(OUT_BC,50,GetAngle(30));
 #define Into1 RotateMotor(OUT_C,SPEED3,20);  　　//ボールを入れる(1回目)
 #define Return1 RotateMotor(OUT_C,-SPEED3,25);　 //レールを戻す

int d = searchDirection(120);
 #define Into2 RotateMotor(OUT_C,SPEED4,30);　　　//ボールを入れる(2回目)
 #define Return2 RotateMotor(OUT_C,-SPEED4,15);　 //レールを戻す

if(d > 15){
 fwdDist(d-15.0);}
else {fwdDist(d-15);}
PlaySound(SOUND_UP);

turn_right_here;
Wait(200);
*プログラム [#ec7cd753]
今回使用するプログラムは上記にも示したアーム･レールの動作のプログラムの他に、超音波センサを利用した動作のプログラム、Bluetoothを利用したプログラムである。

RemoteStartProgram(CONN, "arm time1.rxe");
}
**超音波センサを利用した動き [#l3e463dd]
ここで紹介しているプログラムは配られたプリントに書かれている参考のプログラムとほぼ同じものである。

**angle move2 [#j2aa6be8]
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
 #define SPEED 50
 #define SPEED_SLOW 40

int angle = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle);
 float GetAngle(float d) {　　　　　　　　　　//進む距離を計算する
 const float diameter=5.45;　　　　　　　　　　//タイヤの直径
 const float pi=3.1415;　　　　　　　　　　　　//円周率
 float ang = d/(diameter*pi)*360.0; return ang; //角度を計算する
 }

int d = searchDirection(120);
if(d > 15){
 fwdDist(d-15.0);}
else {fwdDist(d-15);}
PlaySound(SOUND_UP);
 const float diameter = 5.45;　　　　　　　　　
 const float track = 15;　　　　　　　　　　　//タイヤのトレッド幅
 const float pi = 3.1415;
 void fwdDist(float d) {　　　　　　　　　　　//距離 d ㎝前進
 long angle;
 angle = d/(diameter*pi)*360.0;
 RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);
 }

turn_right_here;
Wait(200);
 void turnAng(long ang) {　　　　　　　　　　//角度 ang 度の時計回りの旋回
 long angle;
 angle = track/diameter * ang;
 RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);
 }

RemoteStartProgram(CONN, "arm time2.rxe");
}

**angle move3 [#h9f89ef8]
#define CONN 1
#define SPEED 50
#define SPEED_SLOW 40

#define turn_right_here OnFwd(OUT_C,SPEED);OnRev(OUT_B,SPEED);

 float GetAngle(float d) {
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0; return ang;

}


const float diameter = 5.45; const float track = 15; const float pi = 3.1415;

void fwdDist(float d) {
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);
}

void turnAng(long ang) {
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);

}

int searchDirection(long ang) {
long angle,tacho_min=0,tacho_corr;
int d_min;
d_min=300;
angle = (track/diameter)*ang;
turnAng(ang/2);
ResetTachoCount(OUT_BC);
OnFwdSync(OUT_BC,SPEED_SLOW,-100);
while(MotorTachoCount(OUT_B)<=angle){
 int searchDirection(long ang) {　　　　　　　//現在の方向を中心に ang 度の範囲で探し、障害物までの距離を返す
 long angle,tacho_min=0,tacho_corr;
 int d_min;
 d_min=300;　　　　　　　　　　　　　　　　　//仮の最小値
 angle = (track/diameter)*ang;　　　　　　　//旋回角度からタイヤの回転を計算
 turnAng(ang/2);　　　　　　　　　　　　　　//指定された角度の半分を旋回
 ResetTachoCount(OUT_BC);　　　　　　　　　//角度計測をリセット
 OnFwdSync(OUT_BC,SPEED_SLOW,-100);　　　　//反時計回りに旋回
 while(MotorTachoCount(OUT_B)<=angle){
 if(SensorUS(S1)<d_min){
  d_min=SensorUS(S1);
  d_min=SensorUS(S1);　　　　　　　　　　　//仮の最小値を更新
  PlaySound(SOUND_UP);
  tacho_min=MotorTachoCount(OUT_B);
 }
}
OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
Wait(14);
Off(OUT_BC);Wait(500);
return d_min;
 }
 OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
 until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
 Wait(14);　　　　　　　　　　　　　　　　　//微調整
 Off(OUT_BC);Wait(500);
 return d_min;
 }

}
 task main()
 {
 int d = searchDirection(120);　　　　　　　//現在の方向を中心に 120 度の範囲で探し、障害物までの距離を返す
 if(d > ○){
 fwdDist(d-○);}　　　　　　　　　　
 else {fwdDist(d-○);}　　　　　　　　　　　　//紙コップの方向を探した後音を鳴らし、前後の動きで○㎝手前で停止
 PlaySound(SOUND_UP);
 }

task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);

int angle = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle);
**Bluetoothを利用したプログラム [#x935b665]
2つのNXCがBluetoothでつながっている状態であることを前提で話を進める。
ここではマスター側からスレーブ側に指示を出すプログラムを紹介する。　

int d = searchDirection(120);
 #define CONN 1 　　　　　　　　　　　　　　　//スレーブ(通信相手)の接続番号

turn_right_here;
Wait(600);
 task main()
 {
 until (BluetoothStatus(CONN) == NO_ERR);　　//接続できるまで待機

if(d > 15){
 fwdDist(d);}
else {fwdDist(d);}
PlaySound(SOUND_UP);
 //マスター側のプログラム//

RemoteStartProgram(CONN, "rail1.rxe");
}
 RemoteStartProgram(CONN, "～.rxe");　　　　　//スレーブの「～」のプログラムを起動
 }　

逆にスレーブ側からマスター側に指示を出す場合は接続番号を0と変更する。

**angle move4 [#r0e5655e]
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
**問題点・改善点 [#l23eb6c6]
アームやレールの動作をするプログラムで、始めは全て角度を指定させてモーターを動かしていたが、モーターがその角度をぴったり回らない限り動き続ける、すなわち他の部品等でひっかかっていたらその動作は終わらないから次の動作に移り変わらないことが途中で判明したため、なるべく角度指定をせずに出力と時間でモーターを動かし、確実に動作をさせるように改善した。

int angle = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle);
今まで紹介してきたプログラムを複数のプログラムに分け、Bluetoothを利用することで全て繋ぎ合わせようと試みたが、マスター側からスレーブ側にプログラムを指示出すことは成功したものの、続けてスレーブ側からマスター側のプログラムを起動させようとするとマスター側が反応してくれない問題が起きた。マスター側とスレーブ側のNXCを変えてみたり、プログラムを少しだけ変えてみたりして試したものの、反応しないことに変わりはなかったため、マスター側のプログラムを1つにまとめて、スレーブ側のプログラムを実行している時はWaitを利用して待機させ、スレーブ側のプログラムが終わればマスター側の次のプログラムを実行させるよう変更した。

int d = searchDirection(120);
**スレーブ側 [#p93594c5]
***アーム動作（arm time） [#dd328ea5]
 #define SPEED1 45
 #define SPEED2 60
 #define SPEED3 10
 #define SPEED4 30
 #define SPEED5 50
 #define SPEED6 25
 float GetAngle(float d)
 {
 const float diameter=5.45;
 const float pi=3.1415;
 float ang = d/(diameter*pi)*360.0;
 return ang;
 }
 #define DOWN RotateMotor(OUT_A,SPEED3,105);
 #define UP RotateMotor(OUT_A,-SPEED5,105);  
 #define LDown RotateMotor(OUT_A,SPEED3,25);
 #define LUp RotateMotor(OUT_A,-SPEED2,25);

turn_right_here;
Wait(600);
 #define Catch RotateMotor(OUT_B,-SPEED4,105);
 #define Separate RotateMotor(OUT_B,SPEED4,105); 

if(d > 15){
 fwdDist(d);}
else {fwdDist(d);}
PlaySound(SOUND_UP);
 task main ()
 {
 OnFwd(OUT_A,10);
 Wait(1500);
 Catch;	　　　　　　　　　　　　　　//アーム全体を下げ、紙コップをつかむ

RemoteStartProgram(CONN, "rail2.rxe");
}
 OnRev(OUT_A,50);
 Wait(3500);　　　　　　　　　　　　//アーム全体を上げ、レールの上にボールをのせる
 Wait(2000);

 OnFwd(OUT_A,10);　　　　　　　　
 Wait(3500);　　　　　　　　　　　　//アーム全体を下げ、紙コップを置く
 Separate;

**move [#g2dac7b9]
#define CONN 1 
 OnRev(OUT_A,35);
 Wait(2000); 　　　　　　　　　　　//アーム全体を上げる(元の位置に戻す)
 Wait(500);
 }

#define SPEED 50
#define SPEED_SLOW 40

#define turn_right_here OnFwd(OUT_B,SPEED_SLOW);OnFwd(OUT_C,-SPEED_SLOW);

 float GetAngle(float d) {
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0; return ang;

}


const float diameter = 5.45; const float track = 15; const float pi = 3.1415;

void fwdDist(float d) {
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);

}

void turnAng(long ang) {
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);

}

int searchDirection(long ang) {
long angle,tacho_min=0,tacho_corr;
int d_min;
d_min=100;
angle = (track/diameter)*ang;
turnAng(ang/2);
ResetTachoCount(OUT_BC);
OnFwdSync(OUT_BC,SPEED_SLOW,-100);
while(MotorTachoCount(OUT_B)<=angle){
 if(SensorUS(S1)<d_min){
  d_min=SensorUS(S1);
  PlaySound(SOUND_UP);
  tacho_min=MotorTachoCount(OUT_B);
***レール動作（rail） [#u382c2a1]
 #define SPEED1 45
 #define SPEED2 60
 #define SPEED3 10
 #define SPEED4 30
 #define SPEED5 50
 float GetAngle(float d)
 {
 const float diameter=5.45;
 const float pi=3.1415;
 float ang = d/(diameter*pi)*360.0;
 return ang;
 }
}
OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
Wait(14);
Off(OUT_BC);Wait(500);
return d_min;

}
 #define Into1 RotateMotor(OUT_C,SPEED3,20);  //ボールを入れる
 #define Return1 RotateMotor(OUT_C,-SPEED3,25);
 #define Into2 RotateMotor(OUT_C,SPEED4,30);
 #define Return2 RotateMotor(OUT_C,-SPEED4,15);

#define anglemove2 
int angle = GetAngle(20);RotateMotor(OUT_BC,-SPEED,angle);OnFwd(OUT_B,-SPEED);OnFwd(OUT_C,SPEED);Wait(1500);angle += GetAngle(60);RotateMotor(OUT_BC,SPEED,angle);int d = searchDirection(120);if(d > 15){ fwdDist(d-15.0);}else {fwdDist(d-15);}PlaySound(SOUND_UP);turn_right_here;Wait(200);
#define anglemove3 int angle = GetAngle(20);RotateMotor(OUT_BC,-SPEED,angle);OnFwd(OUT_B,-SPEED);OnFwd(OUT_C,SPEED);Wait(1500);angle += GetAngle(60);RotateMotor(OUT_BC,SPEED,angle);int d = searchDirection(120);turn_right_here;Wait(600);if(d > 15){ fwdDist(d);}else {fwdDist(d);}PlaySound(SOUND_UP);
#define anglemove4 int angle = GetAngle(20);RotateMotor(OUT_BC,-SPEED,angle);OnFwd(OUT_B,-SPEED);OnFwd(OUT_C,SPEED);Wait(1500);angle += GetAngle(60);RotateMotor(OUT_BC,SPEED,angle);int d = searchDirection(120);turn_right_here;Wait(600);if(d > 15){ fwdDist(d);}else {fwdDist(d);}PlaySound(SOUND_UP);
 task main(){
 OnFwd(OUT_C,15);
 Wait(1000);
 Wait(3000);     　　　　　　　　//レールを傾け、4つの内2つだけボールを紙コップに入れる
 OnRev(OUT_C,30);
 Wait(500);

 Wait(5000);

#define count 7
#define sound PlaySound(SOUND_CLICK);

task main() {
SetSensorLowspeed(S1);

until (BluetoothStatus(CONN) == NO_ERR);

int nOnline=0;
RotateMotor(OUT_BC,50,GetAngle(50));
OnFwd(OUT_B,30);
OnRev(OUT_C,50);
Wait(3000);
RotateMotor(OUT_BC,50,GetAngle(50));

int d = searchDirection(120);

if(d > 15){
 fwdDist(d-15.0);}
else {fwdDist(d-15);}
PlaySound(SOUND_UP);

turn_right_here;
Wait(200);

RemoteStartProgram(CONN, "arm time1.rxe");

if(nOnline=0){
if(SensorUS(S3)<count){
    sound;
Wait(10000);
nOnline++;
int angle = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle);

int d = searchDirection(120);
if(d > 15){
 fwdDist(d-15.0);}
else {fwdDist(d-15);}
PlaySound(SOUND_UP);

turn_right_here;
Wait(200);

      RemoteStartProgram(CONN, "arm time2.rxe");
}
}
else if(nOnline=1){
if(SensorUS(S3)<count){
    sound;
    nOnline++;
  }
  }
else if(nOnline=2){
int angle = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle);

int d = searchDirection(120);

turn_right_here;
Wait(600);

if(d > 15){
 fwdDist(d);}
else {fwdDist(d);}
PlaySound(SOUND_UP);

       RemoteStartProgram(CONN, "rail1.rxe");
  if(SensorUS(S3)<count){
     sound;
     Wait(10000);
     nOnline++;
      
  }
  }
else if(nOnline=3){
int angle = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle);

int d = searchDirection(120);

turn_right_here;
Wait(600);

if(d > 15){
 fwdDist(d);}
else {fwdDist(d);}
PlaySound(SOUND_UP);

RemoteStartProgram(CONN, "rail2.rxe");
}
}

**ex [#f70778d9]
#define CONN 1 

#define SPEED 50
#define SPEED_SLOW 40

#define turn_right_here OnFwd(OUT_B,SPEED);OnFwd(OUT_C,-SPEED);

 float GetAngle(float d) {
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0; return ang;

}


const float diameter = 5.45; const float track = 15; const float pi = 3.1415;

void fwdDist(float d) {
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);

}

void turnAng(long ang) {
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);

}

int searchDirection(long ang) {
long angle,tacho_min=0,tacho_corr;
int d_min;
d_min=300;
angle = (track/diameter)*ang;
turnAng(ang/2);
ResetTachoCount(OUT_BC);
OnFwdSync(OUT_BC,SPEED_SLOW,-100);
while(MotorTachoCount(OUT_B)<=angle){
 if(SensorUS(S1)<d_min){
  d_min=SensorUS(S1);
  PlaySound(SOUND_UP);
  tacho_min=MotorTachoCount(OUT_B);
 OnFwd(OUT_C,25);
 Wait(1000);
 Wait(3000);       　　　　　　　//レールを傾け、レールに残っているボールを紙コップに入れる
 OnRev(OUT_C,30);
 Wait(2500);
 }
}
OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
Wait(14);
Off(OUT_BC);Wait(500);
return d_min;

}

#define anglemove2 int angle = GetAngle(20);RotateMotor(OUT_BC,-SPEED,angle);OnFwd(OUT_B,-SPEED);OnFwd(OUT_C,SPEED);Wait(1500);angle += GetAngle(60);RotateMotor(OUT_BC,SPEED,angle);int d = searchDirection(120);if(d > 15){ fwdDist(d-15.0);}else {fwdDist(d-15);}PlaySound(SOUND_UP);turn_right_here;Wait(200);
#define anglemove3 int angle = GetAngle(20);RotateMotor(OUT_BC,-SPEED,angle);OnFwd(OUT_B,-SPEED);OnFwd(OUT_C,SPEED);Wait(1500);angle += GetAngle(60);RotateMotor(OUT_BC,SPEED,angle);int d = searchDirection(120);turn_right_here;Wait(600);if(d > 15){ fwdDist(d);}else {fwdDist(d);}PlaySound(SOUND_UP);
#define anglemove4 int angle = GetAngle(20);RotateMotor(OUT_BC,-SPEED,angle);OnFwd(OUT_B,-SPEED);OnFwd(OUT_C,SPEED);Wait(1500);angle += GetAngle(60);RotateMotor(OUT_BC,SPEED,angle);int d = searchDirection(120);turn_right_here;Wait(600);if(d > 15){ fwdDist(d);}else {fwdDist(d);}PlaySound(SOUND_UP);
**マスター側 [#wa6a612a]
上記のプログラムの説明で利用した定義にプラスする。
 #define turn_right_here OnFwd(OUT_B,SPEED_SLOW);OnFwd(OUT_C,-SPEED_SLOW);  //右旋回

***本番用に用意したプログラム [#ne735f23]

 task main() {
 SetSensorLowspeed(S1);
 until (BluetoothStatus(CONN) == NO_ERR);

task main() {
SetSensorLowspeed(S1);
 OnFwd(OUT_BC,50);　　　　　　　　　　　　//100cm前に前進する
 Wait(5500);

until (BluetoothStatus(CONN) == NO_ERR);
 int d1 = searchDirection(120);　　　　　//紙コップの方向を探す
 if(d1 > 15){
 fwdDist(d1-15.0);}
 else {fwdDist(d1-15);}　　　　　　　　　//最低値を発見したら音を鳴らし、前後の動きで15㎝手前で停止
 PlaySound(SOUND_UP);

OnFwd(OUT_BC,50);
Wait(5500);
turn_right_here;
Wait(2500);
 turn_right_here;　　　　　　　　　　　　//アームが掴める位置までロボット全体を旋回させる
 Wait(2500);

RemoteStartProgram(CONN, "arm time.rxe");
Off(OUT_BC);
 RemoteStartProgram(CONN, "arm time.rxe");　//スレーブの「arm time」のプログラムを実行させる
 Off(OUT_BC);

Wait(10000);
Wait(10000);
Wait(10000);
 Wait(10000);
 Wait(10000);　　　　　　　　　　　　　　　//スレーブ側のプログラム実行中のため待機
 Wait(10000);

int angle1 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle1);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle1 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle1);
 int angle1 = GetAngle(20);
 RotateMotor(OUT_BC,-SPEED,angle1);　　　　//20cm後進する
 OnFwd(OUT_B,-SPEED);　　　　　　　　　　　
 OnFwd(OUT_C,SPEED);　　　　　　　　　　　//180度回転
 Wait(1500);
 angle1 += GetAngle(60);
 RotateMotor(OUT_BC,SPEED,angle1);　　　　//60cm前進する

int d2 = searchDirection(120);
if(d2 > 15){
 int d2 = searchDirection(120);
 if(d2 > 15){
 fwdDist(d2-15.0);}
else {fwdDist(d2-15);}
PlaySound(SOUND_UP);
 else {fwdDist(d2-15);}
 PlaySound(SOUND_UP);

turn_right_here;
Wait(200);
 turn_right_here;
 Wait(200);

RemoteStartProgram(CONN, "arm time.rxe");
Off(OUT_BC);
 RemoteStartProgram(CONN, "arm time.rxe");
 Off(OUT_BC);

Wait(10000);
Wait(10000);
Wait(10000);
 Wait(10000);
 Wait(10000);
 Wait(10000);

int angle2 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle2);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle2 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle2);
 int angle2 = GetAngle(20);
 RotateMotor(OUT_BC,-SPEED,angle2);
 OnFwd(OUT_B,-SPEED);
 OnFwd(OUT_C,SPEED);
 Wait(1500);
 angle2 += GetAngle(60);
 RotateMotor(OUT_BC,SPEED,angle2);

int d3 = searchDirection(120);

turn_right_here;
Wait(600);

if(d3 > 15){
 int d3 = searchDirection(120);
 turn_right_here;　　　　　　　　　　　　//微調整
 Wait(600);
 if(d3 > 15){
 fwdDist(d3);}
else {fwdDist(d3);}
PlaySound(SOUND_UP);
 else {fwdDist(d3);}　　　　　　　　　　//紙コップを見つけたら、離れている距離だけ前進(レールの下に紙コップを持っていくための動作)
 PlaySound(SOUND_UP);
 RemoteStartProgram(CONN, "rail.rxe");　//スレーブの「rail」のプログラムを実行させる
 Off(OUT_BC);

RemoteStartProgram(CONN, "rail1.rxe");
Off(OUT_BC);
 Wait(10000);
 Wait(10000);
 Wait(10000);

Wait(10000);
Wait(10000);
Wait(10000);
 int angle3 = GetAngle(20);
 RotateMotor(OUT_BC,-SPEED,angle3);
 OnFwd(OUT_B,-SPEED);
 OnFwd(OUT_C,SPEED);
 Wait(1500);
 angle3 += GetAngle(60);

int angle3 = GetAngle(20);
RotateMotor(OUT_BC,-SPEED,angle3);
OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(1500);
angle3 += GetAngle(60);
RotateMotor(OUT_BC,SPEED,angle3);

int d4 = searchDirection(120);

turn_right_here;
Wait(600);

if(d4 > 15){
 RotateMotor(OUT_BC,SPEED,angle3);
 int d4 = searchDirection(120);
 turn_right_here;
 Wait(600);
 if(d4 > 15){
 fwdDist(d4);}
else {fwdDist(d4);}
PlaySound(SOUND_UP);
 else {fwdDist(d4);}
 PlaySound(SOUND_UP);
 RemoteStartProgram(CONN, "rail.rxe");
 }

RemoteStartProgram(CONN, "rail2.rxe");
}
***最終的に使用したプログラム [#sb4129f9]
本番でsearchDirectionが上手く作動しなかったため、最終手段として超音波センサを利用せずにボールの入った紙コップのところまで行かせ、別の紙コップにボールを入れるところまで、ほぼOnFwdを使って移動させた。

 task main() {
 SetSensorLowspeed(S1);
 until (BluetoothStatus(CONN) == NO_ERR);
 OnFwd(OUT_BC,50);
 Wait(5500);　　　　　　　　　　　　　　　//100cmほど前進する

**ex2 [#gb9319b5]
 turn_right_here;　　　　　　　　　　　　//アームが掴める位置まで旋回する
 Wait(2000);

#define CONN 1 
#define SPEED 50
#define SPEED_SLOW 40
 RemoteStartProgram(CONN, "arm time.rxe");　//スレーブの「rail」のプログラムを実行させる
 Off(OUT_BC);

#define turn_right_here OnFwd(OUT_B,SPEED);OnFwd(OUT_C,-SPEED);
 Wait(10000);
 Wait(10000);　　　　　　　　　　　　　　//スレーブ側のプログラム実行中のため待機

 float GetAngle(float d) {
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0; return ang;
 OnFwd(OUT_B,-SPEED);
 OnFwd(OUT_C,SPEED);
 Wait(700);　　　　　　　　　　　　　　　//別の紙コップをレール下に持っていくためのプログラム
 OnFwd(OUT_BC,50);
 Wait(4000);

}
 RemoteStartProgram(CONN, "rail.rxe");　　　　//スレーブの「rail」のプログラムを実行させる
 Off(OUT_BC);
 }

このプログラムで本番は1つのボールの移動に成功することが出来た。


task main() {
SetSensorLowspeed(S1);
*感想 [#f09587f0]
ロボット・プログラムの製作が今までと比べ物にならないほど大変で、しかも時間をかけた分、全部が全部結果になるとは限らないことを思い知らされた気がした。プログラムの量が多くなればなるほどミスが起こりやすい、また複雑なプログラムなほど1つ1つちゃんと理解して確認しないといけないことに気づかされた。これからもロボットを製作してプログラムを作っていく機会があるので、このゼミで学んだことを活かしていきたいと思う。

until (BluetoothStatus(CONN) == NO_ERR);

OnFwd(OUT_BC,50);
Wait(5500);
turn_right_here;
Wait(2000);

RemoteStartProgram(CONN, "arm time.rxe");
Off(OUT_BC);

Wait(10000);
Wait(10000);

OnFwd(OUT_B,-SPEED);
OnFwd(OUT_C,SPEED);
Wait(700);

OnFwd(OUT_BC,50);
Wait(4000);
RemoteStartProgram(CONN, "rail.rxe");

Off(OUT_BC);

}


*スレーブ側 [#g12768a7]
**arm time [#dd328ea5]
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50
#define SPEED6 25

float GetAngle(float d)
{
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0;
return ang;
}


#define DOWN RotateMotor(OUT_A,SPEED3,105);
#define UP RotateMotor(OUT_A,-SPEED5,105);   //アーム全体の動き
#define LDown RotateMotor(OUT_A,SPEED3,25);
#define LUp RotateMotor(OUT_A,-SPEED2,25);

#define Catch RotateMotor(OUT_B,-SPEED4,105);
#define Separate RotateMotor(OUT_B,SPEED4,105);  //アームの動き

#define Into1 RotateMotor(OUT_C,SPEED1,15);  //ボールを入れる
#define Into2 RotateMotor(OUT_C,SPEED1,15);
#define Return RotateMotor(OUT_C,-SPEED1,15);


task main ()
{
OnFwd(OUT_A,10);
Wait(1500);
Catch;	
OnRev(OUT_A,50);
Wait(3500);
Wait(2000);
OnFwd(OUT_A,10);
Wait(3500);
Separate;
OnRev(OUT_A,35);
Wait(2000); 
Wait(500);
}


**rail [#u382c2a1]
#define SPEED1 45
#define SPEED2 60
#define SPEED3 10
#define SPEED4 30
#define SPEED5 50

float GetAngle(float d)
{
const float diameter=5.45;
const float pi=3.1415;
float ang = d/(diameter*pi)*360.0;
return ang;
}

#define Into1 RotateMotor(OUT_C,SPEED3,20);  //ボールを入れる
#define Return1 RotateMotor(OUT_C,-SPEED3,25);
#define Into2 RotateMotor(OUT_C,SPEED4,30);
#define Return2 RotateMotor(OUT_C,-SPEED4,15);

task main(){
OnFwd(OUT_C,15);
Wait(1000);
Wait(3000);
OnRev(OUT_C,30);
Wait(500);

Wait(5000);

OnFwd(OUT_C,25);
Wait(1000);
Wait(3000);
OnRev(OUT_C,30);
Wait(2500);
}