로봇C 미로찾기

#pragma config(Sensor, S1,     sonar,               sensorSONAR)
#pragma config(Sensor, S2,     touch,               sensorTouch)
#pragma config(Sensor, S3,     light,               sensorLightActive)
#pragma config(Sensor, S4,     sound,               sensorSoundDBA)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define THRESHOLD 6
#define AXIS_LENGTH 120.0
#define WHEEL_DIAMETER 56.0
#define Light_HOLD_M 55
float convert(float turn_angle){
 return (2* AXIS_LENGTH / WHEEL_DIAMETER * turn_angle);
}
task main{
 int s_value;
 float turn_angle = 90.0;
 int rotation = (int)convert(turn_angle);
 
 //ready
 nxtDisplayTextLine(0, "Ready");
 wait1Msec(1000);
 nxtDisplayTextLine(0, "Start");
 PlaySound(soundFastUpwardTones);
 wait1Msec(1000);
 eraseDisplay();

 while(1){
    s_value = SensorValue(sonar);
    nxtDisplayTextLine(0, "SonarValue = %d",s_value);
     motor[motorA] = 30;
     motor[motorC] = 30;

   //normal_drive
      if((s_value > 7) && (s_value <13))
      {     motor[motorA] = 35;
            motor[motorC] = 35;
            wait1Msec(100);
            nxtDisplayTextLine(1, "normal drive:");
      }
   //in_cource problem
      if( (s_value >= 0) && (s_value <= 7)){
             motor[motorA] = 25;
             motor[motorC] = 30;
             wait1Msec(100);
             nxtDisplayTextLine(2, "check_left:%d");
      }
   //out_cource problem
      if( (s_value >14) && (s_value <=255)){
             motor[motorA] = 35;
             motor[motorC] = 15;
             wait1Msec(100);
             nxtDisplayTextLine(2, "check_right");
      }
   // touch value check_back_in cource
      if(SensorValue(touch) == 1){
           
            nxtDisplayTextLine(2, "check_touch retire ");
            nxtDisplayTextLine(4, "touch : %d",SensorValue(touch));     
           
            motor[motorA] = -10;
            motor[motorC] = -10;
            wait1Msec(500);
           
            // 90' turn
            nMotorEncoderTarget[motorA] = rotation;
            motor[motorA] = -50;
            motor[motorC] = 0;
            wait1Msec(850);
      }
    // point_check   light& sound
      if(SensorValue(light) <= Light_HOLD_M)
      {
        PlaySound(soundBeepBeep);
        wait1Msec(100);
        nxtDisplayTextLine(2, "check_point");
      }
   }//while
}//task main