2019b/Member/jh1dmz/Mission3
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
[[2019b/Member]]
#contents
*課題３ [#r533db35]
青と赤のボールを運搬して、空き缶の上に載せる。
*戦略 [#c9cd3ca8]
**概観 [#t5118e8f]
缶を中央のIの先に持っていくロボットとボールを同じく中央に持っていくロボットの２つを制作して対応する。中央で缶にボールを載せて缶を持ってきたロボットが元の位置に戻す。
ロボット、プログラム制作は二人一組でロボットごとに別れて行った。~
自分は缶を運ぶ側のロボットの制作である。~
**担当したロボットの戦略 [#f031ac71]
ルートは以下のようにした。３つ全ての缶でIの先でボールの受け渡しを行うため、Iが行きの終着点となる。
１つ目の缶
行き A → J → H(左折) → G(左折) → C(缶を掴む) → G(右折) → H(左折) → I~
帰り I → H(右折) → G(左折) → C(缶を離す)
２つ目の缶
行き C → G(直進) → F(缶を掴む) → G(左折) → H(左折) → I~
帰り I → H(右折) → G(右折) → F(缶を離す)
３つ目の缶
行き F → G(右折) → D(右折) → E(缶を掴む) → D(左折) → G(直進) → H(左折) → I~
帰り I → H(右折) → G(直進) → D(右折) → E(缶を離す)
*ロボットについて [#h402ded3]
**缶を運ぶ側 [#hfa51697]
&ref(2019b/Member/jh1dmz/Mission3/DSC_0927.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0928.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0936.jpg,10%);
ロボットの基本形をもとに今回も制作した。~
コントローラーを上部に持っていき、ロボットの土台とコントローラーの間に缶を掴む用のアームを動かすためのモーターを組み込んだ。~
また、コントローラーから超音波センサーを吊り下げるように下向きに取り付けた。~
これによって、缶がアームに入っている状態では超音波センサーの値が小さくなる。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0934.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0935.jpg,10%);
缶を掴むためのアームは、上下で缶を支えるようにしてある。~
そして４つあるアームのうち一つを長くすることで、万が一缶が完全にアーム内に入っていない状態でアームを閉じたとしても引き寄せられるようになる。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0929.jpg,10%);
光センサーはアームの内側に取り付けてある。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0939.jpg,10%);
光センサーを中心にうまく収め、アームと干渉しないようにアームとモーターの位置を調節した。
**ボールを運ぶ側 [#w5db20fe]
&ref(2019b/Member/jh1dmz/Mission3/DSC_0944.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0945.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0946.jpg,10%);
アームが缶に乗っているボールの位置になるように調整してある。~
アームを動かすモーターはコントローラー斜め上に設置し、回転の向きを変えてアームまで回転を伝えている。~
超音波センサーは光センサーの直上に設置してあり、これで缶との位置を認識する。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0949.jpg,10%);
アームを上から固定している。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0943.jpg,10%);
上の写真の右のアームにL字のパーツを付けることで、アームを開くときに掴んでいるボールを押し出し、缶の上にうまく乗るようになる。
*プログラムについて（缶を運ぶ側） [#g5d34081]
**定義 [#z1bb3a36]
#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); // 高速右回転
定義ではライントレース時の白から黒までの定義値、各種スピードなどを定義した。
ここでの左回転、右回転は左右のタイヤを逆回転させて回転することである。また、左旋回、右旋回は片方のタイヤを止めて旋回することである。これらは高速と通常速度の２種類を定義した。
**関数・サブルーチン [#ldcc8c3c]
***角度の計算 [#c70b8515]
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;
}
この関数は以前のMissionで作ったものを流用した。dに数値を入れるとその角度に向くためのタイヤの回転角を出力する。
***基本動作 [#re53d045]
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);
}
上から順に入力した方向に右回転、左回転する関数、入力した数値の分だけ後退する関数である。
***ライントレース [#g8fdf42a]
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);
}
ライントレースは先程定義した白から黒までの4段階感知を使ってトレースするようにした。~
灰色のときは直進で黒灰、白灰では片方のタイヤのみ回転する旋回で、黒、白を感知すると左右のタイヤを逆回転させて大幅に修正するようにしている。~
また、黒をq[ms]間感知すると交差点と判断し停止する。~
変数qは交差点の感知する感度を調整することができるようにするために導入した。~
follow_lineは黒線の左トレースでfollow_lineRは黒線の右側をトレースするようになっている。どちらも黒が続いた時間で交差点を判断する。
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、follow_lineRの交差点を感知せず指定時間だけひたすらライントレースをするプログラムである。基本的な動きは変わらない。時間指定は変数yになっている。以下、説明部ではintersectionと略して使用する。
***缶を掴む・離す [#w65466aa]
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);
}
}
}
}
これらはすべて缶を掴む・離すことに使用する関数である。~
fetch_ballやopen、closeでアームを動かす。~
followC、followE、followFでは缶が置いてあるそれぞれの交差点でそれぞれに数値を最適化させたものをまとめている。これらは缶と超音波センサーの距離が15cm以下になったときはclose関数を使用して掴む動作を行い、缶との距離が15cm以上であればライントレースを行うようになっている。
***ボールの感知 [#d81748dd]
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);
}
}
この関数では超音波センサーによってボールを感知するプログラムである。缶を持っていき、ボールを乗せてもらったときに超音波センサーが計測する値は缶だけのときよりも小さくなる。そのためこの関数では11cm以下になったときにボールがあると捉えるようにした。~
試行段階では使用していたが完成版にでは使われていない。しかし、制作段階で使ったり使わなかったりしていたため、プログラム内には残してある。
**task main [#yf7454f4]
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);
最初のCにたどり着くためのプログラムである。最初の１０秒間はきついカーブの交差点の誤認識を防止するために交差点を認識しないintersectionを入れている。~
３行目のfollow_line(200)でHの交差点を認識し、その後のintersectionで交差点を曲がり切り、次のfollow_lineで次のG交差点を探しながらライントレースする。~
今後も交差点の認識と通過はこの仕組である。~
それぞれのintersectionで数値が異なるのはその状況に合わせて細かく調整をしているからである。
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で缶を探しながらライントレースを行う。缶を見つけると缶を掴んだあとにUターンをする。~
Uターン後は位置が安定しないため、intersectionで整える。その後にG交差点を感知してRotateMotor以下３行でG交差点を曲がる。intersectionで曲がらないのは、確実に次のライントレースを始められるように位置を調整するためである。~
そして、最後のH交差点を曲がり、３秒間進んだところで停止しボールが運ばれてくるのを待つ。~
待っている10秒間でボールを缶に乗せてもらう。そして次の工程に進む。
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にある缶に当たることなく逆向きになる。
&ref(2019b/Member/jh1dmz/Mission3/プレゼンテーション.jpg,50%);
逆に向いたあとは黒線の右トレースをすることでH交差点を感知できるようにした。~
H交差点を感知したあとは左のタイヤだけ回転させることで曲がった先で黒線の左にセンサーが来るようになっている。~
Cまでは今まで通り交差点を通過していく。交差点Cを感知したところで後退して缶を離す。この動きをすることで缶を円の中に収めることができる。
back(160);
turnR(130);
RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true);
follow_line_for_intersection(2000);
一度下がり、アームが置いた缶に当たらないようにして右回転でUターンをする。~
その後、前進することで確実にCから見て交差点Gの先にセンサーが来るようになる。~
ライントレースを行い、Fに近づいていく。
followF(); // F開始
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(9000);
Fで缶を掴みIまで持っていく。これは1つ目の缶を持っていくときとほぼ変わらない。~
ボールを受け取るまでの時間調整で9秒間待つ。
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 終了
1つ目の缶でボールを受け取ったあとと同じように方向転換をして右側ライントレースをGまで行う。そこから先程と同じように左のタイヤだけ回転させて左トレースに戻り、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);
Cで缶を置いたときと同じようにアームが缶に当たらないように下がったあとにG-Dの黒線の右側にセンサーが来るように方向転換をする。そしてEまで右側トレースで進む。
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);
Eで缶を掴んだあとはそのままintersectionでDを通過し、Gを感知しても直進することで通過、Iまで行く。~
15秒間待ち、最後の缶を戻しに行く。
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);
ここまででボールを受け取ったあとの方向転換、からGまでのトレースである。
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終了
Gの交差点感知から直進後は進路が右に向きすぎているため、右タイヤのみ回転させて左に向きを調整する。これが一行目のRotateMotorである。~
そこからは今まで通りEまで運ぶ。~
最後にEだけは確実に円内に缶を収めるためにE交差点を感知して後退したあとに左に向きを調節している。
back(300);
}
最後に缶から離れて終了である。
*プログラムについて（ボールを運ぶ側） [#se7d821a]
**定義 [#s987e72d]
#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);
各種必要な数値や動きを定義した。速度はhighとlowの2段階である。~
turn_left、turn_rightの1が左右のタイヤの逆回転、０が片方のタイヤだけを回転させてロボットが回転する。
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
変数dに与えた角度に向くためのタイヤの回転数を返す関数である。
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++;
}
}
ボールを探しながら左トレースをひたすら続ける関数である。缶との距離が6.1以下で缶があると認識する。~
ラインの黒から白までの判断はTHRESHOLDからどれくらい離れているかで判断している。
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++;
}
}
右トレースをして、交差点を２回右折する関数である。~
最初のwhileで交差点が２回以下のときは続けるようになっており、次のwhileで交差点の判断を行う。
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の右トレース版である。
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++;
}
}
follow_linerの左トレース版である。
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++;
}
}
右トレースをしてボールを受け渡すときの関数である。~
缶を探しながらライントレースをし続け、超音波センサーの数値が6.2以下になったときにwhileループを脱して、ボールを離す。
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++;
}
}
follow_line1の左トレース版である。
**task main [#n1d3e449]
task main()
{
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
angle1の角度は90度、angle2の角度が60度と指定した。
OnFwd(OUT_BC,30);
Wait(500);
catch_balll();
Aを出発して左トレースを缶を感知するまでひたすら続ける。~
catch_balllは交差点を感知しないが、缶を感知するまで左トレースを続けるためH、Gを左折してCまでたどり着くことができる。~
缶を感知するとアームを閉じてボールを掴む。
u_turnl2;
follow_stop();
Uターンをして次の交差点を認識すると止まる。Uターン後の光センサーの位置から最初に交差点を感知するのはHとなるため、Hで一時停止する。
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Wait(7500);
Iまでボールを持っていき、１つ目受け渡し完了。
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();
}
最後に３つ目も運ぶ。
*感想・反省 [#ia9bfa34]
今回は２つのロボットでミッションをクリアしようとした。
缶を運ぶ側での制作を担当したが、ハードでの制作に思っていたよりも時間がかかってしまった。主にアームの取り付け位置やを試行錯誤することが多かった印象である。プログラミングは最初に作っていたもので色々試していくと電池の残量が減ってきて軌道が大幅にずれてきたり、缶への干渉が大きくなってきてそのたびに修正をしていくことが一番大変で時間がかかった。
どちらのロボットもロボコン前にはかなりの完成度になっており、ロボコンもうまくいくかなと思っていたが実際には一つボールを運べただけになってしまった。もっと再現性を上げられたら良かったなと思う。
このレポートを書くにあたってボールを運ぶ側のプログラムの説明はhttp://yakushi.shinshu-u.ac.jp/robotics/?2019b/Member/Hayashi/Mission3とhttp://yakushi.shinshu-u.ac.jp/robotics/?2019b/Member/udon/Mission3のお二人のレポートを参考にさせていただいた。
この授業で、実際にロボット作りとプログラミングを体験してものづくり、特にプログラミングの大変さを知った。今後はこれ以上に難しい問題にも出会っていくと思うが、この経験を生かして行きたいと思う。
終了行:
[[2019b/Member]]
#contents
*課題３ [#r533db35]
青と赤のボールを運搬して、空き缶の上に載せる。
*戦略 [#c9cd3ca8]
**概観 [#t5118e8f]
缶を中央のIの先に持っていくロボットとボールを同じく中央に持っていくロボットの２つを制作して対応する。中央で缶にボールを載せて缶を持ってきたロボットが元の位置に戻す。
ロボット、プログラム制作は二人一組でロボットごとに別れて行った。~
自分は缶を運ぶ側のロボットの制作である。~
**担当したロボットの戦略 [#f031ac71]
ルートは以下のようにした。３つ全ての缶でIの先でボールの受け渡しを行うため、Iが行きの終着点となる。
１つ目の缶
行き A → J → H(左折) → G(左折) → C(缶を掴む) → G(右折) → H(左折) → I~
帰り I → H(右折) → G(左折) → C(缶を離す)
２つ目の缶
行き C → G(直進) → F(缶を掴む) → G(左折) → H(左折) → I~
帰り I → H(右折) → G(右折) → F(缶を離す)
３つ目の缶
行き F → G(右折) → D(右折) → E(缶を掴む) → D(左折) → G(直進) → H(左折) → I~
帰り I → H(右折) → G(直進) → D(右折) → E(缶を離す)
*ロボットについて [#h402ded3]
**缶を運ぶ側 [#hfa51697]
&ref(2019b/Member/jh1dmz/Mission3/DSC_0927.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0928.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0936.jpg,10%);
ロボットの基本形をもとに今回も制作した。~
コントローラーを上部に持っていき、ロボットの土台とコントローラーの間に缶を掴む用のアームを動かすためのモーターを組み込んだ。~
また、コントローラーから超音波センサーを吊り下げるように下向きに取り付けた。~
これによって、缶がアームに入っている状態では超音波センサーの値が小さくなる。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0934.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0935.jpg,10%);
缶を掴むためのアームは、上下で缶を支えるようにしてある。~
そして４つあるアームのうち一つを長くすることで、万が一缶が完全にアーム内に入っていない状態でアームを閉じたとしても引き寄せられるようになる。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0929.jpg,10%);
光センサーはアームの内側に取り付けてある。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0939.jpg,10%);
光センサーを中心にうまく収め、アームと干渉しないようにアームとモーターの位置を調節した。
**ボールを運ぶ側 [#w5db20fe]
&ref(2019b/Member/jh1dmz/Mission3/DSC_0944.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0945.jpg,10%);
&ref(2019b/Member/jh1dmz/Mission3/DSC_0946.jpg,10%);
アームが缶に乗っているボールの位置になるように調整してある。~
アームを動かすモーターはコントローラー斜め上に設置し、回転の向きを変えてアームまで回転を伝えている。~
超音波センサーは光センサーの直上に設置してあり、これで缶との位置を認識する。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0949.jpg,10%);
アームを上から固定している。
&ref(2019b/Member/jh1dmz/Mission3/DSC_0943.jpg,10%);
上の写真の右のアームにL字のパーツを付けることで、アームを開くときに掴んでいるボールを押し出し、缶の上にうまく乗るようになる。
*プログラムについて（缶を運ぶ側） [#g5d34081]
**定義 [#z1bb3a36]
#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); // 高速右回転
定義ではライントレース時の白から黒までの定義値、各種スピードなどを定義した。
ここでの左回転、右回転は左右のタイヤを逆回転させて回転することである。また、左旋回、右旋回は片方のタイヤを止めて旋回することである。これらは高速と通常速度の２種類を定義した。
**関数・サブルーチン [#ldcc8c3c]
***角度の計算 [#c70b8515]
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;
}
この関数は以前のMissionで作ったものを流用した。dに数値を入れるとその角度に向くためのタイヤの回転角を出力する。
***基本動作 [#re53d045]
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);
}
上から順に入力した方向に右回転、左回転する関数、入力した数値の分だけ後退する関数である。
***ライントレース [#g8fdf42a]
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);
}
ライントレースは先程定義した白から黒までの4段階感知を使ってトレースするようにした。~
灰色のときは直進で黒灰、白灰では片方のタイヤのみ回転する旋回で、黒、白を感知すると左右のタイヤを逆回転させて大幅に修正するようにしている。~
また、黒をq[ms]間感知すると交差点と判断し停止する。~
変数qは交差点の感知する感度を調整することができるようにするために導入した。~
follow_lineは黒線の左トレースでfollow_lineRは黒線の右側をトレースするようになっている。どちらも黒が続いた時間で交差点を判断する。
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、follow_lineRの交差点を感知せず指定時間だけひたすらライントレースをするプログラムである。基本的な動きは変わらない。時間指定は変数yになっている。以下、説明部ではintersectionと略して使用する。
***缶を掴む・離す [#w65466aa]
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);
}
}
}
}
これらはすべて缶を掴む・離すことに使用する関数である。~
fetch_ballやopen、closeでアームを動かす。~
followC、followE、followFでは缶が置いてあるそれぞれの交差点でそれぞれに数値を最適化させたものをまとめている。これらは缶と超音波センサーの距離が15cm以下になったときはclose関数を使用して掴む動作を行い、缶との距離が15cm以上であればライントレースを行うようになっている。
***ボールの感知 [#d81748dd]
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);
}
}
この関数では超音波センサーによってボールを感知するプログラムである。缶を持っていき、ボールを乗せてもらったときに超音波センサーが計測する値は缶だけのときよりも小さくなる。そのためこの関数では11cm以下になったときにボールがあると捉えるようにした。~
試行段階では使用していたが完成版にでは使われていない。しかし、制作段階で使ったり使わなかったりしていたため、プログラム内には残してある。
**task main [#yf7454f4]
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);
最初のCにたどり着くためのプログラムである。最初の１０秒間はきついカーブの交差点の誤認識を防止するために交差点を認識しないintersectionを入れている。~
３行目のfollow_line(200)でHの交差点を認識し、その後のintersectionで交差点を曲がり切り、次のfollow_lineで次のG交差点を探しながらライントレースする。~
今後も交差点の認識と通過はこの仕組である。~
それぞれのintersectionで数値が異なるのはその状況に合わせて細かく調整をしているからである。
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で缶を探しながらライントレースを行う。缶を見つけると缶を掴んだあとにUターンをする。~
Uターン後は位置が安定しないため、intersectionで整える。その後にG交差点を感知してRotateMotor以下３行でG交差点を曲がる。intersectionで曲がらないのは、確実に次のライントレースを始められるように位置を調整するためである。~
そして、最後のH交差点を曲がり、３秒間進んだところで停止しボールが運ばれてくるのを待つ。~
待っている10秒間でボールを缶に乗せてもらう。そして次の工程に進む。
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にある缶に当たることなく逆向きになる。
&ref(2019b/Member/jh1dmz/Mission3/プレゼンテーション.jpg,50%);
逆に向いたあとは黒線の右トレースをすることでH交差点を感知できるようにした。~
H交差点を感知したあとは左のタイヤだけ回転させることで曲がった先で黒線の左にセンサーが来るようになっている。~
Cまでは今まで通り交差点を通過していく。交差点Cを感知したところで後退して缶を離す。この動きをすることで缶を円の中に収めることができる。
back(160);
turnR(130);
RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true);
follow_line_for_intersection(2000);
一度下がり、アームが置いた缶に当たらないようにして右回転でUターンをする。~
その後、前進することで確実にCから見て交差点Gの先にセンサーが来るようになる。~
ライントレースを行い、Fに近づいていく。
followF(); // F開始
follow_line_for_intersection(1000);
follow_line(200);
follow_line_for_intersection(3000);
Wait(9000);
Fで缶を掴みIまで持っていく。これは1つ目の缶を持っていくときとほぼ変わらない。~
ボールを受け取るまでの時間調整で9秒間待つ。
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 終了
1つ目の缶でボールを受け取ったあとと同じように方向転換をして右側ライントレースをGまで行う。そこから先程と同じように左のタイヤだけ回転させて左トレースに戻り、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);
Cで缶を置いたときと同じようにアームが缶に当たらないように下がったあとにG-Dの黒線の右側にセンサーが来るように方向転換をする。そしてEまで右側トレースで進む。
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);
Eで缶を掴んだあとはそのままintersectionでDを通過し、Gを感知しても直進することで通過、Iまで行く。~
15秒間待ち、最後の缶を戻しに行く。
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);
ここまででボールを受け取ったあとの方向転換、からGまでのトレースである。
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終了
Gの交差点感知から直進後は進路が右に向きすぎているため、右タイヤのみ回転させて左に向きを調整する。これが一行目のRotateMotorである。~
そこからは今まで通りEまで運ぶ。~
最後にEだけは確実に円内に缶を収めるためにE交差点を感知して後退したあとに左に向きを調節している。
back(300);
}
最後に缶から離れて終了である。
*プログラムについて（ボールを運ぶ側） [#se7d821a]
**定義 [#s987e72d]
#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);
各種必要な数値や動きを定義した。速度はhighとlowの2段階である。~
turn_left、turn_rightの1が左右のタイヤの逆回転、０が片方のタイヤだけを回転させてロボットが回転する。
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
変数dに与えた角度に向くためのタイヤの回転数を返す関数である。
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++;
}
}
ボールを探しながら左トレースをひたすら続ける関数である。缶との距離が6.1以下で缶があると認識する。~
ラインの黒から白までの判断はTHRESHOLDからどれくらい離れているかで判断している。
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++;
}
}
右トレースをして、交差点を２回右折する関数である。~
最初のwhileで交差点が２回以下のときは続けるようになっており、次のwhileで交差点の判断を行う。
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の右トレース版である。
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++;
}
}
follow_linerの左トレース版である。
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++;
}
}
右トレースをしてボールを受け渡すときの関数である。~
缶を探しながらライントレースをし続け、超音波センサーの数値が6.2以下になったときにwhileループを脱して、ボールを離す。
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++;
}
}
follow_line1の左トレース版である。
**task main [#n1d3e449]
task main()
{
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
angle1の角度は90度、angle2の角度が60度と指定した。
OnFwd(OUT_BC,30);
Wait(500);
catch_balll();
Aを出発して左トレースを缶を感知するまでひたすら続ける。~
catch_balllは交差点を感知しないが、缶を感知するまで左トレースを続けるためH、Gを左折してCまでたどり着くことができる。~
缶を感知するとアームを閉じてボールを掴む。
u_turnl2;
follow_stop();
Uターンをして次の交差点を認識すると止まる。Uターン後の光センサーの位置から最初に交差点を感知するのはHとなるため、Hで一時停止する。
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Wait(7500);
Iまでボールを持っていき、１つ目受け渡し完了。
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();
}
最後に３つ目も運ぶ。
*感想・反省 [#ia9bfa34]
今回は２つのロボットでミッションをクリアしようとした。
缶を運ぶ側での制作を担当したが、ハードでの制作に思っていたよりも時間がかかってしまった。主にアームの取り付け位置やを試行錯誤することが多かった印象である。プログラミングは最初に作っていたもので色々試していくと電池の残量が減ってきて軌道が大幅にずれてきたり、缶への干渉が大きくなってきてそのたびに修正をしていくことが一番大変で時間がかかった。
どちらのロボットもロボコン前にはかなりの完成度になっており、ロボコンもうまくいくかなと思っていたが実際には一つボールを運べただけになってしまった。もっと再現性を上げられたら良かったなと思う。
このレポートを書くにあたってボールを運ぶ側のプログラムの説明はhttp://yakushi.shinshu-u.ac.jp/robotics/?2019b/Member/Hayashi/Mission3とhttp://yakushi.shinshu-u.ac.jp/robotics/?2019b/Member/udon/Mission3のお二人のレポートを参考にさせていただいた。
この授業で、実際にロボット作りとプログラミングを体験してものづくり、特にプログラミングの大変さを知った。今後はこれ以上に難しい問題にも出会っていくと思うが、この経験を生かして行きたいと思う。
ページ名:
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
