//*!!Sensor,    S1,                  cam, sensorI2CCustomFast,      ,            !!*//
//*!!Motor,  motorA,              r_motor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!Motor,  motorC,              l_motor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors cam                  = (tSensors) S1;   //sensorI2CCustomFast //*!!!!*//
const tMotor   r_motor              = (tMotor) motorA; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor   l_motor              = (tMotor) motorC; //tmotorNxtEncoderClosedLoop //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

#include "nxtcamlib.c"

/************************************************************************************/
// blob_chase.c - fun demo of nxtcam using Robot C. Needs nxtcamlib.c.
// Gordon Wyeth
// 30 October 2007
// Updated 4/12/07 to use new version of nxtcamlib.c that works around Robot-C compiler
// bug.
//
// When a single blob is found in the image the robot will try to centre the blob by moving
// the motors. When more the one blob is found the robot halts and displays a message.
// The constants chosen work well with the standard NXT wheelchair robot with the
// camera mounted at the front of the robot looking down at the floor.
//
/************************************************************************************/

task main ()
{
   int nblobs;  // Number of blobs
   int_array bc, bl, bt, br, bb;
   int x_centre, x_error;
   int y_centre, y_error;
   bool erased = false;
   int i, sq, n, width, height;

   // Initialise the camera
   init_camera(cam);

   while (true) {

      // Get the blobs from the camera into the array
      get_blobs(cam, nblobs, bc, bl, bt, br, bb);

      if (nblobs == 1) {
         if (!erased) {
            nxtDisplayTextLine(0,"Tracking ...");
            erased = TRUE;
         }

         // Find the centre of the blob using double resolution of camera
         x_centre = bl[0] + br[0];
         y_centre = bt[0] + bb[0];

         // Compute the error from the desired position of the blob (using double resolution)
         x_error = 176 - x_centre;
         y_error = 143 - y_centre;

         // Drive the motors proportional to the error
         motor[l_motor] = (y_error - x_error) / 5;
         motor[r_motor] = (y_error + x_error) / 5;

      } else {
         motor[l_motor] = 0;
         motor[r_motor] = 0;
         nxtDisplayTextLine(0,"Found %d blobs.",nblobs);
         erased = FALSE;
      }
   }
}

task main()
{
   while (true)
   {
   checkAllDirections(); //
   motor[sonarMotor] = 0;
   wait1Msec(5);
   if (wallRight > 30)
     {
     turnRight();
     goStraight();
     }
       if (wallFront > 30 & wallRight < 30 & wallLeft < 30)
       {
       goStraight();
       }
       if (wallFront > 30 & wallRight < 30 & wallLeft > 30)
       {
       goStraight();
       }
           if (wallLeft > 30 & wallFront < 30 & wallRight < 30)
           {
           turnLeft();
           goStraight();
           }
               if (wallRight < 30 & wallFront  < 30 &wallLeft < 30)
               {
               goOpposite();

               }
}
}

task main()
{
   while (true)
   {
    _color = SensorValue(HTCS2);
   if (_color == 7)
   {
       BeethovenFifth();
   }
}}

task main()
{
    while (true)
    {
        if (SensorValue(HTCS2) == 7)
        {
            BeethovenFifth();
        }
        if (SensorValue(HTCS2) != 7)
        {
    checkAllDirections(); //
    motor[sonarMotor] = 0;
    wait1Msec(5);
    if (wallRight > 30)
      {
      turnRight();
      goStraight();
      }
        if (wallFront > 30 & wallRight < 30 & wallLeft < 30)
        {
        goStraight();
        }
        if (wallFront > 30 & wallRight < 30 & wallLeft > 30)
        {
        goStraight();
        }
            if (wallLeft > 30 & wallFront < 30 & wallRight < 30)
            {
            turnLeft();
            goStraight();
            }
                if (wallRight < 30 & wallFront  < 30 &wallLeft < 30)
                {
                goOpposite();

                }
}
}
}

task main() {
  while (true) {
    if (SensorValue(HTCS2) == 7) {
      BeethovenFifth();
    }
   
    checkAllDirections(); //
    motor[sonarMotor] = 0;
    wait1Msec(5);
    if (wallRight > 30) {
         turnRight();
         goStraight();
    } else if (wallFront > 30 & wallRight < 30 & wallLeft < 30) {
      goStraight();
    } else if (wallFront > 30 & wallRight < 30 & wallLeft > 30) {
      goStraight();
    } else if (wallLeft > 30 & wallFront < 30 & wallRight < 30) {
         turnLeft();
         goStraight();
    } else if (wallRight < 30 & wallFront  < 30 &wallLeft < 30) {
      goOpposite();
    }
  }
}