[[2014b/Member]]
#contents
*はじめに [#o4d8a879]
課題3はスタート地点からロボットを進め、2つの紙コップにそれぞれ入っている2個のボールを入れ替えるというものである。
([[2014年後期/課題3:http://yakushi.shinshu-u.ac.jp/robotics/?2014b%2FMission3]]参照)
私たちの班のメンバーは、
-[[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
と私の4人である。
私たちの班の考案は、アームとレールを作り、アームを使って４つのボールをレールにのせ、レールを傾かせることで紙コップにボールを入れるというものである。
*ロボット、またはその動作の説明 [#h9193c48]
**全体 [#ne8db113]
#ref(2014b/Member/arisyka/Mission3/IMG_3870.JPG,20%)
写真のような構造である。
右と左のタイヤの負荷がそれぞれ異なるため、プログラムで左右のタイヤの出力を変えることで直進や回転、旋回をスムーズに行わせる。
**アーム [#y4bd3434]
#ref(2014b/Member/arisyka/Mission3/IMG_3869.gif,30%)
上の写真の赤で囲まれた部分。
1のモーターでアーム全体を上げ下げし、2のモーターでアームの挟む動きを操作する。
アームを動かすために使った定義は以下のようである。
#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 Little LDown;Wait(2000);LUp; //ボールをレールに確実にのせるための動き
#ref(2014b/Member/arisyka/Mission3/armループ2.gif,45%)
ここの定義はアームのハサミ部分の動きを定義している。(上の写真の�△離癲璽拭爾砲茲詁阿�)
#define Catch RotateMotor(OUT_B,-SPEED4,105); //アームでつかむ
#define Separate RotateMotor(OUT_B,SPEED4,105); //アームを離す
#ref(2014b/Member/arisyka/Mission3/catch&separate.gif,40%)
**レール [#k920454a]
#ref(2014b/Member/arisyka/Mission3/IMG_3873.gif,30%)
上の写真の赤で囲まれた部分。また、橙色で囲まれた部分はボールを紙コップに入れる際に、紙コップが倒れないようにするためのストッパーである。倒す角度を調整することで紙コップに入れるボールの数を変える。
レールを動かすために使った動作は上記に書いたアームの定義と同じものを使用した。
#define Into1 RotateMotor(OUT_C,SPEED3,20); //ボールを入れる(1回目)
#define Return1 RotateMotor(OUT_C,-SPEED3,25); //レールを戻す
#define Into2 RotateMotor(OUT_C,SPEED4,30); //ボールを入れる(2回目)
#define Return2 RotateMotor(OUT_C,-SPEED4,15); //レールを戻す
*プログラム [#ec7cd753]
今回使用するプログラムは上記にも示したアーム･レールの動作のプログラムの他に、超音波センサを利用した動作のプログラム、Bluetoothを利用したプログラムである。
**超音波センサを利用した動き [#l3e463dd]
ここで紹介しているプログラムは配られたプリントに書かれている参考のプログラムとほぼ同じものである。
#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) { //距離 d �兪或�
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);
}
void turnAng(long ang) { //角度 ang 度の時計回りの旋回
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);
}
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); //仮の最小値を更新
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;
}
task main()
{
int d = searchDirection(120); //現在の方向を中心に 120 度の範囲で探し、障害物までの距離を返す
if(d > ○){
fwdDist(d-○);}
else {fwdDist(d-○);} //紙コップの方向を探した後音を鳴らし、前後の動きで○�兌蠢阿把篁�
PlaySound(SOUND_UP);
}
**Bluetoothを利用したプログラム [#x935b665]
2つのNXCがBluetoothでつながっている状態であることを前提で話を進める。
ここではマスター側からスレーブ側に指示を出すプログラムを紹介する。
#define CONN 1 //スレーブ(通信相手)の接続番号
task main()
{
until (BluetoothStatus(CONN) == NO_ERR); //接続できるまで待機
//マスター側のプログラム//
RemoteStartProgram(CONN, "〜.rxe"); //スレーブの「〜」のプログラムを起動
逆にスレーブ側からマスター側に指示を出す場合は接続番号を0と変更する。
**苦労した点・改善点 [#l23eb6c6]
**スレーブ側 [#p93594c5]
**マスター側 [#wa6a612a]
*感想 [#h6ea9121]
*マスター側(コピー用) [#hacfc6a7]
#define CONN 1 //スレーブ(通信相手)の接続番号
#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) { //距離 d �兪或�
long angle;
angle = d/(diameter*pi)*360.0;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);
}
void turnAng(long ang) { //角度 ang 度の時計回りの旋回
long angle;
angle = track/diameter * ang;
RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);
}
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); //仮の最小値を更新
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;
}
#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);
#define count 7
#define sound PlaySound(SOUND_CLICK);
**angle move1 [#d59f8a17]
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR); //接続できるまで待機
RotateMotor(OUT_BC,50,GetAngle(50)); //50�兪或�
OnFwd(OUT_B,30);
OnRev(OUT_C,50); //右に90度旋回
Wait(1500);
RotateMotor(OUT_BC,50,GetAngle(30)); //30�兪或�
int d = searchDirection(120);
if(d > 15){
fwdDist(d-15.0);}
else {fwdDist(d-15);} //紙コップの方向を探した後音を鳴らし、前後の動きで15�兌蠢阿把篁�
PlaySound(SOUND_UP);
turn_right_here;
Wait(200);
RemoteStartProgram(CONN, "arm time1.rxe"); //スレーブの「arm time1」のプログラムを起動
}
**angle move2 [#j2aa6be8]
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); //後ろに20�儻綽覆靴晋紂�180度旋回して60�兪或�
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");
}
**angle move3 [#h9f89ef8]
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);
int d = searchDirection(120);
turn_right_here;
Wait(600);
if(d > 15){
fwdDist(d);}
else {fwdDist(d);}
PlaySound(SOUND_UP);
RemoteStartProgram(CONN, "rail1.rxe");
}
**angle move4 [#r0e5655e]
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);
int d = searchDirection(120);
turn_right_here;
Wait(600);
if(d > 15){
fwdDist(d);}
else {fwdDist(d);}
PlaySound(SOUND_UP);
RemoteStartProgram(CONN, "rail2.rxe");
}
**move [#g2dac7b9]
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]
task main() {
SetSensorLowspeed(S1);
until (BluetoothStatus(CONN) == NO_ERR);
OnFwd(OUT_BC,50);
Wait(5500);
turn_right_here;
Wait(2500);
RemoteStartProgram(CONN, "arm time.rxe");
Off(OUT_BC);
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 d2 = searchDirection(120);
if(d2 > 15){
fwdDist(d2-15.0);}
else {fwdDist(d2-15);}
PlaySound(SOUND_UP);
turn_right_here;
Wait(200);
RemoteStartProgram(CONN, "arm time.rxe");
Off(OUT_BC);
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 d3 = searchDirection(120);
turn_right_here;
Wait(600);
if(d3 > 15){
fwdDist(d3);}
else {fwdDist(d3);}
PlaySound(SOUND_UP);
RemoteStartProgram(CONN, "rail1.rxe");
Off(OUT_BC);
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);
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);
RemoteStartProgram(CONN, "rail2.rxe");
}
**ex2 [#gb9319b5]
task main() {
SetSensorLowspeed(S1);
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); //アームの動き
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); //レールを傾け、4つの内2つだけボールを紙コップに入れる
OnRev(OUT_C,30);
Wait(500);
Wait(5000);
OnFwd(OUT_C,25);
Wait(1000);
Wait(3000); //レールを傾け、レールに残っているボールを紙コップに入れる
OnRev(OUT_C,30);
Wait(2500);
}
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
