- 追加された行はこの色です。
- 削除された行はこの色です。
#define set_power_L SetPower(OUT_A,1);SetPower(OUT_C,1);
#define THRESHOLD 45
#define go_forward OnFwd(OUT_AC);
#define turn_right0 OnFwd(OUT_A);OnRev(OUT_C);
#define turn_right1 OnFwd(OUT_A);Off(OUT_C);
#define turn_left1 Off(OUT_A);OnFwd(OUT_C);
#define turn_left0 OnRev(OUT_A);OnFwd(OUT_C);
#define STEP 1
#define U_turn OnFwd(OUT_A);OnRev(OUT_C);Wait(180);Off(OUT_AC);
/*コップを持ち上げる、下ろす動きをマクロ化*/
#define cup_up OnFwd(OUT_B);Wait(130);Off(OUT_B);
#define cup_down OnRev(OUT_B);Wait(130);Off(OUT_B);
/*グローバル変数。コップの色に応じて1〜3の値をとり、点Aからのパターン分岐を決める。*/
int number=0;
/*論理積を使ったライントレースのサブルーチン*/
sub line(){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{go_forward;
Wait(STEP);
}
}
/*紙コップを掴んだ後に「点A」に辿り着いてから行う操作のサブルーチン*/
sub carry_cup(){
if(number==1){
while((SENSOR_2<47)&&(SENSOR_2>35)){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
go_forward;
Wait(10);//紙コップに完全に接触するまで前進。そこからの位置調整も要調査
Off(OUT_AC);
cup_down;
U_turn;
while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップBかCを認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
}else if(number==2){
while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){//交差点を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
turn_right0;
Wait(75);//コップ2の逆方向を向かせる
go_forward;
Wait(10);
U_turn;//交差点でそのまま左折するとコップ2を押してしまう為
while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップ2を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
go_forward;
Wait(20);//紙コップに完全に接触するまで前進。そこからの位置調整も要調査
Off(OUT_AC);
cup_down;
OnRev(OUT_AC);
Wait(130);
while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){//交差点を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
turn_left0;
Wait(75);//軸にそって紙コップBかCに向かせる
while((SENSOR_2>35)&&(SENSOR_2<47)){//紙コップを認識するまで
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
}else{
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
go_forward;
Wait(10);
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる
go_forward;
}
turn_right0;
Wait(75);
while((SENSOR_2>35)&&(SENSOR_2<47)){//紙コップを認識するまで
go_forward;
}
go_forward;
Wait(25);
Off(OUT_AC);
cup_down;
OnRev(OUT_AC);
Wait(110);
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる
OnRev(OUT_AC);
}
turn_right0;
Wait(75);
}
while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップを認識するまで
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
}
/*開始直後の紙コップAを掴み振り向くまでの動きをインライン関数化*/
void start_dash(){
go_forward;
Wait(120);//スタート地点からでる為の動き
while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進
go_forward;
}
turn_right0;
Wait(75);
while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップを認識するまで
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
go_forward;
Wait(100);//コップに接触するまで
Off(OUT_AC);
if(SENSOR_2>47){//白
number=1;
}else if((SENSOR_2<47)&&(SENSOR_2>37)){
number=2;
}else{
number=3;
}//センサーの値に応じてnumberに値を入れる
cup_up;
U_turn;
}
task main(){
SetSensor(SENSOR_1,SENSOR_LIGHT);
SetSensor(SENSOR_2,SENSOR_LIGHT);
SetSensor(SENSOR_3,SENSOR_LIGHT);
set_power_L;
int cross=0;//交差点通過回数のカウンタ
start_dash();//コップAを掴む
carry_cup();
go_forward;
Wait(50);//コップBに接触するまで
Off(OUT_AC);
if(SENSOR_2>47){
number=1;
}else if((SENSOR_2>35)&&(SENSOR_2<47)){
number=2;
}else{
number=3;
}//センサーの値に応じてnumberに値を入れる
cup_up;
U_turn;
ClearTimer(0);//タイマー設置
while(Timer(0)<40){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
carry_cup();
go_forward;
Wait(50);//コップCに接触するまで
Off(OUT_AC);
if(SENSOR_2>47){
number=1;
}else if((SENSOR_2>35)&&(SENSOR_2<47)){
number=2;
}else{
number=3;
}//センサーの値に応じてnumberに値を入れる
cup_up;
U_turn;
ClearTimer(0);
while(Timer(0)<20){
if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){
go_forward;
Wait(STEP);
}else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){
turn_right1;
Wait(STEP);
}else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){
turn_left1;
Wait(STEP);
}else{
go_forward;
Wait(STEP);
}
}
carry_cup();
Off(OUT_AC);
}