text: #pragma config(Sensor, S1,     soundSensor,    sensorSoundDBA)
  #pragma config(Sensor, S2,     lightSensor,    sensorLightInactive)
  #pragma config(Sensor, S3,     sonarSensor,    sensorSONAR)
  #pragma config(Sensor, S4,     touchSensor,    sensorTouch)
  #pragma config(Motor,  motorA,          rightMotor,    tmotorNXT, PIDControl, encoder)
  #pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
  //*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
  
  #define NUM_FREQ                22
  #define NUM_NOTES               84
  #define DURATION_SCALE          8
  
  int g_FreqTable[NUM_FREQ] =
  {
      0,          //00: Rest
      262,        //01: Do
      294,        //02: Re
      330,        //03: Me
      349,        //04: Fa
      392,        //05: So
      440,        //06: La
      494,        //07: Ti
      0,          //08: Rest
      0,          //09: Rest
      0,          //10: Rest
      523,        //11: Do
      587,        //12: Re
      659,        //13: Me
      698,        //14: Fa
      784,        //15: So
      880,        //16: La
      988,        //17: Ti
      0,          //18: Rest
      0,          //19: Rest
      0,          //20: Rest
      1047        //21: Do
  };
  
  int g_Notes[NUM_NOTES] =
  {
      5, 5, 5,
      11, 15,
      14, 13, 12, 21, 15,
      14, 13, 12, 21, 15,
      14, 13, 14, 12, 5, 5,
      11, 15,
      14, 13, 12, 21, 15,
      14, 13, 12, 21, 15,
      14, 13, 14, 12, 5, 5,
      6, 6, 14, 13, 12, 11,
      11, 12, 13, 12, 7, 5, 5,
      6, 6, 14, 13, 12, 11,
      15, 12, 5, 5,
      6, 6, 14, 13, 12, 11,
      11, 12, 13, 12, 7, 15, 15,
      21, 932, 831, 15, 14, 622, 12, 11,
      15
  };
  
  int g_Durations[NUM_NOTES] =
  {
      2, 2, 2,
      12, 12,
      2, 2, 2, 12, 6,
      2, 2, 2, 12, 6,
      2, 2, 2, 12, 3, 3,
      12, 12,
      2, 2, 2, 12, 6,
      2, 2, 2, 12, 6,
      2, 2, 2, 12, 3, 3,
      9, 3, 3, 3, 3, 3,
      2, 2, 2, 6, 6, 3, 3,
      9, 3, 3, 3, 3, 3,
      6, 12, 3, 3,
      9, 3, 3, 3, 3, 3,
      2, 2, 2, 6, 6, 3, 3,
      3, 3, 3, 3, 3, 3, 3, 3,
      18
  };
  
  int g_NoteIndex = 0;
  
  void SoundTask()
  {
      if (!bSoundActive)
      {
          int freq = (g_Notes[g_NoteIndex] < NUM_FREQ)?
                      g_FreqTable[g_Notes[g_NoteIndex]]:
                      g_Notes[g_NoteIndex];
          nxtDisplayTextLine(0, "[%02d]%4d,%d",
                             g_NoteIndex, freq, g_Durations[g_NoteIndex]);
          PlayTone(freq, g_Durations[g_NoteIndex]*DURATION_SCALE);
          g_NoteIndex++;
          if (g_NoteIndex >= NUM_NOTES)
          {
              //
              // We reached the end, loop back.
              //
              g_NoteIndex = 0;
          }
      }
  }   //SoundTask
  
  #define CLICKS_PER_INCH         53.6
  #define CLICKS_PER_DEGREE       3.42
  #define DRIVE_KP                18.0
  #define TURN_KP                 1.0
  #define DRIVE_TOLERANCE         0.5
  #define TURN_TOLERANCE          0.5
  
  
  #define OP_DRIVE                0
  #define OP_TURN                 1
  #define NUM_OP                  8
  
  typedef struct
  {
      float   targetValue;
      bool    enabled;
  } TARGET;
  
  int g_DriveOps[NUM_OP] =
  {
      OP_DRIVE,
      OP_TURN,
      OP_DRIVE,
      OP_TURN,
      OP_DRIVE,
      OP_TURN,
      OP_DRIVE,
      OP_TURN
  };
  
  float g_DriveValues[NUM_OP] =
  {
      24.0,
      90.0,
      12.0,
      90.0,
      24.0,
      90.0,
      12.0,
      90.0
  };
  
  TARGET  g_DriveTarget;
  TARGET  g_TurnTarget;
  bool    g_Driving = false;
  
  void InitTarget(TARGET &target)
  {
      target.targetValue = 0.0;
      target.enabled = false;
  }   //InitTarget
  
  void SetTarget(TARGET &target, float targetValue)
  {
      nxtDisplayTextLine(3, "Target:%6.1f", targetValue);
      target.targetValue = targetValue;
      nMotorEncoder[leftMotor] = 0;
      nMotorEncoder[rightMotor] = 0;
      target.enabled = true;
      g_Driving = true;
  }   //SetTarget
  
  void DriveTask(TARGET &target)
  {
      if (target.enabled)
      {
          float leftPos = nMotorEncoder[leftMotor]/CLICKS_PER_INCH;
          float rightPos = nMotorEncoder[rightMotor]/CLICKS_PER_INCH;
          nxtDisplayTextLine(4, "LPos=%6.1f", leftPos);
          nxtDisplayTextLine(5, "RPos=%6.1f", rightPos);
          if (abs(target.targetValue - (leftPos + rightPos)/2.0) <=
              DRIVE_TOLERANCE)
          {
              motor[leftMotor] = 0;
              motor[rightMotor] = 0;
              target.enabled = false;
              g_Driving = false;
          }
          else
          {
              motor[leftMotor] = DRIVE_KP*(target.targetValue - leftPos);
              motor[rightMotor] = DRIVE_KP*(target.targetValue - rightPos);
          }
      }
  }   //DriveTask
  
  void TurnTask(TARGET &target)
  {
      if (target.enabled)
      {
          float currAngle = (nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor])/
                            CLICKS_PER_DEGREE;
          nxtDisplayTextLine(7, "currAngle=%6.1f", currAngle);
          if (abs(target.targetValue - currAngle) <= TURN_TOLERANCE)
          {
              motor[leftMotor] = 0;
              motor[rightMotor] = 0;
              target.enabled = false;
              g_Driving = false;
          }
          else
          {
              int motorPower = TURN_KP*(target.targetValue - currAngle);
              motor[leftMotor] = motorPower;
              motor[rightMotor] = -motorPower;
          }
      }
  }   //TurnTask
  
  task main()
  {
      int driveIndex = 0;
  
      InitTarget(g_DriveTarget);
      InitTarget(g_TurnTarget);
  
      while (true)
      {
          SoundTask();
  
          nxtDisplayTextLine(1, "LEnc=%4d", nMotorEncoder[leftMotor]);
          nxtDisplayTextLine(2, "REnc=%4d", nMotorEncoder[rightMotor]);
  
          if (!g_Driving)
          {
              if (driveIndex < NUM_OP)
              {
                  if (g_DriveOps[driveIndex] == OP_DRIVE)
                  {
                      SetTarget(g_DriveTarget, g_DriveValues[driveIndex]);
                  }
                  else if (g_DriveOps[driveIndex] == OP_TURN)
                  {
                      SetTarget(g_TurnTarget, g_DriveValues[driveIndex]);
                  }
                  driveIndex++;
              }
              else
              {
                  break;
              }
          }
  
          DriveTask(g_DriveTarget);
          TurnTask(g_TurnTarget);
  
          EndTimeSlice();
      }
  }   //main
  