2019b/Member/Hayashi/Mission3
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
[[2019b/Member]]
*目次 [#m04647f0]
#contents
*課題3 [#z24163fc]
青と赤のボールを運搬して、空き缶の上に載せる。
#ref(kadai.png)
-空き缶を6本用意し、そのうち3本をC,E,Fの交差点に描いた半...
-同様に残りの空き缶をC', E', F' 地点に置く。
**ボールを運ぶ側(自分担当） [#oc7024b6]
+まず、Cでボールをつかみ、C→G→H→Iの順番で、缶を運ぶほうの...
+次に、I→H→G→Fの道を通り、Fでボールをつかんだ後、F→G→H→I...
+最後に、I→H→G→D→Eの道を通り、Eでボールをつかんだ後、E→D→...
*ロボットの説明 [#y32fba97]
**ボールを運ぶ側（自分担当） [#d83cded7]
下のような、ロボットを作りました。
#ref(5.jpeg)
缶よりも高い位置にある、ボールをとれるように、アームの高...
#ref(2.jpeg)
歯車を回している間や、車体が移動している間に、固定をしな...
#ref(4.jpeg)
左から見ると、こんな感じです。
**缶を運ぶ側（相手担当） [#o658b4bd]
#ref(9.jpeg)
コントローラーを上部に持っていき、ロボットの土台とコント...
#ref(10.jpeg)
缶を掴むためのアームは、上下で缶を支えるようにしてありま...
*プログラムの説明 [#g9e0166e]
**ボールを運ぶ側（自分担当） [#hd03e177]
#define THRESHOLD 52 //閾値
#define SPEED_H 35 //ハイスピード
#define SPEED_L 25 //ロウスピード
#define onRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OU...
#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,SPE...
#define u_turnr1
OnRev(OUT_BC,SPEED_L);Wait(1400);RotateMotorEx(OUT_BC,SP...
#define u_turnl2
OnRev(OUT_BC,SPEED_L);Wait(1200);RotateMotorEx(OUT_BC,SP...
#define u_turnr2
OnRev(OUT_BC,SPEED_L);Wait(1000);RotateMotorEx(OUT_BC,SP...
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(7500);
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();
}
***関数 [#f490d1ca]
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
この関数は、上で定義した"u-turn"での回る角度を計算するた...
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++;
}
}
右トレースして、交差点を2回渡るときに、使いました。
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++;
}
}
左トレースして、交差点を2回渡るときに、使いました。
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++;
}
}
左トレースで、交差点を1回渡るときに、使いました。
***task main()の中身 [#te133ba1]
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
上のように、90度回転する場合と、60度回転する場合の2つを定...
****1個目 [#w3ba509f]
OnFwd(OUT_BC,30);
Wait(500);
Aを出発
catch_balll();
J→H→G→Cまで左トレースして、缶を認識して、つかむ
u_turnl2;
follow_stop();
Cで左に回転して、空白のところを行き、線GH上に行く、そして...
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Hの場所で、渡り、右トレースして、Iの場所で運んできた缶に...
Wait(7500);
向こうの機械が、ボールが乗った缶を持って去るのを、待つ。
****2個目 [#c839102a]
u_turnl1;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
Iの位置で、左回転し、右トレースをする。（曲がる間に交差点...
follow_liner();
catch_ballr();
HとGの交差点で、右に曲がるようにする。そして、G→Fでトレー...
u_turnr1;
follow_line2();
catch_releaser();
Fで右回転し、空白の場所に行ったあと、線GHでライントレース...
Wait(7500);
ボールを乗せた管を運ぶ機械が、去るまで待つ。
****3個目 [#s6cd99ae]
u_turnl2;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
Iで左回転し、右トレースするようにする。（曲がる間に交差点...
follow_stop();
turn_right1;
Wait(1000);
Hの交差点で止まり、右に曲がる。
follow_line1();
Gの交差点を一回だけ渡る。
follow_stop();
turn_right1;
Wait(1000);
Dの交差点で止まり、右に曲がる。
catch_ballr();
u_turnr1;
D→Eでボールをつかんだ後、右に回転して、空白のところを行き...
follow_line2();
Gを渡る。
follow_line2();
catch_releaser();
Hを渡り、H→Iで右トレースして、運んできた缶にボールを乗せ...
**缶を運ぶ側（相手担当） [#k7e2de8d]
#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(OU...
#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 < GBLAC...
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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);
}
***関数 [#vd84cf21]
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 < GBLAC...
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
turn_left
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
left_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
ライントレースは定義した白から黒までの4段階感知を使ってト...
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 < GBLAC...
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
turn_left
}else if(WHITE <= SENSOR_3){
left_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
上記の関数はfollow_line、follow_lineRの交差点を感知せず指...
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...
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()の中身 [#o1a6a17d]
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にたどり着くためのプログラムです。最初の１０秒間は...
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で缶を探しながらライントレースを行います。缶を見つ...
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終...
最初の３行はボールを受け取ったあとの動きです。以下の図の...
#ref(12.jpg)
H交差点を感知したあとは左のタイヤだけ回転させることで曲が...
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);
Fで缶を掴みIまで持っていきます。これは1つ目の缶を持ってい...
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つ目の缶でボールを受け取ったあとと同じように方向転換をし...
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で缶を置いたときと同じようにアームが缶に当たらないように...
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を感...
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の交差点感知から直進後は進路が右に向きすぎているため、右...
back(300);
}
最後に缶から離れて終了です。
*まとめ [#e0ed3342]
今回は，ボールを運ぶ側と，缶を運ぶ側の二手に分かれて，機...
終了行:
[[2019b/Member]]
*目次 [#m04647f0]
#contents
*課題3 [#z24163fc]
青と赤のボールを運搬して、空き缶の上に載せる。
#ref(kadai.png)
-空き缶を6本用意し、そのうち3本をC,E,Fの交差点に描いた半...
-同様に残りの空き缶をC', E', F' 地点に置く。
**ボールを運ぶ側(自分担当） [#oc7024b6]
+まず、Cでボールをつかみ、C→G→H→Iの順番で、缶を運ぶほうの...
+次に、I→H→G→Fの道を通り、Fでボールをつかんだ後、F→G→H→I...
+最後に、I→H→G→D→Eの道を通り、Eでボールをつかんだ後、E→D→...
*ロボットの説明 [#y32fba97]
**ボールを運ぶ側（自分担当） [#d83cded7]
下のような、ロボットを作りました。
#ref(5.jpeg)
缶よりも高い位置にある、ボールをとれるように、アームの高...
#ref(2.jpeg)
歯車を回している間や、車体が移動している間に、固定をしな...
#ref(4.jpeg)
左から見ると、こんな感じです。
**缶を運ぶ側（相手担当） [#o658b4bd]
#ref(9.jpeg)
コントローラーを上部に持っていき、ロボットの土台とコント...
#ref(10.jpeg)
缶を掴むためのアームは、上下で缶を支えるようにしてありま...
*プログラムの説明 [#g9e0166e]
**ボールを運ぶ側（自分担当） [#hd03e177]
#define THRESHOLD 52 //閾値
#define SPEED_H 35 //ハイスピード
#define SPEED_L 25 //ロウスピード
#define onRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OU...
#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,SPE...
#define u_turnr1
OnRev(OUT_BC,SPEED_L);Wait(1400);RotateMotorEx(OUT_BC,SP...
#define u_turnl2
OnRev(OUT_BC,SPEED_L);Wait(1200);RotateMotorEx(OUT_BC,SP...
#define u_turnr2
OnRev(OUT_BC,SPEED_L);Wait(1000);RotateMotorEx(OUT_BC,SP...
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(7500);
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();
}
***関数 [#f490d1ca]
float GetAngle(float d)
{
const float diameter = 5.5;
const float distance=12;
float ang = (distance*d)/diameter;
return ang;
}
この関数は、上で定義した"u-turn"での回る角度を計算するた...
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++;
}
}
右トレースして、交差点を2回渡るときに、使いました。
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++;
}
}
左トレースして、交差点を2回渡るときに、使いました。
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++;
}
}
左トレースで、交差点を1回渡るときに、使いました。
***task main()の中身 [#te133ba1]
int angle1 = GetAngle(90);
int angle2 = GetAngle(60);
上のように、90度回転する場合と、60度回転する場合の2つを定...
****1個目 [#w3ba509f]
OnFwd(OUT_BC,30);
Wait(500);
Aを出発
catch_balll();
J→H→G→Cまで左トレースして、缶を認識して、つかむ
u_turnl2;
follow_stop();
Cで左に回転して、空白のところを行き、線GH上に行く、そして...
OnFwd(OUT_BC,25);
Wait(200);
catch_releaser();
Hの場所で、渡り、右トレースして、Iの場所で運んできた缶に...
Wait(7500);
向こうの機械が、ボールが乗った缶を持って去るのを、待つ。
****2個目 [#c839102a]
u_turnl1;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
Iの位置で、左回転し、右トレースをする。（曲がる間に交差点...
follow_liner();
catch_ballr();
HとGの交差点で、右に曲がるようにする。そして、G→Fでトレー...
u_turnr1;
follow_line2();
catch_releaser();
Fで右回転し、空白の場所に行ったあと、線GHでライントレース...
Wait(7500);
ボールを乗せた管を運ぶ機械が、去るまで待つ。
****3個目 [#s6cd99ae]
u_turnl2;
Wait(500);
OnFwd(OUT_BC,25);
Wait(100);
Iで左回転し、右トレースするようにする。（曲がる間に交差点...
follow_stop();
turn_right1;
Wait(1000);
Hの交差点で止まり、右に曲がる。
follow_line1();
Gの交差点を一回だけ渡る。
follow_stop();
turn_right1;
Wait(1000);
Dの交差点で止まり、右に曲がる。
catch_ballr();
u_turnr1;
D→Eでボールをつかんだ後、右に回転して、空白のところを行き...
follow_line2();
Gを渡る。
follow_line2();
catch_releaser();
Hを渡り、H→Iで右トレースして、運んできた缶にボールを乗せ...
**缶を運ぶ側（相手担当） [#k7e2de8d]
#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(OU...
#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 < GBLAC...
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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);
}
***関数 [#vd84cf21]
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 < GBLAC...
turn_left
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
t0 = CurrentTick();
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
t0 = CurrentTick();
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
turn_left
t0 = CurrentTick();
}else if(WHITE <= SENSOR_3){
left_rotation
t0 = CurrentTick();
}
Wait(STEP);
}
Off(OUT_BC);
}
ライントレースは定義した白から黒までの4段階感知を使ってト...
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 < GBLAC...
turn_left
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
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 < GBLAC...
turn_right
}else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GW...
go
}else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHIT...
turn_left
}else if(WHITE <= SENSOR_3){
left_rotation
}
Wait(STEP);
}
Off(OUT_BC);
}
上記の関数はfollow_line、follow_lineRの交差点を感知せず指...
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...
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()の中身 [#o1a6a17d]
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にたどり着くためのプログラムです。最初の１０秒間は...
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で缶を探しながらライントレースを行います。缶を見つ...
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終...
最初の３行はボールを受け取ったあとの動きです。以下の図の...
#ref(12.jpg)
H交差点を感知したあとは左のタイヤだけ回転させることで曲が...
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);
Fで缶を掴みIまで持っていきます。これは1つ目の缶を持ってい...
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つ目の缶でボールを受け取ったあとと同じように方向転換をし...
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で缶を置いたときと同じようにアームが缶に当たらないように...
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を感...
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の交差点感知から直進後は進路が右に向きすぎているため、右...
back(300);
}
最後に缶から離れて終了です。
*まとめ [#e0ed3342]
今回は，ボールを運ぶ側と，缶を運ぶ側の二手に分かれて，機...
ページ名:
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
