ボール運搬ロボット ピンポン玉または青と赤のボールを運搬して,所定の容器の中に入れる。
センサーライトを2つを並べて使用した。
ボールを拾うアームと容器を持ち上げるアームを取り付けた。
容器を持ち上げて容器から出てきたボールを滑らして容器に入れる。
#define BLACKS 38 #define BLACK 45 #define THRESHOLD 53 #define WHITE 60 #define WHITES 68
#define left_s OnFwd(OUT_A,60);OnRev(OUT_B,60); #define left OnFwd(OUT_A,60);Off(OUT_B); #define right_s OnFwd(OUT_B,60);OnRev(OUT_A,90); #define right OnFwd(OUT_B,60);Off(OUT_A); #define forward OnFwd(OUT_AB,60); #define back OnRev(OUT_AB,60);
#define CONN 1 #define SIGNALON1 10 #define SIGNALOFF1 11 #define SIGNALON2 20 #define SIGNALOFF2 21 #define SIGNALON3 30 #define SIGNALOFF3 31 #define HALF 1900 #define QUARTER 220
sub follow_line_left() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if((SENSOR_1<BLACKS)&&(SENSOR_2<BLACKS)){ t0=CurrentTick(); forward; until((SENSOR_1<THRESHOLD)&&(SENSOR_2<THRESHOLD)); t1=CurrentTick(); }else if(SENSOR_1<BLACKS){ left_s; }else if(SENSOR_1<BLACK){ left; }else if(SENSOR_1<THRESHOLD){ forward; }else if(SENSOR_1<WHITE){ right; }else if(SENSOR_1<WHITES){ right_s; } } Off(OUT_AB); Wait(1000); }
sub follow_line_right() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if((SENSOR_1<BLACKS)&&(SENSOR_2<BLACKS)){ t0=CurrentTick(); forward; until((SENSOR_1<THRESHOLD)&&(SENSOR_2<THRESHOLD)); t1=CurrentTick(); }else if(SENSOR_2<BLACKS){ right_s; }else if(SENSOR_2<BLACK){ right; }else if(SENSOR_2<THRESHOLD){ forward; }else if(SENSOR_2<WHITE){ left; }else if(SENSOR_2<WHITES){ left_s; } } Off(OUT_AB); Wait(1000); }
sub follow_line_back() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if((SENSOR_1<BLACKS)&&(SENSOR_2<BLACKS)){ t0=CurrentTick(); back; until((SENSOR_1<THRESHOLD)&&(SENSOR_2<THRESHOLD)); t1=CurrentTick(); }else if(SENSOR_2<BLACKS){ right_s; }else if(SENSOR_2<BLACK){ right; }else if(SENSOR_2<THRESHOLD){ back; }else if(SENSOR_2<WHITE){ left; }else if(SENSOR_2<WHITES){ left_s; } } Off(OUT_AB); Wait(1000); } sub start_right() { right; Wait(100); }
sub start_left() { left; Wait(100); }
sub across() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if((SENSOR_1<BLACKS)&&(SENSOR_2<BLACKS)){ t0=CurrentTick(); forward; until((SENSOR_1<THRESHOLD)&&(SENSOR_2<THRESHOLD)); t1=CurrentTick(); }else if(SENSOR_1<WHITES){ forward; } } Off(OUT_AB); Wait(1000); }
sub send_msg1() { int msg; while(msg!=SIGNALOFF1){ SendRemoteNumber(CONN,MAILBOX1,SIGNALON1); ReceiveRemoteNumber(MAILBOX1,true,msg); }
}
vsub send_msg2() { int msg; while(msg!=SIGNALOFF2){ SendRemoteNumber(CONN,MAILBOX2,SIGNALON2); ReceiveRemoteNumber(MAILBOX2,true,msg); } } sub send_msg3() { int msg; while(msg=""){ SendRemoteNumber(CONN,MAILBOX3,SIGNALON3); ReceiveRemoteNumber(MAILBOX3,true,msg); } msg=""; }
task main() { follow_line_left(); right_s; Wait(200); forward; Wait(900); Off(OUT_AB); send_msg1(); right_s; Wait(HALF); follow_line_right(); send_msg2(); }
#define CONN 1 #define SIGNALON1 10 #define SIGNALOFF1 11 #define SIGNALON2 20 #define SIGNALOFF2 21 #define SIGNALON3 30 #define SIGNALOFF3 31
sub ball_catch1() { OnFwd(OUT_A,40); Wait(200); OnRev(OUT_B,50); Wait(500); OnRev(OUT_A,60); Wait(700); Off(OUT_AB); }
sub ball_throw1() { OnFwd(OUT_A,40); Wait(100); OnFwd(OUT_B,50); Wait(500); OnRev(OUT_A,60); Wait(700); Off(OUT_AB); }
sub ball_catch2() { OnRev(OUT_C,50); Wait(150); Off(OUT_C); }
sub receive_msg1() { int msg; while(msg!=SIGNALON1){ ReceiveRemoteNumber(MAILBOX1,true,msg); //msgが空の間,MAILBOX1にmsgを受信し続け る } ball_catch1(); PlaySound(SOUND_UP); SendResponseNumber(MAILBOX1,SIGNALOFF1);//親機MAILBOX1にSIGNALOFF1を送る
}
sub receive_msg2() { int msg; while(msg!=SIGNALON2){ ReceiveRemoteNumber(MAILBOX2,true,msg); //msgが空の間,MAILBOX1にmsgを受信し続け る } ball_throw1(); PlaySound(SOUND_UP); SendResponseNumber(MAILBOX2,SIGNALOFF2);//親機MAILBOX1にSIGNALOFF1を送る }
sub receive_msg3() { int msg; while(msg=""){ ReceiveRemoteNumber(MAILBOX3,true,msg); } ball_catch2(); SendResponseNumber(MAILBOX3,SIGNALOFF3); msg=""; }
task main() { receive_msg1(); receive_msg2(); }
#define BLACKS 45 #define BLACK 53 #define THRESHOLD 61 #define WHITE 69 #define WHITES 75 #define left_s OnFwd(OUT_A,50);OnRev(OUT_B,50); #define left OnFwd(OUT_A,50);Off(OUT_B); #define right_s OnFwd(OUT_B,50);OnRev(OUT_A,50); #define right OnFwd(OUT_B,50);Off(OUT_A); #define forward OnFwd(OUT_AB,50); #define CONN 1 #define SIGNALON1 10 #define SIGNALOFF1 11 #define SIGNALON2 20 #define SIGNALOFF2 21 #define SIGNALON3 30 #define SIGNALOFF3 31 #define HALF 450 #define QUARTER 220 sub follow_line_left() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if((SENSOR_1<BLACKS)&&(SENSOR_2<BLACKS)){ t0=CurrentTick(); forward; until((SENSOR_1<THRESHOLD)&&(SENSOR_2<THRESHOLD)); t1=CurrentTick(); }else if(SENSOR_1<BLACKS){ left_s; }else if(SENSOR_1<BLACK){ left; }else if(SENSOR_1<THRESHOLD){ forward; }else if(SENSOR_1<WHITE){ right; }else if(SENSOR_1<WHITES){ right_s; } } Off(OUT_AB); Wait(1000); }
sub follow_line_right() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if((SENSOR_1<BLACKS)&&(SENSOR_2<BLACKS)){ t0=CurrentTick(); forward; until((SENSOR_1<THRESHOLD)&&(SENSOR_2<THRESHOLD)); t1=CurrentTick(); }else if(SENSOR_2<BLACKS){ right_s; }else if(SENSOR_2<BLACK){ right; }else if(SENSOR_2<THRESHOLD){ forward; }else if(SENSOR_2<WHITE){ left; }else if(SENSOR_2<WHITES){ left_s; } } Off(OUT_AB); Wait(1000); }
sub follow_line_leftT() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if(SENSOR_2<BLACKS){ t0=CurrentTick(); forward; until(SENSOR_2<THRESHOLD); t1=CurrentTick(); }else if(SENSOR_1<BLACKS){ left_s; }else if(SENSOR_1<BLACK){ left; }else if(SENSOR_1<THRESHOLD){ forward; }else if(SENSOR_1<WHITE){ right; }else if(SENSOR_1<WHITES){ right_s; } } Off(OUT_AB); Wait(1000); }
sub follow_line_rightT() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if(SENSOR_1<BLACKS){ t0=CurrentTick(); until(SENSOR_1<THRESHOLD) t1=CurrentTick(); }else if(SENSOR_2<BLACKS){ right_s; }else if(SENSOR_2<BLACK){ right; }else if(SENSOR_2<THRESHOLD){ forward; }else if(SENSOR_2<WHITE){ left; }else if(SENSOR_2<WHITES){ left_s; } } Off(OUT_AB); Wait(1000); }
sub start_right() { right; Wait(100); }
sub start_left() { left; Wait(100); }
subacross() { SetSensorLight(S1); SetSensorLight(S2);
long t0=0,t1=0,stop_time=1;
while(t1-t0<stop_time){ if((SENSOR_1<BLACKS)&&(SENSOR_2<BLACKS)){ t0=CurrentTick(); forward; until((SENSOR_1<THRESHOLD)&&(SENSOR_2<THRESHOLD)); t1=CurrentTick(); }else if(SENSOR_1<WHITES){ forward; } } Off(OUT_AB); Wait(1000); }
sub send_msg1() { int msg; while(msg=""){ SendRemoteNumber(CONN,MAILBOX1,SIGNALON1); ReceiveRemoteNumber(MAILBOX1,true,msg); } msg=""; }
sub send_msg2() { int msg; while(msg=""){ SendRemoteNumber(CONN,MAILBOX2,SIGNALON2); ReceiveRemoteNumber(MAILBOX2,true,msg); } msg=""; }
sub send_msg3() { int msg; while(msg=""){ SendRemoteNumber(CONN,MAILBOX3,SIGNALON3); ReceiveRemoteNumber(MAILBOX3,true,msg); } msg=""; }
task main() { long t2=3000; forward; Wait(500); follow_line_rightT(); forward; Wait(t2); Off(OUT_AB); send_msg1(); //toL' right_s; Wait(HALF); follow_line_leftT(); right_s; Wait(QUARTER); follow_line_right(); send_msg2(); //toJ' left_s; Wait(HALF); follow_line_right(); start_right(); follow_line_right(); //toM' start_right(); follow_line_left(); start_right(); follow_line_rightT(); left_s; Wait(QUARTER); follow_line_rightT(); //toB follow_line_rightT(); left_s; Wait(QUARTER); follow_line_rightT(); forward; Wait(t2); //ktoL send_msg1(); //ball left_s; Wait(HALF); follow_line_leftT(); right_s; Wait(QUARTER); follow_line_left(); //toJ send_msg2(); left_s; Wait(QUARTER); OnFwd(OUT_C,40); Wait(500); send_msg3
今回はライントレースするところから難しかった。容器の玉を入れることを捨てて確実に一点とる作戦でもよっかたのではないかなと思った。