#pragma config(Sensor, S1,     touchSensorL,   sensorTouch)
#pragma config(Sensor, S2,     lightSensor,    sensorLightActive)
#pragma config(Sensor, S3,     touchSensorR,   sensorTouch)
#pragma config(Sensor, S4,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  motorB,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*RESCUE ROBOTICS WORKSHOP #2


                           L I N E   F O L L O W E R
                                  Ron Kessler
                               Created 8/13/2018

Updated 8/14/2018


PURPOSE:	Demonstrates how to make bot follow a line using the light sensor.
					Makes use of decision structures. The robot will follow the black line
					on the test track.

BEHAVIORS: When the bot detects the dark color of the line, it will move away and when it
						detects the light part of the track, it will move the opposite direction.
						You will notice that the bot zig-zags around the course as it detects changes in
						reflected light.

SETUP:		When setting up sensors, be sure to select lightSensor | ActiveLight on port 2.
					The light sensor returns a number that indicates how much light is reflected back
					from the surface it is on.
						HIGHER NUMBER = BRIGHTER
						LOWER NUMBER = DARKER

					We need to find out what these readings are in your bot. The light sensor works best
					the closer it is to the surface. Keep it about the thickness of a quarter away from the track.

					1.	Turn on the Lego and press the >> button twice to get to the VIEW menu.
					2.	Click the >> button until you see Light Sensor.
					3.	Press the orange button to select it.
					4.	Now select the port# (I am using port 2). So click >> again and choose Port 2 and click
							the orange button to lock it in.

					5.	You will see a number indicating % of reflected light.
					6.	Place the robots light sensor directly over the black line on the test track and record the reading.
					7.	Now place it over the white part of the track and record that reading.
					8.	Mine is 34 & 46.
					9.	Now compute the average. This will be our threshold value to use in our code.This number will
							change as lighting changes so you should get these readings before any run. If the bot
							moves in random directions or is inconsistent in its movements, adjust the sensor closer to the
							surface and record readings again.

							We will use this threshold value to make the bot go left or right.


*/

//---STEP 1: CREATE OUR FUNCTIONS TO CONTROL MOTOR MOVEMENTS

int const lightThreshold = 52; 						//for my setup
int const fasterSpeed = 20;
int const slowerSpeed = 10;


void MoveLeft ()
{
	//---make rt motor go faster than left one
	motor[rightMotor] = fasterSpeed;
	motor[leftMotor] = slowerSpeed;
}

void MoveRight ()
{
	//---make left motor go faster than right one
	motor[rightMotor] = slowerSpeed;
	motor[leftMotor] = fasterSpeed;
}



task main()
{
   wait1Msec(50);
   while(true)                           // Infinite loop
   {
      if(SensorValue[lightSensor] < lightThreshold)
      {
        MoveLeft();
      }
      else
      {
        MoveRight();

      }
   }
}