マスター
//Tire AC
//Arm B
//sensor line S1
//sensor uhf S2
///////////////////////////////////////////////////////////////define//////////////////////////////////////////////////////////////////
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<ryu>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#define BLACK_01 27
#define BLACK_02 35
#define WHITE_01 50
#define WHITE_02 60
#define SPEED 30
#define go_forward OnFwd(OUT_AC,SPEED);
#define turn_right OnRev(OUT_A,SPEED);OnFwd(OUT_C,SPEED);
#define turn_left OnFwd(OUT_A,SPEED);OnRev(OUT_C,SPEED);
#define right Off(OUT_A);OnFwd(OUT_C,SPEED);
#define left OnFwd(OUT_A,SPEED);Off(OUT_C);
#define STEP 1
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<hayato>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#define CENTER 45
#define CCC 14000
#define SPEED_H 45
#define SPEED_L 25
#define turn_R Off(OUT_A); OnFwd(OUT_C,SPEED_L);
#define turn_L Off(OUT_C); OnFwd(OUT_A,SPEED_L);
#define turn_SR OnRev(OUT_A,SPEED_L); OnFwd(OUT_C,SPEED_L);
#define turn_SL OnRev(OUT_C,SPEED_L); OnFwd(OUT_A,SPEED_L);
#define go_F OnFwd(OUT_AC,SPEED_H);
#define nMAX 250
#define short_break Off(OUT_AC);Wait(400);
#define CROSS_TIME 400
#define cross_line OnFwd(OUT_A,SPEED_L);OnFwd(OUT_C,SPEED_L);Wait(CROSS_TIME);short_break;
#define cross OnFwd(OUT_C,SPEED_L);OnFwd(OUT_A,SPEED_L);Wait(CROSS_TIME+200);short_break;
#define go_b OnFwd(OUT_A,-40);OnFwd(OUT_C,-40);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>catch<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
#define catch OnFwd(OUT_B,33);Wait(600);Off(OUT_B);
#define throw OnRev(OUT_B,80);Wait(1000);Off(OUT_B);
///////////////////////////////////////////////////////////////////left////////////////////////////////////////////////////////////////////
sub right_n()
{
if (SENSOR_1<BLACK_01) {turn_left;}
else if (SENSOR_1<BLACK_02) {left;}
else if (SENSOR_1<WHITE_01) {go_forward;}
else if (SENSOR_1<WHITE_02) {right;}
else {turn_right;}Wait(STEP);
}
//////////////////////////////////////////////////////////////////cross_L//////////////////////////////////////////////////////////////
sub trace_R()
{
int n;
while(true) {
while(n < nMAX) {
if(SENSOR_1 < CENTER-12) {
turn_SL;
n++;
} else {
if(SENSOR_1 < CENTER-6) {
turn_L;
} else if(SENSOR_1 < CENTER+6) {
go_F;
} else if(SENSOR_1 < CENTER+12) {
turn_SL turn_R;
} else {
turn_SR;
}
n=0;
}
Wait(STEP);
}
short_break;
turn_SR;Wait(nMAX*STEP*2);
cross;
n=0;
break;
}
}
//////////////////////////////////////////////////////////////////cross_R///////////////////////////////////////////////////////////////
sub trace_L()
{
int n = 0;
while(true) {
while(n < nMAX) {
if(SENSOR_1 < CENTER-12) {
turn_SR;
n++;
} else {
if(SENSOR_1 < CENTER-6) {
turn_R;
} else if(SENSOR_1 < CENTER+6) {
go_F;
} else if(SENSOR_1 < CENTER+12) {
turn_L;
} else {
turn_SL;
}
n=0;
}
Wait(STEP);
}
short_break;
turn_SL;Wait(nMAX*STEP);
cross;
n=0;
break;
}
}
///////////////////////////////////////////////////////////////////right//////////////////////////////////////////////////////////////////
sub left_n()
{
if (SENSOR_1<BLACK_01) {
turn_right;
}
else if (SENSOR_1<BLACK_02) {
right;
}
else if (SENSOR_1<WHITE_01) {
go_forward;
}
else if (SENSOR_1<WHITE_02) {
left;
}
else {
turn_left;
}
Wait(STEP);
}
///////////////////////////////////////////////////////////////////main/////////////////////////////////////////////////////////////////
task main()
{
SetSensorLight(S1);
SetSensorLight(S2);
long timess = 0;
/////////////////////////////start///////////////////////////////////
trace_L();
Wait(1000);
go_b;
Wait(950);
turn_SL;
Wait(800);
PlaySound(SOUND_CLICK);//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>ブザー１
go_F;
Wait(550);
go_F;
catch;
while(SENSOR_1 > CENTER+8 )
{
Wait(STEP);
}
PlaySound(SOUND_CLICK);//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>ブザー２
Off(OUT_AC);
/////////////////////////////ボールキャッチ後///////////////////////////////
go_F;
Wait(90);
turn_SR;
Wait(750);
PlaySound(SOUND_CLICK);//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>ブザー３
/////////////////////////////////////////////////////////////////////////////////
//RotateMotor(OUT_C,70,1040);
//go_F;
//Wait(150);
Wait(1000);
//
//Wait(100);
trace_R();
turn_SR;
Wait(700);
trace_L();//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>左側トレース
Wait(1000);
PlaySound(SOUND_CLICK);
PlaySound(SOUND_CLICK);
//////////////////////////////二回目の交差点終了////////////////////////////////
//trace_L();
//////////////////////////////////三回目の交差点認識/////////////////////////
go_b;
Wait(800);
Off(OUT_AC);
Wait(100);
RotateMotor(OUT_A,-70,410);
throw;
RotateMotor(OUT_A,70,470);
go_F;
Wait(200);
///////////////////////////////////一回目の発射/////////////////////////////////(向き直っている)
//turn_SR;
//Wait(500);
timess = CurrentTick();
while(CurrentTick() - timess < 4000) {
left_n();
}
Off(OUT_AC);
Wait(2000);
RotateMotor(OUT_A,-70,440);
Off(OUT_AC);
Wait(10000);
catch;
catch;
RotateMotor(OUT_A,70,440);
//""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
go_F;
Wait(1700);
timess = CurrentTick();
Off(OUT_AC);
RotateMotor(OUT_A,-70,460);
Wait(2000);
throw;
//RotateMotor(OUT_C,70,410);
//通信動け
//もう片方の機種が動き出す
//相方機からポジションについた知らせが来る
//ボールを発射する
//2秒回転して９０度向きを左に帰る
//相方機がボール発射状態になったこと受け取る
//受け取り状態になったことを伝える。
}
スレイブ
#define BLACK_01 27
#define BLACK_02 35
#define WHITE_01 50
#define WHITE_02 58
#define SPEED 38
#define SPEED_H 50
#define SPEED_L 30
#define OnRL(speedR,speedL) OnFwd(OUT_A,speedR);OnFwd(OUT_C,speedL);
#define go_forward OnRL(SPEED,SPEED);
#define turn_right OnRev(OUT_C,SPEED);OnFwd(OUT_A,SPEED);
#define turn_left OnFwd(OUT_C,SPEED);OnRev(OUT_A,SPEED);
#define right Off(OUT_C);OnFwd(OUT_A,SPEED);
#define left OnFwd(OUT_C,SPEED);Off(OUT_A);
#define STEP 1
#define MOVE_TIME 8000
#define shoot OnRev(OUT_B,95);Wait(700);
#define go_b OnFwd(OUT_A,-50);OnFwd(OUT_C,-50);
#define go_F OnFwd(OUT_AC,SPEED_H);
#define turn_R Off(OUT_A); OnFwd(OUT_C,SPEED_L);
#define turn_L Off(OUT_C); OnFwd(OUT_A,SPEED_L);
#define turn_SL OnRev(OUT_C,SPEED_L); OnFwd(OUT_A,SPEED_L);
#define turn_SR OnRev(OUT_A,SPEED_L); OnFwd(OUT_C,SPEED_L);
#define catch OnFwd(OUT_B,33);Wait(600);Off(OUT_B);
#define nMAX 200
#define CENTER 45
#define short_break Off(OUT_AC);Wait(400);
#define cross OnFwd(OUT_C,SPEED_L);OnFwd(OUT_A,SPEED_L);Wait(CROSS_TIME+200);short_break;
#define CROSS_TIME 400
#define white1 39
#define white2 45
#define black1 55
#define black2 63
#define turn_left1 OnFwd(OUT_C,40);Off(OUT_A)
#define turn_left2 OnFwd(OUT_C,40);OnRev(OUT_A,30)
#define turn_right1 OnFwd(OUT_A,40);Off(OUT_C)
#define turn_right2 OnFwd(OUT_A,40);OnRev(OUT_C,20)
#define CCC 14000
#define cross_line OnFwd(OUT_A,SPEED_L);OnFwd(OUT_C,SPEED_L);Wait(CROSS_TIME);short_break;
#define go_R OnFwd(OUT_A,SPEED_H);OnFwd(OUT_C,SPEED_L+10);
#define mMAX 5
sub trace_L()
{
int n;
while(true) {
while(n < nMAX) {
if(SENSOR_1 < CENTER-12) {turn_SR;n++;}
else {
if(SENSOR_1 < CENTER-6) {turn_R;}
else if(SENSOR_1 < CENTER+6) {go_forward;}
else if(SENSOR_1 < CENTER+12) {turn_L;}
else {turn_SL;}
n=0;
}
Wait(STEP);
}
short_break;
turn_SL;Wait(nMAX*STEP);
cross;
n=0;
break;
}
}
sub trace_L_n()
{
if (SENSOR_1<BLACK_01) {turn_left;}
else {
if (SENSOR_1<BLACK_02) {left;}
else if (SENSOR_1<WHITE_01) {go_forward;}
else if (SENSOR_1<WHITE_02) {right;}
else {turn_right;}
}
Wait(STEP);
}
sub right_n()
{
if (SENSOR_1<BLACK_01) {turn_left;}
else if (SENSOR_1<BLACK_02) {left;}
else if (SENSOR_1<WHITE_01) {go_forward;}
else if (SENSOR_1<WHITE_02) {right;}
else {turn_right;}Wait(STEP);
}
sub trace_L_m()
{
if (SENSOR_1 < white1) {turn_left2;}
else if (SENSOR_1 < white2) {turn_left1;}
else if (SENSOR_1 < black1) {go_forward;}
else if (SENSOR_1 < black2) {turn_right1;}
else {turn_right2;}
Wait(STEP);
}
sub trace_L_a()
{
int n;
if(SENSOR_1 < BLACK_01){turn_left;n++;}
else {
if(SENSOR_1 < BLACK_02) {left;n++;}
else if(SENSOR_1 < WHITE_01) {go_forward;}
else if(SENSOR_1 < WHITE_02) {right;}
else {turn_right;}
n=0;
}
Wait(STEP);
}
sub trace_R_n()
{
int n;
if (SENSOR_1<BLACK_01) {turn_right;n++;}
else {
if (SENSOR_1<BLACK_02) {right;n++;}
else if (SENSOR_1<WHITE_01) {go_forward;}
else if (SENSOR_1<WHITE_02) {left;}
else {turn_left;}
n=0;
}
Wait(STEP);
}
task main()
{
SetSensorLight(S1);
long timess = 0;
int nOnline=0;
int n=0;
Wait(40000);
long t3 = CurrentTick();
while(CurrentTick()-t3 < 2000) {trace_R_n();}
Off(OUT_ABC);
Wait(1000);
RotateMotor(OUT_C,70,430);
Off(OUT_AC);
Wait(10000);
OnFwd(OUT_B,33);
Wait(600);
Off(OUT_B);
Wait(2000);
RotateMotor(OUT_A,-70,430);Off(OUT_ABC);Wait(500);
go_R;Wait(800);
Off(OUT_ABC);Wait(500);
PlaySound(SOUND_CLICK);
long t2 = CurrentTick();
while(CurrentTick()-t2 <= 1700){right_n();}
PlaySound(SOUND_CLICK);
Off(OUT_ABC);
Wait(1000);
RotateMotor(OUT_A,70,430);Off(OUT_ABC);Wait(1000);
shoot;
PlaySound(SOUND_CLICK);
Off(OUT_ABC);
long t1 = CurrentTick();
RotateMotor(OUT_A,-70,430);Off(OUT_ABC);Wait(500);
while (CurrentTick()-t1 <= 4800) {trace_L_n();}
PlaySound(SOUND_CLICK);
Off(OUT_ABC);Wait(1000);
RotateMotor(OUT_A,70,430);Off(OUT_ABC);Wait(10000);
OnFwd(OUT_B,33);Wait(700);Off(OUT_ABC);Wait(1000);
RotateMotor(OUT_A,-70,430);
PlaySound(SOUND_CLICK);
Off(OUT_ABC);
Wait(1000);
long t0 = CurrentTick();
while (CurrentTick()-t0 <= MOVE_TIME) {trace_L_n();}
PlaySound(SOUND_CLICK);
Off(OUT_ABC);
Wait(1000);
shoot;
Off(OUT_ABC);
Wait(1000);
PlaySound(SOUND_CLICK);
trace_L();
Off(OUT_ABC);
Wait(1000);
go_b;
Wait(950);
turn_SL;
Wait(600);
PlaySound(SOUND_CLICK);
go_F;
Wait(1600);
catch;
Off(OUT_ABC);
Wait(1500);
turn_left;
Wait(1200);
PlaySound(SOUND_CLICK);
Off(OUT_ABC);
Wait(1000);
trace_L();
Off(OUT_ABC);
}
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
