2019b/Member/Nishi/Mission3
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
[[2019b/Member]]
目次
#contents
*課題：ボール運搬ロボット [#se26b6b2]
#ref(./2019b-mission3.png,center,65%);
青と赤のボールを運搬して、空き缶の上に載せる。今回はロボットを２台使用し、C,F,Eの順に運搬を行った。一台はボールを、もう一台は空き缶を掴んでコースの中央に集合し、持ってきた空き缶の上にボールをのせ、セットにして運搬し、設置する。これを３回行い、すべてのボールを運んだ。今回私は「ボールを受け取る側」を担当した。
#ref(./mis_1.png,center,50%);
#ref(./mis_2.png,center,50%);
#ref(./mis_3.png,center,50%);
#ref(./mis_4.png,center,50%);
- 青線 = ボールを渡す側
- 赤線 = ボールを受け取る側
*ロボット [#xf3f8954]
**ボールを渡す側 [#h57583e3]
#ref(./IMG_3765a.jpg,10%);
こちらのロボットは空き缶の上にのったボールだけを掴み、もう一つのロボットが持ってくる空き缶の上にのせるという動作を行う。
#ref(./IMG_3766a.jpg,10%);
缶やもう一つのロボットとの距離は超音波センサーで感知する。
**ボールを受け取る側（担当） [#r9e26148]
#ref(./IMG_3760a.jpg,10%);
こちらのロボットは空き缶を掴んでからボールを受け取り、ボールをのせた空き缶をC,F,E地点に置くという動作を行う。ボールは受け渡し時にもう一つのロボットが空き缶の上に置いてくれるため、ボールを支えたり動かしたりするパーツはこのロボットには無く、アームは低い位置に２組、空き缶をしっかりと掴むために付けられている。
#ref(./IMG_3759a.jpg,10%);
空き缶の感知はアームの上に付けられた超音波センサーで行う。空き缶を横から感知する場合、缶は円柱であるために思った通りに感知しないことがある。しかし今回の超音波センサーは下を向いているため、感知するのは円柱の上の部分の平面になる。したがって、距離の変化をより望んだように感知させることができた。
#ref(./IMG_3762b.jpg,10%);
アームを動かすとき、過度に開きすぎてタイヤに当たることがあったため、開きすぎないようにストッパーになるものを取り付けた。また、缶を持ったまま後退する際、缶がアームから離れることがあるためアームがくの字になるようにした。
*プログラム（ボールを渡す側） [#e6a5c4bf]
#define THRESHOLD 52
#define SPEED_H 35
#define SPEED_L 25
#define onRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go_forward onRL(SPEED_L,SPEED_L);
#define turn_left1 onRL(SPEED_H,-SPEED_H);
#define turn_left0 onRL(SPEED_H,0);
#define turn_right0 onRL(0,SPEED_H);
#define turn_right1 onRL(-SPEED_H,SPEED_H);
#define STEP 1
#define nMAX 140
#define short_break Off(OUT_BC);Wait(1000);
#define cross_line onRL(SPEED_L,SPEED_L);Wait(1000);
#define u_turnl1
OnRev(OUT_BC,SPEED_L);Wait(200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr1
OnRev(OUT_BC,SPEED_L);Wait(1400);RotateMotorEx(OUT_BC,SPEED_H,angle1,100,true,true);
#define u_turnl2
OnRev(OUT_BC,SPEED_L);Wait(1200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr2
OnRev(OUT_BC,SPEED_L);Wait(1000);RotateMotorEx(OUT_BC,SPEED_H,angle2,100,true,true);
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
sub catch_balll()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.1){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right0;
}else{
turn_right1;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
sub follow_liner()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_right1;
Wait(1000);
nOnline=0;
cross++;
}
}
sub catch_ballr()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
sub follow_linel()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;
Wait(800);
nOnline=0;
cross++;
}
}
sub catch_releaser()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
sub catch_releasel()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
sub follow_stop()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross++;
}
}
sub follow_line1()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;Wait(nMAX*STEP);
cross_line;
nOnline=0;
cross++;
}
}
sub follow_line2()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross_line;
nOnline=0;
cross++;
}
}
task main()
{
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
OnFwd(OUT_BC,30);
Wait(500);
catch_balll();
u_turnl2;
follow_stop();
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Wait(6000);
u_turnl1;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_liner();
catch_ballr();
u_turnr1;
follow_line2();
catch_releaser();
Wait(10000);
u_turnl2;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_stop();
turn_right1;
Wait(1000);
follow_line1();
follow_stop();
turn_right1;
Wait(1000);
catch_ballr();
u_turnr1;
follow_line2();
follow_line2();
catch_releaser();
}
**関数、定義など [#ydc4861f]
***定義 [#jbf393e4]
#define THRESHOLD 52
#define SPEED_H 35
#define SPEED_L 25
#define onRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go_forward onRL(SPEED_L,SPEED_L);
#define turn_left1 onRL(SPEED_H,-SPEED_H);
#define turn_left0 onRL(SPEED_H,0);
#define turn_right0 onRL(0,SPEED_H);
#define turn_right1 onRL(-SPEED_H,SPEED_H);
#define STEP 1
#define nMAX 140
#define short_break Off(OUT_BC);Wait(1000);
#define cross_line onRL(SPEED_L,SPEED_L);Wait(1000);
#define u_turnl1
OnRev(OUT_BC,SPEED_L);Wait(200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr1
OnRev(OUT_BC,SPEED_L);Wait(1400);RotateMotorEx(OUT_BC,SPEED_H,angle1,100,true,true);
#define u_turnl2
OnRev(OUT_BC,SPEED_L);Wait(1200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr2
OnRev(OUT_BC,SPEED_L);Wait(1000);RotateMotorEx(OUT_BC,SPEED_H,angle2,100,true,true);
速度、Uターン、旋回や回転などの定義。こちらのプログラムにおいて、ライントレースでは時間ではなく繰り返しの回数を利用しているため、繰り返しの最大値が定められている。
***角度の計算 [#u35348b5]
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
括弧内（float dにあたる）に方向転換したい角度を入れ、タイヤの回転角度を計算する。計算した角度はロボットを回転させるプログラムで使用する。
***ライントレース [#m613d00a]
sub catch_balll()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.1){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right0;
}else{
turn_right1;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
sub catch_ballr()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
- catch_balll() ボールを掴むまで、左側をライントレースする
- catch_ballr() ボールを掴むまで、右側をライントレースする プログラム。
超音波センサーの値が6.1未満になると缶があると認識し、アームを閉じる。
sub follow_linel()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;
Wait(800);
nOnline=0;
cross++;
}
}
sub follow_liner()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_right1;
Wait(1000);
nOnline=0;
cross++;
}
}
- follow_linel() 交差点を二回左に曲がるまで、左側をライントレースする
- follow_liner() 交差点を二回右に曲がるまで、右側をライントレースする プログラム。
ここではwhileが２つ使用されており、２回右折あるいは左折していない場合に２つめのwhileが実行されるようになっている。
sub catch_releaser()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
sub catch_releasel()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
- catch_releaser() 右トレースをし、ボールを渡す
- catch_releasel() 左トレースをし、ボールを渡す プログラム。
超音波センサーの値が6.2未満になると、相手のロボットが前方にいると認識し、アームを開く。相手側は左トレースで受け取り場に来るため、位置を合わせるためにこのロボットは右トレースで行く必要があり、catch_releasel()は使用しなかった。
sub follow_stop()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross++;
}
}
右側をトレースし、交差点を感知すると停止するプログラム。
sub follow_line1()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;Wait(nMAX*STEP);
cross_line;
nOnline=0;
cross++;
}
}
sub follow_line2()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross_line;
nOnline=0;
cross++;
}
}
- follow_line1() 右トレースをし、交差点で直進する
- follow_line2() 左トレースをし、交差点で直進する プログラム。
**task main [#z12b10de]
***導入 [#vc867b56]
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
angle1, angle2をそれぞれ90, 60度回転するための角度に指定。
***１つめのボール [#d37da357]
OnFwd(OUT_BC,30);
Wait(500);
catch_balll();
u_turnl2;
follow_stop();
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Wait(6000);
Aから出発し、左トレースをつづけて１つめのボールをアームで掴む。その後は方向転換をし、交差点を感知するまでトレースを行う。交差点を越え、地点Hまでトレースをする。地点Hを感知したら線を乗り越え、右トレースでもう一つのロボットに近づいていき、ボールを渡して少し待機する。
***２つめのボール [#e088a84c]
u_turnl1;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_liner();
catch_ballr();
u_turnr1;
follow_line2();
catch_releaser();
Wait(10000);
待機が終わったらUターンで引き返しを始める。右トレースを行い、地点Fにある２つめのボールがのった缶を感知する。そうしてボールを掴んだら、１つめと同じように地点Hで右トレースになるようにし、ボールを渡しに行く。
***３つめのボール [#k902d020]
u_turnl2;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_stop();
turn_right1;
Wait(1000);
follow_line1();
follow_stop();
turn_right1;
Wait(1000);
catch_ballr();
u_turnr1;
follow_line2();
follow_line2();
catch_releaser();
２つめを渡したら同じように右トレースで引き返す。交差点２つに当たるが、２つめの交差点Gの時点ではfollow_line1()が使用されており、直進で乗り越える。その後は地点Dで右折をし、ボールを捉える。Uターンをし、follow_line2()を２つで交差点G, Hを乗り越える。あとは右トレースで進みボールを渡して終了。
*プログラム（ボールを受け取る側(担当)） [#d731a02f]
#define WHITE 56 // 白色
#define GWHITE 48 // 白灰色
#define GBLACK 40 // 黒灰色
#define BLACK 34 // 黒色
#define SPEED_H 47 // ハイスピード
#define SPEED 43 // ノーマルスピード
#define SPEED_L 40 // ロースピード
#define STEP 1
#define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go OnFwdSync(OUT_BC,SPEED_H,0); // 直進
#define gofwd OnFwdSync(OUT_BC,SPEED_L,0); // 直進（スロー）
#define left_rotation_s OnRL(SPEED,-SPEED); // 高速左回転
#define left_rotation OnRL(SPEED_L,-SPEED_L); // 左回転
#define turn_left Off(OUT_C);OnFwd(OUT_B,SPEED); // 左旋回
#define turn_right Off(OUT_B);OnFwd(OUT_C,SPEED); // 右旋回
#define right_rotation OnRL(-SPEED_L,SPEED_L); // 右回転
#define right_rotation_s OnRL(-SPEED,SPEED); // 高速右回転
float GetAngle(float d)
{
const float diameter = 5.45;
const float pi=3.1415;
const float distance=12.1;
float ang = (distance*d)/diameter;
return ang;
}
void turnR(long u)
{
int angle = GetAngle(u);
RotateMotorEx(OUT_BC,SPEED,angle,100,true,true);
}
void turnL(long l)
{
int angle = GetAngle(l);
RotateMotorEx(OUT_BC,SPEED,angle,-100,true,true);
}
void back(long b)
{
RotateMotorEx(OUT_BC,-SPEED,b,0,true,true);
}
void follow_line(long q)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < q){
if(SENSOR_3 <= BLACK){
left_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
right_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_lineR(long p)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < p){
if(SENSOR_3 <= BLACK){
right_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
left_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_line_for_intersection(long t)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < t){
if(SENSOR_3 <= BLACK){
left_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
}else if(WHITE <= SENSOR_3){
right_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_line_for_intersectionR(long y)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < y){
if(SENSOR_3 <= BLACK){
right_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
}else if(WHITE <= SENSOR_3){
left_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
void fetch_ball()
{
SetSensorLowspeed(S2);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < 200){
if(SensorUS(S1) <= 15){
Off(OUT_BC);
RotateMotor(OUT_A,-SPEED,70);
}else{
gofwd
t0=CurrentTick();
}
}
Off(OUT_BC);
}
void open()
{
OnFwd(OUT_A,30);
Wait(400);
Off(OUT_A);
}
void close()
{
OnRev(OUT_A,30);
Wait(700);
Off(OUT_A);
}
void followC()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
int tenkan = GetAngle(70);
RotateMotorEx(OUT_BC,SPEED,tenkan,100,true,true);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followF()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(66);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followE()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(50);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}else if(SENSOR_3<GBLACK){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}
}
}
}
void catch()
{
SetSensorLowspeed(S2);
long tO;
tO = CurrentTick();
while(CurrentTick()-tO < 10000){
int O = SensorUS(S2);
if(O <= 11){
break;
}else if(11 < O){
Wait(STEP);
}
Wait(STEP);
Wait(2500);
}
}
task main()
{
RotateMotorEx(OUT_BC,SPEED_H,180,0,true,true);
follow_line_for_intersection(10000);
follow_line(200);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(1500);
followC(); // C開始
follow_line_for_intersection(750);
follow_line(150);
RotateMotor(OUT_C,SPEED,150);
turnR(40);
RotateMotorEx(OUT_BC,SPEED_L,60,0,false,true);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(10000);
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(135);
RotateMotor(OUT_C,SPEED,70);
follow_line_for_intersection(1000);
follow_line(180);
follow_line_for_intersection(1000);
follow_line(180);
back(120);
open(); // C終了
back(160);
turnR(130);
RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true);
follow_line_for_intersection(2000);
followF(); // F開始
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(9000);
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(140);
follow_line_for_intersectionR(1000);
follow_lineR(140);
RotateMotor(OUT_C,SPEED_H,90);
follow_line(160);
back(120);
open(); //F 終了
back(150);
turnL(77);
RotateMotorEx(OUT_BC,SPEED_L,50,0,false,true);
follow_line_for_intersectionR(1000);
follow_lineR(140);
follow_line_for_intersectionR(2000);
followE(); // E開始
follow_line_for_intersection(1500);
follow_line(160);
RotateMotor(OUT_C,SPEED,75);
follow_line_for_intersection(500);
follow_line(160);
follow_line_for_intersection(3000);
Wait(15000);
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(150);
follow_line_for_intersectionR(1500);
follow_lineR(160);
RotateMotorEx(OUT_BC,SPEED_L,65,0,false,true);
RotateMotor(OUT_B,SPEED,30);
follow_line_for_intersectionR(1000);
follow_lineR(150);
follow_line_for_intersectionR(1750);
follow_lineR(140);
back(120);
turnL(15);
open(); // E終了
back(300);
}
**関数、定義など [#s6035e80]
***定義 [#w1ea71db]
#define WHITE 56 // 白色
#define GWHITE 48 // 白灰色
#define GBLACK 40 // 黒灰色
#define BLACK 34 // 黒色
#define SPEED_H 47 // ハイスピード
#define SPEED 43 // ノーマルスピード
#define SPEED_L 40 // ロースピード
#define STEP 1
#define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go OnFwdSync(OUT_BC,SPEED_H,0); // 直進
#define gofwd OnFwdSync(OUT_BC,SPEED_L,0); // 直進（スロー）
#define left_rotation_s OnRL(SPEED,-SPEED); // 高速左回転
#define left_rotation OnRL(SPEED_L,-SPEED_L); // 左回転
#define turn_left Off(OUT_C);OnFwd(OUT_B,SPEED); // 左旋回
#define turn_right Off(OUT_B);OnFwd(OUT_C,SPEED); // 右旋回
#define right_rotation OnRL(-SPEED_L,SPEED_L); // 右回転
#define right_rotation_s OnRL(-SPEED,SPEED); // 高速右回転
ライントレースのための色の値、速度、旋回や回転などを定義した。回転は二つのタイヤを逆回転させてその場で方向を変えること、旋回は一つのタイヤだけを回転させ、カーブを描きながら進むことを示す。
***角度の計算 [#c8a742e0]
float GetAngle(float d)
{
const float diameter = 5.45;
const float pi=3.1415;
const float distance=12.1;
float ang = (distance*d)/diameter;
return ang;
}
括弧内（float dにあたる）に方向転換したい角度を入れ、タイヤの回転角度を計算する。計算した角度はロボットを回転させるプログラムで使用する。
***回転 [#yf78dbe0]
void turnR(long u)
{
int angle = GetAngle(u);
RotateMotorEx(OUT_BC,SPEED,angle,100,true,true);
}
void turnL(long l)
{
int angle = GetAngle(l);
RotateMotorEx(OUT_BC,SPEED,angle,-100,true,true);
}
- turnR 右回転
- turnL 左回転 のプログラム。
括弧内（long u と long l にあたる）に方向転換したい角度を入れ、回転する。計算して出した角度はここで使用する。
***後退 [#p4f8c3aa]
void back(long b)
{
RotateMotorEx(OUT_BC,-SPEED,b,0,true,true);
}
タイヤ２つを同時に動かし後退するプログラム。括弧内（long bにあたる）にタイヤの回転角度を入れ、その分後退する。
***ライントレース [#q4c8ac20]
void follow_line(long q)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < q){
if(SENSOR_3 <= BLACK){
left_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
right_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_lineR(long p)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < p){
if(SENSOR_3 <= BLACK){
right_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
left_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
- follow_line 黒い線の左縁をトレースする
- follow_lineR 黒い線の右縁をトレースする プログラム。
ラインの縁の進行中に交差点を感知すると停止する。交差点は光センサーが黒色を感知する時間（long q, long pの部分に入力する）で判断するようになっている。最初にlong t0とし、t0 = CurrentTick()でサブルーチンの開始時刻を設定しており、またt0 = CurrentTick()が黒色以外を感知した場合のプログラムに入り、時間をリセットする。そのためセンサーが黒色を感知している間に限り時間が経過し、whileの括弧内で定められている時間が経過すると、このループから抜けて次のプログラムへと移行する。
void follow_line_for_intersection(long t)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < t){
if(SENSOR_3 <= BLACK){
left_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
}else if(WHITE <= SENSOR_3){
right_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_line_for_intersectionR(long y)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < y){
if(SENSOR_3 <= BLACK){
right_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
}else if(WHITE <= SENSOR_3){
left_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
- follow_line_for_intersection 交差点を無視して黒い線の左縁をトレースする
- follow_lineR_for_intersection 交差点を無視して黒い線の右縁をトレースする プログラム。
これらのプログラムは最初の括弧内（long t, long yにあたる）にライントレース全体の時間を入力する。交差点ではなく時間で停止するところがfollow_line, follow_lineRと異なるが、基本的には同じになっている。
***缶の捕捉 [#hbd61204]
void fetch_ball()
{
SetSensorLowspeed(S2);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < 200){
if(SensorUS(S1) <= 15){
Off(OUT_BC);
RotateMotor(OUT_A,-SPEED,70);
}else{
gofwd
t0=CurrentTick();
}
}
Off(OUT_BC);
}
超音波センサーが缶を感知するとアームが缶を捉えるプログラム。具体的には、センサーが１５cmより大きい値を感知してる間は前進し、１５cm以下の値を0,2秒間感知すると前進をやめてモーターが回り、アームを閉じるようになっている。しかしアームをRotateMotorで動かすと缶を掴む際の調整が細かく必要になるため、使用しなかった。
void open()
{
OnFwd(OUT_A,30);
Wait(400);
Off(OUT_A);
}
void close()
{
OnRev(OUT_A,30);
Wait(700);
Off(OUT_A);
}
- open() アームを開く
- close() アームを閉じる プログラム。
缶を掴む、離すという動作を確実にしたかったため、少しばかり必要以上にアームを動かすようにした。使用したのがRotateMotorではないため、プログラム通りに開閉せずプログラムが停止する、ということは起きなかった。
void followC()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
int tenkan = GetAngle(70);
RotateMotorEx(OUT_BC,SPEED,tenkan,100,true,true);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followF()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(66);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followE()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(50);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}else if(SENSOR_3<GBLACK){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}
}
}
}
- followC() C地点の缶を捉える
- followF() F地点の缶を捉える
- followE() E地点の缶を捉える ためのプログラム。
地点によって内容は当然異なるが、
+ ライントレース
+ 空き缶の感知
+ アームを閉じる
+ 方向転換
という流れになっている。このプログラムでは方向転換まで済んで、その後抜け出すためにbreakが使用されている。
void catch()
{
SetSensorLowspeed(S2);
long tO;
tO = CurrentTick();
while(CurrentTick()-tO < 10000){
int O = SensorUS(S2);
if(O <= 11){
break;
}else if(11 < O){
Wait(STEP);
}
Wait(STEP);
Wait(2500);
}
}
ボールの受け渡しをする場所に着いてから１０秒たつ、またはボールを感知して２,５秒たつと次へいくプログラム。しかし実際にやってみるとボールが球体であるが故にセンサーが捉えたり捉えなかったりしたため、fetch_ballと同様に最終的には使用されなくなった。
**task main [#a6d9e75d]
***地点Cへの移動（区間A-J-H-G-C） [#m15ca196]
RotateMotorEx(OUT_BC,SPEED_H,180,0,true,true);
follow_line_for_intersection(10000);
follow_line(200);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(1500);
最初に直進してAの枠から出た後、ライントレースを開始する。最初の左カーブでは黒色を感知する時間が長く、センサーが交差点と認識してしまうことがあったため、交差点を無視するライントレースの時間を設けた。また、交差点を感知した直後に少し交差点を無視する時間を設けないと１つの交差点で複数の交差点に当たったと認識してしまうため、そこでも無視する時間を設けた。
***１つめの缶 （区間C-G-H-I-H-G-C） [#d05f3a60]
followC(); // C開始
follow_line_for_intersection(750);
follow_line(150);
RotateMotor(OUT_C,SPEED,150);
turnR(40);
RotateMotorEx(OUT_BC,SPEED_L,60,0,false,true);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(10000);
followCでC地点の空き缶を掴んで方向転換まで行い、その後ライントレースで向きを微調整しつつ交差点Gまで進む。そこをRotateMotor, turnR等で右斜め前に進行するかたちで乗り越え、ライントレースを再開し、ボールの受け渡し場で停止する。ボールの受け渡しが完了したかどうかの確認のプログラムは無く、時間で進行していく。
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(135);
RotateMotor(OUT_C,SPEED,70);
follow_line_for_intersection(1000);
follow_line(180);
follow_line_for_intersection(1000);
follow_line(180);
back(120);
open(); // C終了
ボールの受け渡しを行った後、引き返すことになっているが、その場で回転すると地点Fの缶やもう一つのロボットに当たることがあった。そのため、右後ろに後退、前進、左回転という手順を踏んで戻る。これは２つめ、３つめの缶の場合も同様に行われる。そして地点Cに行くには交差点Hを感知する必要があるため直後は右トレースをする。交差点を感知した後は左トレースに変更し、交差点の感知を利用して、地点Cに缶とボールを置く。
#ref(./Turn.png,center,50%);
***地点Fへの移動 （区間C-G-F） [#af89a04a]
back(160);
turnR(130);
RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true);
follow_line_for_intersection(2000);
１つめの缶が終わったらアームが缶に当たらない位置まで後退し２つめの缶を目指す。向きを変え交差点Gを越えて地点Fへ向かう。
***２つめの缶 （区間F-G-H-I-H-G-F） [#if5dd39e]
followF(); // F開始
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(9000);
１つめの缶のときと同じように方向転換、位置調整を行いながら進行していく。地点Fの缶をとった後は左トレースで受け取り場まで行き、もう一つのロボットが来るまで停止する。
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(140);
follow_line_for_intersectionR(1000);
follow_lineR(140);
RotateMotor(OUT_C,SPEED_H,90);
follow_line(160);
back(120);
open(); //F 終了
ボール受け取った後、方向転換し右トレースで地点Gまで戻り、そこで線を乗り越える。左トレースに切り替え交差点の感知を利用して、地点Fに２つめの缶とボールを置く。
***地点Eへの移動 （区間F-G-D-E） [#r15de770]
back(150);
turnL(77);
RotateMotorEx(OUT_BC,SPEED_L,50,0,false,true);
follow_line_for_intersectionR(1000);
follow_lineR(140);
follow_line_for_intersectionR(2000);
１つめと同様に、アームが缶に当たらない位置まで後退し３つめの缶を目指す。向きを変え、右トレースで地点Eへ向かう。
***３つめの缶 （区間E-D-G-H-I-H-G-D-E） [#s67a5821]
followE(); // E開始
follow_line_for_intersection(1500);
follow_line(160);
RotateMotor(OUT_C,SPEED,75);
follow_line_for_intersection(500);
follow_line(160);
follow_line_for_intersection(3000);
Wait(15000);
最後の缶に取りかかる。followEで掴みから方向転換まで済ませ、左トレースをする。交差点Gを感知したらRotateMotorで乗り越え、引き続き左トレースで進み、受け取り場まで行く。
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(150);
follow_line_for_intersectionR(1500);
follow_lineR(160);
RotateMotorEx(OUT_BC,SPEED_L,65,0,false,true);
RotateMotor(OUT_B,SPEED,30);
follow_line_for_intersectionR(1000);
follow_lineR(150);
follow_line_for_intersectionR(1750);
follow_lineR(140);
back(120);
turnL(15);
open(); // E終了
back(300);
ボール受け取った後、方向転換し右トレースで地点Gまで戻り、直進や旋回で線を乗り越える。再び右トレースで進み、交差点の感知を利用して地点Eに行く。位置調整のために少し左回転をし、最後の缶とボールを地点Eに置く。アームを開き、後退で距離をとり終了。
*まとめ [#u09588fa]
最初にロボット２台を起動する際、同時にスイッチを入れるようにしていたが、通信機能を利用していれば、人間が呼吸を合わせることなく楽に開始ができた。本番では思ったようにはいかず、最後まで改善点が見つかる結果となってしまったものの、１つだけでもボールの受け渡しが本番でできたのは嬉しかった。課題に取り組み、機械の扱いの難しさを身にしみて理解することができた。
終了行:
[[2019b/Member]]
目次
#contents
*課題：ボール運搬ロボット [#se26b6b2]
#ref(./2019b-mission3.png,center,65%);
青と赤のボールを運搬して、空き缶の上に載せる。今回はロボットを２台使用し、C,F,Eの順に運搬を行った。一台はボールを、もう一台は空き缶を掴んでコースの中央に集合し、持ってきた空き缶の上にボールをのせ、セットにして運搬し、設置する。これを３回行い、すべてのボールを運んだ。今回私は「ボールを受け取る側」を担当した。
#ref(./mis_1.png,center,50%);
#ref(./mis_2.png,center,50%);
#ref(./mis_3.png,center,50%);
#ref(./mis_4.png,center,50%);
- 青線 = ボールを渡す側
- 赤線 = ボールを受け取る側
*ロボット [#xf3f8954]
**ボールを渡す側 [#h57583e3]
#ref(./IMG_3765a.jpg,10%);
こちらのロボットは空き缶の上にのったボールだけを掴み、もう一つのロボットが持ってくる空き缶の上にのせるという動作を行う。
#ref(./IMG_3766a.jpg,10%);
缶やもう一つのロボットとの距離は超音波センサーで感知する。
**ボールを受け取る側（担当） [#r9e26148]
#ref(./IMG_3760a.jpg,10%);
こちらのロボットは空き缶を掴んでからボールを受け取り、ボールをのせた空き缶をC,F,E地点に置くという動作を行う。ボールは受け渡し時にもう一つのロボットが空き缶の上に置いてくれるため、ボールを支えたり動かしたりするパーツはこのロボットには無く、アームは低い位置に２組、空き缶をしっかりと掴むために付けられている。
#ref(./IMG_3759a.jpg,10%);
空き缶の感知はアームの上に付けられた超音波センサーで行う。空き缶を横から感知する場合、缶は円柱であるために思った通りに感知しないことがある。しかし今回の超音波センサーは下を向いているため、感知するのは円柱の上の部分の平面になる。したがって、距離の変化をより望んだように感知させることができた。
#ref(./IMG_3762b.jpg,10%);
アームを動かすとき、過度に開きすぎてタイヤに当たることがあったため、開きすぎないようにストッパーになるものを取り付けた。また、缶を持ったまま後退する際、缶がアームから離れることがあるためアームがくの字になるようにした。
*プログラム（ボールを渡す側） [#e6a5c4bf]
#define THRESHOLD 52
#define SPEED_H 35
#define SPEED_L 25
#define onRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go_forward onRL(SPEED_L,SPEED_L);
#define turn_left1 onRL(SPEED_H,-SPEED_H);
#define turn_left0 onRL(SPEED_H,0);
#define turn_right0 onRL(0,SPEED_H);
#define turn_right1 onRL(-SPEED_H,SPEED_H);
#define STEP 1
#define nMAX 140
#define short_break Off(OUT_BC);Wait(1000);
#define cross_line onRL(SPEED_L,SPEED_L);Wait(1000);
#define u_turnl1
OnRev(OUT_BC,SPEED_L);Wait(200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr1
OnRev(OUT_BC,SPEED_L);Wait(1400);RotateMotorEx(OUT_BC,SPEED_H,angle1,100,true,true);
#define u_turnl2
OnRev(OUT_BC,SPEED_L);Wait(1200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr2
OnRev(OUT_BC,SPEED_L);Wait(1000);RotateMotorEx(OUT_BC,SPEED_H,angle2,100,true,true);
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
sub catch_balll()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.1){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right0;
}else{
turn_right1;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
sub follow_liner()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_right1;
Wait(1000);
nOnline=0;
cross++;
}
}
sub catch_ballr()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
sub follow_linel()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;
Wait(800);
nOnline=0;
cross++;
}
}
sub catch_releaser()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
sub catch_releasel()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
sub follow_stop()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross++;
}
}
sub follow_line1()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;Wait(nMAX*STEP);
cross_line;
nOnline=0;
cross++;
}
}
sub follow_line2()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross_line;
nOnline=0;
cross++;
}
}
task main()
{
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
OnFwd(OUT_BC,30);
Wait(500);
catch_balll();
u_turnl2;
follow_stop();
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Wait(6000);
u_turnl1;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_liner();
catch_ballr();
u_turnr1;
follow_line2();
catch_releaser();
Wait(10000);
u_turnl2;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_stop();
turn_right1;
Wait(1000);
follow_line1();
follow_stop();
turn_right1;
Wait(1000);
catch_ballr();
u_turnr1;
follow_line2();
follow_line2();
catch_releaser();
}
**関数、定義など [#ydc4861f]
***定義 [#jbf393e4]
#define THRESHOLD 52
#define SPEED_H 35
#define SPEED_L 25
#define onRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go_forward onRL(SPEED_L,SPEED_L);
#define turn_left1 onRL(SPEED_H,-SPEED_H);
#define turn_left0 onRL(SPEED_H,0);
#define turn_right0 onRL(0,SPEED_H);
#define turn_right1 onRL(-SPEED_H,SPEED_H);
#define STEP 1
#define nMAX 140
#define short_break Off(OUT_BC);Wait(1000);
#define cross_line onRL(SPEED_L,SPEED_L);Wait(1000);
#define u_turnl1
OnRev(OUT_BC,SPEED_L);Wait(200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr1
OnRev(OUT_BC,SPEED_L);Wait(1400);RotateMotorEx(OUT_BC,SPEED_H,angle1,100,true,true);
#define u_turnl2
OnRev(OUT_BC,SPEED_L);Wait(1200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true);
#define u_turnr2
OnRev(OUT_BC,SPEED_L);Wait(1000);RotateMotorEx(OUT_BC,SPEED_H,angle2,100,true,true);
速度、Uターン、旋回や回転などの定義。こちらのプログラムにおいて、ライントレースでは時間ではなく繰り返しの回数を利用しているため、繰り返しの最大値が定められている。
***角度の計算 [#u35348b5]
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
括弧内（float dにあたる）に方向転換したい角度を入れ、タイヤの回転角度を計算する。計算した角度はロボットを回転させるプログラムで使用する。
***ライントレース [#m613d00a]
sub catch_balll()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.1){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right0;
}else{
turn_right1;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
sub catch_ballr()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,-22);
Wait(2000);
Off(OUT_A);
catch++;
}
}
- catch_balll() ボールを掴むまで、左側をライントレースする
- catch_ballr() ボールを掴むまで、右側をライントレースする プログラム。
超音波センサーの値が6.1未満になると缶があると認識し、アームを閉じる。
sub follow_linel()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;
Wait(800);
nOnline=0;
cross++;
}
}
sub follow_liner()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<2){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_right1;
Wait(1000);
nOnline=0;
cross++;
}
}
- follow_linel() 交差点を二回左に曲がるまで、左側をライントレースする
- follow_liner() 交差点を二回右に曲がるまで、右側をライントレースする プログラム。
ここではwhileが２つ使用されており、２回右折あるいは左折していない場合に２つめのwhileが実行されるようになっている。
sub catch_releaser()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
sub catch_releasel()
{
SetSensorLight(S4);
SetSensorLowspeed(S1);
int catch=0;
while(catch<1){
while(SensorUS(S1)>=6.2){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
}
Wait(STEP);
}
short_break;
OnFwd(OUT_A,20);
Wait(500);
Off(OUT_A);
catch++;
}
}
- catch_releaser() 右トレースをし、ボールを渡す
- catch_releasel() 左トレースをし、ボールを渡す プログラム。
超音波センサーの値が6.2未満になると、相手のロボットが前方にいると認識し、アームを開く。相手側は左トレースで受け取り場に来るため、位置を合わせるためにこのロボットは右トレースで行く必要があり、catch_releasel()は使用しなかった。
sub follow_stop()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross++;
}
}
右側をトレースし、交差点を感知すると停止するプログラム。
sub follow_line1()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_right1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_right0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_left1;
}else{
turn_left0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
turn_left1;Wait(nMAX*STEP);
cross_line;
nOnline=0;
cross++;
}
}
sub follow_line2()
{
SetSensorLight(S4);
int nOnline=0;
int cross=0;
while(cross<1){
while(nOnline<nMAX){
if(SENSOR_4<THRESHOLD-15){
turn_left1;
nOnline++;
}else{
if(SENSOR_4<THRESHOLD-7){
turn_left0;
}else if(SENSOR_4<THRESHOLD+7){
go_forward;
}else if(SENSOR_4<THRESHOLD+15){
turn_right1;
}else{
turn_right0;
}
nOnline=0;
}
Wait(STEP);
}
short_break;
cross_line;
nOnline=0;
cross++;
}
}
- follow_line1() 右トレースをし、交差点で直進する
- follow_line2() 左トレースをし、交差点で直進する プログラム。
**task main [#z12b10de]
***導入 [#vc867b56]
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
angle1, angle2をそれぞれ90, 60度回転するための角度に指定。
***１つめのボール [#d37da357]
OnFwd(OUT_BC,30);
Wait(500);
catch_balll();
u_turnl2;
follow_stop();
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Wait(6000);
Aから出発し、左トレースをつづけて１つめのボールをアームで掴む。その後は方向転換をし、交差点を感知するまでトレースを行う。交差点を越え、地点Hまでトレースをする。地点Hを感知したら線を乗り越え、右トレースでもう一つのロボットに近づいていき、ボールを渡して少し待機する。
***２つめのボール [#e088a84c]
u_turnl1;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_liner();
catch_ballr();
u_turnr1;
follow_line2();
catch_releaser();
Wait(10000);
待機が終わったらUターンで引き返しを始める。右トレースを行い、地点Fにある２つめのボールがのった缶を感知する。そうしてボールを掴んだら、１つめと同じように地点Hで右トレースになるようにし、ボールを渡しに行く。
***３つめのボール [#k902d020]
u_turnl2;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
follow_stop();
turn_right1;
Wait(1000);
follow_line1();
follow_stop();
turn_right1;
Wait(1000);
catch_ballr();
u_turnr1;
follow_line2();
follow_line2();
catch_releaser();
２つめを渡したら同じように右トレースで引き返す。交差点２つに当たるが、２つめの交差点Gの時点ではfollow_line1()が使用されており、直進で乗り越える。その後は地点Dで右折をし、ボールを捉える。Uターンをし、follow_line2()を２つで交差点G, Hを乗り越える。あとは右トレースで進みボールを渡して終了。
*プログラム（ボールを受け取る側(担当)） [#d731a02f]
#define WHITE 56 // 白色
#define GWHITE 48 // 白灰色
#define GBLACK 40 // 黒灰色
#define BLACK 34 // 黒色
#define SPEED_H 47 // ハイスピード
#define SPEED 43 // ノーマルスピード
#define SPEED_L 40 // ロースピード
#define STEP 1
#define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go OnFwdSync(OUT_BC,SPEED_H,0); // 直進
#define gofwd OnFwdSync(OUT_BC,SPEED_L,0); // 直進（スロー）
#define left_rotation_s OnRL(SPEED,-SPEED); // 高速左回転
#define left_rotation OnRL(SPEED_L,-SPEED_L); // 左回転
#define turn_left Off(OUT_C);OnFwd(OUT_B,SPEED); // 左旋回
#define turn_right Off(OUT_B);OnFwd(OUT_C,SPEED); // 右旋回
#define right_rotation OnRL(-SPEED_L,SPEED_L); // 右回転
#define right_rotation_s OnRL(-SPEED,SPEED); // 高速右回転
float GetAngle(float d)
{
const float diameter = 5.45;
const float pi=3.1415;
const float distance=12.1;
float ang = (distance*d)/diameter;
return ang;
}
void turnR(long u)
{
int angle = GetAngle(u);
RotateMotorEx(OUT_BC,SPEED,angle,100,true,true);
}
void turnL(long l)
{
int angle = GetAngle(l);
RotateMotorEx(OUT_BC,SPEED,angle,-100,true,true);
}
void back(long b)
{
RotateMotorEx(OUT_BC,-SPEED,b,0,true,true);
}
void follow_line(long q)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < q){
if(SENSOR_3 <= BLACK){
left_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
right_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_lineR(long p)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < p){
if(SENSOR_3 <= BLACK){
right_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
left_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_line_for_intersection(long t)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < t){
if(SENSOR_3 <= BLACK){
left_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
}else if(WHITE <= SENSOR_3){
right_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_line_for_intersectionR(long y)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < y){
if(SENSOR_3 <= BLACK){
right_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
}else if(WHITE <= SENSOR_3){
left_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
void fetch_ball()
{
SetSensorLowspeed(S2);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < 200){
if(SensorUS(S1) <= 15){
Off(OUT_BC);
RotateMotor(OUT_A,-SPEED,70);
}else{
gofwd
t0=CurrentTick();
}
}
Off(OUT_BC);
}
void open()
{
OnFwd(OUT_A,30);
Wait(400);
Off(OUT_A);
}
void close()
{
OnRev(OUT_A,30);
Wait(700);
Off(OUT_A);
}
void followC()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
int tenkan = GetAngle(70);
RotateMotorEx(OUT_BC,SPEED,tenkan,100,true,true);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followF()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(66);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followE()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(50);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}else if(SENSOR_3<GBLACK){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}
}
}
}
void catch()
{
SetSensorLowspeed(S2);
long tO;
tO = CurrentTick();
while(CurrentTick()-tO < 10000){
int O = SensorUS(S2);
if(O <= 11){
break;
}else if(11 < O){
Wait(STEP);
}
Wait(STEP);
Wait(2500);
}
}
task main()
{
RotateMotorEx(OUT_BC,SPEED_H,180,0,true,true);
follow_line_for_intersection(10000);
follow_line(200);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(1500);
followC(); // C開始
follow_line_for_intersection(750);
follow_line(150);
RotateMotor(OUT_C,SPEED,150);
turnR(40);
RotateMotorEx(OUT_BC,SPEED_L,60,0,false,true);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(10000);
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(135);
RotateMotor(OUT_C,SPEED,70);
follow_line_for_intersection(1000);
follow_line(180);
follow_line_for_intersection(1000);
follow_line(180);
back(120);
open(); // C終了
back(160);
turnR(130);
RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true);
follow_line_for_intersection(2000);
followF(); // F開始
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(9000);
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(140);
follow_line_for_intersectionR(1000);
follow_lineR(140);
RotateMotor(OUT_C,SPEED_H,90);
follow_line(160);
back(120);
open(); //F 終了
back(150);
turnL(77);
RotateMotorEx(OUT_BC,SPEED_L,50,0,false,true);
follow_line_for_intersectionR(1000);
follow_lineR(140);
follow_line_for_intersectionR(2000);
followE(); // E開始
follow_line_for_intersection(1500);
follow_line(160);
RotateMotor(OUT_C,SPEED,75);
follow_line_for_intersection(500);
follow_line(160);
follow_line_for_intersection(3000);
Wait(15000);
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(150);
follow_line_for_intersectionR(1500);
follow_lineR(160);
RotateMotorEx(OUT_BC,SPEED_L,65,0,false,true);
RotateMotor(OUT_B,SPEED,30);
follow_line_for_intersectionR(1000);
follow_lineR(150);
follow_line_for_intersectionR(1750);
follow_lineR(140);
back(120);
turnL(15);
open(); // E終了
back(300);
}
**関数、定義など [#s6035e80]
***定義 [#w1ea71db]
#define WHITE 56 // 白色
#define GWHITE 48 // 白灰色
#define GBLACK 40 // 黒灰色
#define BLACK 34 // 黒色
#define SPEED_H 47 // ハイスピード
#define SPEED 43 // ノーマルスピード
#define SPEED_L 40 // ロースピード
#define STEP 1
#define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go OnFwdSync(OUT_BC,SPEED_H,0); // 直進
#define gofwd OnFwdSync(OUT_BC,SPEED_L,0); // 直進（スロー）
#define left_rotation_s OnRL(SPEED,-SPEED); // 高速左回転
#define left_rotation OnRL(SPEED_L,-SPEED_L); // 左回転
#define turn_left Off(OUT_C);OnFwd(OUT_B,SPEED); // 左旋回
#define turn_right Off(OUT_B);OnFwd(OUT_C,SPEED); // 右旋回
#define right_rotation OnRL(-SPEED_L,SPEED_L); // 右回転
#define right_rotation_s OnRL(-SPEED,SPEED); // 高速右回転
ライントレースのための色の値、速度、旋回や回転などを定義した。回転は二つのタイヤを逆回転させてその場で方向を変えること、旋回は一つのタイヤだけを回転させ、カーブを描きながら進むことを示す。
***角度の計算 [#c8a742e0]
float GetAngle(float d)
{
const float diameter = 5.45;
const float pi=3.1415;
const float distance=12.1;
float ang = (distance*d)/diameter;
return ang;
}
括弧内（float dにあたる）に方向転換したい角度を入れ、タイヤの回転角度を計算する。計算した角度はロボットを回転させるプログラムで使用する。
***回転 [#yf78dbe0]
void turnR(long u)
{
int angle = GetAngle(u);
RotateMotorEx(OUT_BC,SPEED,angle,100,true,true);
}
void turnL(long l)
{
int angle = GetAngle(l);
RotateMotorEx(OUT_BC,SPEED,angle,-100,true,true);
}
- turnR 右回転
- turnL 左回転 のプログラム。
括弧内（long u と long l にあたる）に方向転換したい角度を入れ、回転する。計算して出した角度はここで使用する。
***後退 [#p4f8c3aa]
void back(long b)
{
RotateMotorEx(OUT_BC,-SPEED,b,0,true,true);
}
タイヤ２つを同時に動かし後退するプログラム。括弧内（long bにあたる）にタイヤの回転角度を入れ、その分後退する。
***ライントレース [#q4c8ac20]
void follow_line(long q)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < q){
if(SENSOR_3 <= BLACK){
left_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
right_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_lineR(long p)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < p){
if(SENSOR_3 <= BLACK){
right_rotation
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
left_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
- follow_line 黒い線の左縁をトレースする
- follow_lineR 黒い線の右縁をトレースする プログラム。
ラインの縁の進行中に交差点を感知すると停止する。交差点は光センサーが黒色を感知する時間（long q, long pの部分に入力する）で判断するようになっている。最初にlong t0とし、t0 = CurrentTick()でサブルーチンの開始時刻を設定しており、またt0 = CurrentTick()が黒色以外を感知した場合のプログラムに入り、時間をリセットする。そのためセンサーが黒色を感知している間に限り時間が経過し、whileの括弧内で定められている時間が経過すると、このループから抜けて次のプログラムへと移行する。
void follow_line_for_intersection(long t)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < t){
if(SENSOR_3 <= BLACK){
left_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_right
}else if(WHITE <= SENSOR_3){
right_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
void follow_line_for_intersectionR(long y)
{
SetSensorLight(S3);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < y){
if(SENSOR_3 <= BLACK){
right_rotation_s
}else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){
turn_left
}else if(WHITE <= SENSOR_3){
left_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
- follow_line_for_intersection 交差点を無視して黒い線の左縁をトレースする
- follow_lineR_for_intersection 交差点を無視して黒い線の右縁をトレースする プログラム。
これらのプログラムは最初の括弧内（long t, long yにあたる）にライントレース全体の時間を入力する。交差点ではなく時間で停止するところがfollow_line, follow_lineRと異なるが、基本的には同じになっている。
***缶の捕捉 [#hbd61204]
void fetch_ball()
{
SetSensorLowspeed(S2);
long t0;
t0 = CurrentTick();
while(CurrentTick()-t0 < 200){
if(SensorUS(S1) <= 15){
Off(OUT_BC);
RotateMotor(OUT_A,-SPEED,70);
}else{
gofwd
t0=CurrentTick();
}
}
Off(OUT_BC);
}
超音波センサーが缶を感知するとアームが缶を捉えるプログラム。具体的には、センサーが１５cmより大きい値を感知してる間は前進し、１５cm以下の値を0,2秒間感知すると前進をやめてモーターが回り、アームを閉じるようになっている。しかしアームをRotateMotorで動かすと缶を掴む際の調整が細かく必要になるため、使用しなかった。
void open()
{
OnFwd(OUT_A,30);
Wait(400);
Off(OUT_A);
}
void close()
{
OnRev(OUT_A,30);
Wait(700);
Off(OUT_A);
}
- open() アームを開く
- close() アームを閉じる プログラム。
缶を掴む、離すという動作を確実にしたかったため、少しばかり必要以上にアームを動かすようにした。使用したのがRotateMotorではないため、プログラム通りに開閉せずプログラムが停止する、ということは起きなかった。
void followC()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
int tenkan = GetAngle(70);
RotateMotorEx(OUT_BC,SPEED,tenkan,100,true,true);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followF()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(66);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}else if(SENSOR_3<GBLACK){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}
}
}
}
void followE()
{
SetSensorLowspeed(S2);
SetSensorLight(S3);
while(true){
int s=SensorUS(S2);
if(s<15){
Off(OUT_BC);
Wait(300);
close();
back(120);
turnR(50);
break;
}else{
if(SENSOR_3<BLACK){
RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true);
}else if(SENSOR_3<GBLACK){
Off(OUT_B);
OnFwd(OUT_C,70);
}else if(SENSOR_3<GWHITE){
OnFwd(OUT_BC,100);
Wait(10);
Off(OUT_BC);
}else if(SENSOR_3<WHITE){
OnFwd(OUT_B,70);
Off(OUT_C);
}else if(SENSOR_3>=WHITE){
RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true);
}
}
}
}
- followC() C地点の缶を捉える
- followF() F地点の缶を捉える
- followE() E地点の缶を捉える ためのプログラム。
地点によって内容は当然異なるが、
+ ライントレース
+ 空き缶の感知
+ アームを閉じる
+ 方向転換
という流れになっている。このプログラムでは方向転換まで済んで、その後抜け出すためにbreakが使用されている。
void catch()
{
SetSensorLowspeed(S2);
long tO;
tO = CurrentTick();
while(CurrentTick()-tO < 10000){
int O = SensorUS(S2);
if(O <= 11){
break;
}else if(11 < O){
Wait(STEP);
}
Wait(STEP);
Wait(2500);
}
}
ボールの受け渡しをする場所に着いてから１０秒たつ、またはボールを感知して２,５秒たつと次へいくプログラム。しかし実際にやってみるとボールが球体であるが故にセンサーが捉えたり捉えなかったりしたため、fetch_ballと同様に最終的には使用されなくなった。
**task main [#a6d9e75d]
***地点Cへの移動（区間A-J-H-G-C） [#m15ca196]
RotateMotorEx(OUT_BC,SPEED_H,180,0,true,true);
follow_line_for_intersection(10000);
follow_line(200);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(1500);
最初に直進してAの枠から出た後、ライントレースを開始する。最初の左カーブでは黒色を感知する時間が長く、センサーが交差点と認識してしまうことがあったため、交差点を無視するライントレースの時間を設けた。また、交差点を感知した直後に少し交差点を無視する時間を設けないと１つの交差点で複数の交差点に当たったと認識してしまうため、そこでも無視する時間を設けた。
***１つめの缶 （区間C-G-H-I-H-G-C） [#d05f3a60]
followC(); // C開始
follow_line_for_intersection(750);
follow_line(150);
RotateMotor(OUT_C,SPEED,150);
turnR(40);
RotateMotorEx(OUT_BC,SPEED_L,60,0,false,true);
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(10000);
followCでC地点の空き缶を掴んで方向転換まで行い、その後ライントレースで向きを微調整しつつ交差点Gまで進む。そこをRotateMotor, turnR等で右斜め前に進行するかたちで乗り越え、ライントレースを再開し、ボールの受け渡し場で停止する。ボールの受け渡しが完了したかどうかの確認のプログラムは無く、時間で進行していく。
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(135);
RotateMotor(OUT_C,SPEED,70);
follow_line_for_intersection(1000);
follow_line(180);
follow_line_for_intersection(1000);
follow_line(180);
back(120);
open(); // C終了
ボールの受け渡しを行った後、引き返すことになっているが、その場で回転すると地点Fの缶やもう一つのロボットに当たることがあった。そのため、右後ろに後退、前進、左回転という手順を踏んで戻る。これは２つめ、３つめの缶の場合も同様に行われる。そして地点Cに行くには交差点Hを感知する必要があるため直後は右トレースをする。交差点を感知した後は左トレースに変更し、交差点の感知を利用して、地点Cに缶とボールを置く。
#ref(./Turn.png,center,50%);
***地点Fへの移動 （区間C-G-F） [#af89a04a]
back(160);
turnR(130);
RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true);
follow_line_for_intersection(2000);
１つめの缶が終わったらアームが缶に当たらない位置まで後退し２つめの缶を目指す。向きを変え交差点Gを越えて地点Fへ向かう。
***２つめの缶 （区間F-G-H-I-H-G-F） [#if5dd39e]
followF(); // F開始
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(9000);
１つめの缶のときと同じように方向転換、位置調整を行いながら進行していく。地点Fの缶をとった後は左トレースで受け取り場まで行き、もう一つのロボットが来るまで停止する。
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(140);
follow_line_for_intersectionR(1000);
follow_lineR(140);
RotateMotor(OUT_C,SPEED_H,90);
follow_line(160);
back(120);
open(); //F 終了
ボール受け取った後、方向転換し右トレースで地点Gまで戻り、そこで線を乗り越える。左トレースに切り替え交差点の感知を利用して、地点Fに２つめの缶とボールを置く。
***地点Eへの移動 （区間F-G-D-E） [#r15de770]
back(150);
turnL(77);
RotateMotorEx(OUT_BC,SPEED_L,50,0,false,true);
follow_line_for_intersectionR(1000);
follow_lineR(140);
follow_line_for_intersectionR(2000);
１つめと同様に、アームが缶に当たらない位置まで後退し３つめの缶を目指す。向きを変え、右トレースで地点Eへ向かう。
***３つめの缶 （区間E-D-G-H-I-H-G-D-E） [#s67a5821]
followE(); // E開始
follow_line_for_intersection(1500);
follow_line(160);
RotateMotor(OUT_C,SPEED,75);
follow_line_for_intersection(500);
follow_line(160);
follow_line_for_intersection(3000);
Wait(15000);
最後の缶に取りかかる。followEで掴みから方向転換まで済ませ、左トレースをする。交差点Gを感知したらRotateMotorで乗り越え、引き続き左トレースで進み、受け取り場まで行く。
RotateMotor(OUT_C,-SPEED,330);
RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true);
turnL(40);
follow_lineR(150);
follow_line_for_intersectionR(1500);
follow_lineR(160);
RotateMotorEx(OUT_BC,SPEED_L,65,0,false,true);
RotateMotor(OUT_B,SPEED,30);
follow_line_for_intersectionR(1000);
follow_lineR(150);
follow_line_for_intersectionR(1750);
follow_lineR(140);
back(120);
turnL(15);
open(); // E終了
back(300);
ボール受け取った後、方向転換し右トレースで地点Gまで戻り、直進や旋回で線を乗り越える。再び右トレースで進み、交差点の感知を利用して地点Eに行く。位置調整のために少し左回転をし、最後の缶とボールを地点Eに置く。アームを開き、後退で距離をとり終了。
*まとめ [#u09588fa]
最初にロボット２台を起動する際、同時にスイッチを入れるようにしていたが、通信機能を利用していれば、人間が呼吸を合わせることなく楽に開始ができた。本番では思ったようにはいかず、最後まで改善点が見つかる結果となってしまったものの、１つだけでもボールの受け渡しが本番でできたのは嬉しかった。課題に取り組み、機械の扱いの難しさを身にしみて理解することができた。
ページ名:
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
