mutex moveMutex;

#define POW_R       45
#define POW_L       60
#define MOVE_TIME1  1
#define MOVE_TIME   3000
#define TURN45_TIME 500
#define TH_LIGHT    40
#define TH_DIST     30
#define POW         50

void forward(int time){      // 前進
    OnFwd(OUT_AC, POW);
    Wait(time);
}


void turn_right(int time){   // 右回転
    Off(OUT_A);
    OnFwd(OUT_C, POW);
    Wait(time);
}


void turn_left(int time){    // 左回転
    OnFwd(OUT_A, POW);
    Off(OUT_C);
    Wait(time);
}

void curve(int time){    // カーブして回避
    OnFwd(OUT_A, POW_R);
    OnFwd(OUT_C, POW_L);
    Wait(time);
}


task line_trace()
{
   while(true) {
      if (Sensor(IN_2) > TH_LIGHT) {      //IN_2が40より大きい(白)
         if (Sensor(IN_3) > TH_LIGHT) {   //IN_3が40より大きい(白)
            Acquire(moveMutex);
            forward(MOVE_TIME1);      //前進
            Release(moveMutex);
         }else{
            Acquire(moveMutex);
            turn_left(MOVE_TIME1);   //IN_3が40以下(黒)左回転
            Release(moveMutex);
         }
      }else{
         Acquire(moveMutex);
         turn_right(MOVE_TIME1);     //IN_2が40以下(黒)右回転
         Release(moveMutex);
      }
   }
}

task collision_avoidance(){

  while(true) {

    until(SensorUS(IN_4) < TH_DIST); // 障害物との距離が30cmよりちいさいならば実行
      Acquire(moveMutex);
      turn_left(TURN45_TIME);
      curve(MOVE_TIME);
      Release(moveMutex);
  }
}

task rotate_arm(){

  while(true) {
    if(Sensor(IN_1) == 1){            //センサ1が押されたら実行

      Acquire(moveMutex);
      RotateMotor(OUT_B, POW, 360);
      Release(moveMutex);

    }
  }
}

task display(){

   int dist, data_2, data_3;
   while(true) {
      data_2 = Sensor(IN_2);
      data_3 = Sensor(IN_3);
      dist = SensorUS(IN_4);
      NumOut(0,56,data_2);       // ライトセンサ読み取り値を画面に表示
      NumOut(0,40,data_3);       // ライトセンサ読み取り値を画面に表示
      NumOut(0,24,dist);         // 超音波センサ読み取り値を画面に表示
   }
}

task main()
{
   SetSensorTouch(IN_1);       //入力ポート1をタッチセンサに設定
   SetSensorLight(IN_2);       //入力ポート2をライトセンサを設定
   SetSensorLight(IN_3);       //入力ポート3をライトセンサを設定
   SetSensorLowspeed(IN_4);    //入力ポート4を超音波センサに設定

   Wait(500);
   OnFwd(OUT_AC, POW);
   Wait(500);

   Precedes(line_trace, collision_avoidance, rotate_arm, display);

}