2018b/Member/Tom/Mission2 の履歴ソース(No.1)

• 履歴一覧
• 差分 を表示
• 現在との差分 を表示
• 2018b/Member/Tom/Mission2 へ行く。
  • 1 (2018-12-24 (月) 13:15:02)
  • 2 (2018-12-25 (火) 15:12:18)
  • 3 (2019-01-09 (水) 13:31:35)

[[2018b/Member]]

#define THRESHOLD 45
#define go_forward OnFwd(OUT_B,40);OnFwd(OUT_C,40)   //直進
#define go_back OnFwd(OUT_B,-40);OnFwd(OUT_C,-40)    //後退
#define turn_left1 OnFwd(OUT_B,35);OnFwd(OUT_C,-30)  //左旋回
#define turn_left2 OnFwd(OUT_B,25);OnFwd(OUT_C,-20)  //左折
#define turn_right1 OnFwd(OUT_C,35);OnFwd(OUT_B,-30) //右旋回
#define turn_right2 OnFwd(OUT_C,25);OnFwd(OUT_B,-20) //右折
#define across OnFwd(OUT_B,40);OnFwd(OUT_C,40);Wait(1000)//交差点を横断する
#define across2 OnFwd(OUT_B,40);OnFwd(OUT_C,40);Wait(400)//交差点を横断する
#define MI 659 //ミの音
#define SPEED 50
#define SPEED_SLOW 30
#define throw Wait(2000);OnFwd(OUT_A,30);Wait(700);Off(OUT_A);Wait(2000);OnFwd(OUT_A,-35);Wait(700);Off(OUT_A);

const float diameter = 5.45; //
const float track = 10.35;   //
const float pi = 3.1415;     //

void fwdDist(float d)
{
    long angle;
    angle = d/(diameter*pi)*360.0 ;   //
    RotateMotorEx(OUT_BC,SPEED_SLOW,angle,0,true,true);
}

void turnAng(long ang)

{
    long angle;
    angle = track/diameter*ang;
    RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 100, true, true);   
}
 
int searchDirection(long ang)

{
    long angle, tacho_min=0, tacho_corr;
    int d_min;

    d_min=300;

    angle = (track/diameter)*ang;
    turnAng(ang/2);
    ResetTachoCount(OUT_BC);

    OnFwdSync(OUT_BC,SPEED_SLOW,-100);
    while(MotorTachoCount(OUT_B)<=angle){
    if(SensorUS(S2)<d_min){
    d_min=SensorUS(S2);
    tacho_min=MotorTachoCount(OUT_B);
    }
}
    OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
    until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S2)<=d_min);

    Wait(14);
    Off(OUT_BC);Wait(500);
    return d_min;
}


void Rfollow_line(long max_t)
{   
    long t0=CurrentTick();
    while (CurrentTick()-t0<=max_t){
        if (SENSOR_4<THRESHOLD-11) {
            turn_left1;
        }
    else if (SENSOR_4<THRESHOLD-7){
            turn_left2;
        } 
    else if (SENSOR_4<THRESHOLD+7){  
            go_forward;
        } 
    else if (SENSOR_4<THRESHOLD+11){
            turn_right1;
        } 
    else {
            turn_right2;
         }
}
}

void Lfollow_line(long max_t)
{   
    long t0=CurrentTick();
    while (CurrentTick()-t0<=max_t){
        if (SENSOR_4<THRESHOLD-11) {
            turn_right1;
        }
    else if (SENSOR_4<THRESHOLD-7){
            turn_right2;
        } 
    else if (SENSOR_4<THRESHOLD+7){  
            go_forward;
        } 
    else if (SENSOR_4<THRESHOLD+11){
            turn_left1;
        } 
    else {
            turn_left2;
         }
}
}

void follow_intersection(long min_t)
{    
    long t1=CurrentTick();
    while (CurrentTick()-t1<=min_t){
        if (SENSOR_4<THRESHOLD-11) {
            Off(OUT_BC);Wait(1000);
        }
}
         if (SENSOR_4<THRESHOLD-7){
            turn_left2;
            t1=CurrentTick();
        }
    else if (SENSOR_4<THRESHOLD+7){ 
            go_forward;
            t1=CurrentTick();
        }
    else if (SENSOR_4<THRESHOLD+11){
            turn_right1;
            t1=CurrentTick();
        }
    else {
            turn_right2;
            t1=CurrentTick();
         }
    PlayTone(MI,900);Off(OUT_BC);Wait(1000);
}

task main()
{
    SetSensorLight(S4);
    SetSensorSound(S1);
    Wait(3000);
    across;
    Rfollow_line(3500);
    Rfollow_line(400);
    follow_intersection(400);
    SetSensorLowspeed(S2);
    int d=searchDirection(120);
    if(d>30){
    fwdDist(d-30.0);
    }
    throw; 
    go_back;Wait(500);
    turn_left2;Wait(3000);
    Lfollow_line(4000);
    follow_intersection(400);
    turn_right2;Wait(1000);
    Rfollow_line(19500);
    follow_intersection(400);
    turn_left2;Wait(1000);
    Lfollow_line(1500);
    across;
}