2018b/Member/Jerry/Mission3 の履歴差分(No.1)

• 履歴一覧
• 現在との差分 を表示
• ソース を表示
• 2018b/Member/Jerry/Mission3 へ行く。
  • 1 (2019-02-06 (水) 10:13:15)
  • 2 (2019-02-06 (水) 13:31:28)
  • 3 (2019-02-06 (水) 15:02:02)
  • 4 (2019-02-08 (金) 03:02:57)
  • 5 (2019-02-09 (土) 01:19:10)
  • 6 (2019-02-12 (火) 03:56:31)

• 追加された行はこの色です。
• 削除された行はこの色です。

目次
#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); //缶に近づく。
    
}