目次 #contents *工夫 [#k1d26e3c] #define THRESHOLD 50 #define go_forward OnFwd(OUT_A,40);OnFwd(OUT_C,40) //直進,Aは一番右のモータ,Cは一番左のモータ #define turn_left1 OnFwd(OUT_A,35);OnFwd(OUT_C,-30) //左旋回 #define turn_right1 OnFwd(OUT_C,35);OnFwd(OUT_A,-30) //右旋回 #define SPEED 50 #define SPEED_SLOW 30 #define SPEED_MSLOW -30 #define Ball_up RotateMotor(OUT_B,-50,160);Wait(2000); #define Ball_up2 RotateMotor(OUT_B,-50,50);Wait(2000); #define Ball_down RotateMotor(OUT_B,30,60);Wait(2000); #define Ball_down2 RotateMotor(OUT_B,30,150);Wait(2000); #define follow_intersection ResetTachoCount(OUT_AC);RotateMotor(OUT_AC,30,100); #define follow_intersection2 ResetTachoCount(OUT_AC);RotateMotor(OUT_AC,30,300); #define Mfollow_intersection ResetTachoCount(OUT_AC);RotateMotor(OUT_AC,-30,210); #define CONN 1 // スレーブの接続番号 int msg;// 子機を動かすための合図。 const float diameter = 5.45; // const float track = 10.35; // const float pi = 3.1415; // void Ball_release() { RemoteStartProgram(1,"Last(S2).rxe"); while(msg != 12){ ReceiveRemoteNumber(MAILBOX1,true,msg); } Wait(3000); } void Ball_catch() { RemoteStartProgram(1,"Last(S1).rxe"); while(msg != 11){ ReceiveRemoteNumber(MAILBOX1,true,msg); } Wait(3000); } void fwdDist(float d) { long angle; angle = d/(diameter*pi)*360.0 ; // RotateMotorEx(OUT_AC,SPEED_SLOW,angle,0,true,true); } void MfwdDist(float d) { long angle; angle = d/(diameter*pi)*360.0 ; // RotateMotorEx(OUT_AC,SPEED_MSLOW,angle,0,true,true); } void turnAng(long ang) { long angle; angle = track/diameter*ang; RotateMotorEx(OUT_AC, SPEED_SLOW, angle, 100, true, true); } int searchDirection(long ang) { long angle, tacho_min=0, tacho_corr, t1, t2; int d_min; d_min=300; angle = (track/diameter)*ang; turnAng(ang/2); ResetTachoCount(OUT_AC); OnFwdSync(OUT_AC,SPEED_SLOW,-100); while(MotorTachoCount(OUT_A)<=angle){ if(SensorUS(S2)<d_min){ d_min=SensorUS(S2); tacho_min=MotorTachoCount(OUT_A); } } OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE); t1=CurrentTick(); until(MotorTachoCount(OUT_A)<=tacho_min||SensorUS(S2)<=d_min); t2=CurrentTick(); Wait(14); Off(OUT_AC);Wait(500); return d_min; ResetTachoCount(OUT_AC); } void e_fwdDist(float e) { long angle2; angle2 = e/(diameter*pi)*360.0 ; // RotateMotorEx(OUT_AC,SPEED_SLOW,angle2,0,true,true); } void e_MfwdDist(float e) { long angle2; angle2 = e/(diameter*pi)*360.0 ; // RotateMotorEx(OUT_AC,SPEED_MSLOW,angle2,0,true,true); } void turnAng2(long ang2) { long angle2; angle2 = track/diameter*ang2; RotateMotorEx(OUT_AC, SPEED_SLOW, angle2, 100, true, true); } int e_searchDirection(long ang2) { long angle2, tacho_min2=0, tacho_corr2; int e_min; e_min=300; angle2 = (track/diameter)*ang2; turnAng2(ang2/2); ResetTachoCount(OUT_AC); OnFwdSync(OUT_AC,SPEED_SLOW,-100); while(MotorTachoCount(OUT_A)<=angle2){ if(SensorUS(S2)<e_min){ e_min=SensorUS(S2); tacho_min2=MotorTachoCount(OUT_A); } } OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_A)<=tacho_min2||SensorUS(S2)<=e_min); Wait(14); Off(OUT_AC);Wait(500); return e_min; ResetTachoCount(OUT_AC); } void Lfollow_line(long min_t) { long t0=CurrentTick(); while(CurrentTick()-t0<=min_t){ if ((SENSOR_1<THRESHOLD-7)&&(SENSOR_4<THRESHOLD-11)){ Off(OUT_AC);Wait(1000); } else if (SENSOR_1<THRESHOLD-11){ turn_left1; } else if (SENSOR_1<THRESHOLD-7){ turn_left1; } else if (SENSOR_1<THRESHOLD+7){ go_forward; } else if (SENSOR_1<THRESHOLD+11){ turn_right1; } else { turn_right1; } } } void follow_line(long min_t) { long t0=CurrentTick(); while(CurrentTick()-t0<=min_t){ if (SENSOR_1<THRESHOLD-11){ turn_left1; } else if (SENSOR_1<THRESHOLD-7){ turn_left1; } else if (SENSOR_1<THRESHOLD+7){ go_forward; } else if (SENSOR_1<THRESHOLD+11){ turn_right1; } else { turn_right1; } } } *全体のプログラム [#h97cd118] task main() { long t1, t2; SetSensorLight(S1); //右のセンサ SetSensorLowspeed(S2); //超音波センサ SetSensorLight(S4); //左のセンサ Wait(2000); //スタートボタンを押してから、離れるまでの時間。 Ball_catch(); //子機にボールを掴ませる。 follow_intersection; //交差点を横断する。 Lfollow_line(3000); //次の交差点まで進む。 follow_intersection; //交差点を横断する。 Ball_up; //アームをあげる。 Lfollow_line(18000); //次の交差点まで進む。 int d=searchDirection(120);//缶を探す。 fwdDist(d-18.0); //缶に近づく。 Ball_down; //アームを下げる。 Ball_release(); //子機で缶にボールを乗せる。 MfwdDist(d-17.0); //缶から離れる。 Ball_up2; follow_line(2000); //道に戻る Lfollow_line(3000); //次の交差点まで進む。 Mfollow_intersection; Ball_down2; //アームを下げる。 Ball_catch(); //子機にボールを掴ませる。 Ball_up; //アームをあげる。 follow_intersection2; int e=e_searchDirection(120); //缶を探す。 e_fwdDist(e-18.0); //缶に近づく。 e_searchDirection(20); //缶を探す。 Ball_down; //アームを下げる。 Ball_release(); //子機で缶にボールを乗せる。 e_MfwdDist(e-18.0); //缶に近づく。 }