LInelightfollower

#include <Pololu3pi.h>
#include <PololuQTRSensors.h>
#include <OrangutanMotors.h>
#include <OrangutanAnalog.h>
#include <OrangutanLEDs.h>
#include <OrangutanLCD.h>
#include <OrangutanPushbuttons.h>
#include <OrangutanBuzzer.h>

int pin_number6=6;
int pin_number7=7;
int counter=0;
int light=1;
int prevlight=0;
int i=0;
int j=1;
int maxs=500;


void lightfollower()
{
 pinMode(pin_number6, INPUT);
 pinMode(pin_number7, INPUT);
 while(counter<3)
  {
   while(i<1)
   {
   set_motors(50,-50);
   delay(700);
   set_motors(50,50);
   delay(500);
   i=1;
   }

  long LDR_1;
  long LDR_2;

  LDR_1=analogRead(pin_number6); // Left LDR Read //
  LDR_2=analogRead(pin_number7); // Right LDR Read //

  // Displaying LDR Values //
  lcd_goto_xy(0,0);
  print_long(LDR_1);
  lcd_goto_xy(0,1);
  print_long(LDR_2);

  // Setting Motors based on LDR's //
  int m1;
  int m2;

  if (LDR_1 <= ((LDR_2)+20) && LDR_1 >= ((LDR_2)-20) && (LDR_1 < maxs && LDR_2 < maxs))
  {
    set_motors(50,50);
    light=1;
    prevlight=1;
  }

 // id (LDR1 < 30 && LDR1>50)
// or ||
// and &&

 if ( LDR_1 < ((LDR_2)-20)&& (LDR_1 < maxs && LDR_2 < maxs) )
 {
  set_motors(35,50);
  light=1;
  prevlight=1;
 }

  if ( LDR_1 > ((LDR_2)+20)&& (LDR_1 < maxs && LDR_2 < maxs) )
 {
  set_motors(50,35);
  light=1;
  prevlight=1;
 }

 if ( LDR_1 < ((LDR_2)-50)&& (LDR_1 < maxs && LDR_2 < maxs) )
 {
  set_motors(20,50);
  light=1;
  prevlight=1;
 }

  if ( LDR_1 > ((LDR_2)+50)&& (LDR_1 < maxs && LDR_2 < maxs) )
 {
  set_motors(50,20);
  light=1;
  prevlight=1;
 }

 if (LDR_1 > maxs && LDR_2 > maxs)
 {
   set_motors(0,0);
   delay (1);
   if(j<3)
   {
   set_motors(0,0);
   delay(500);
   set_motors(-30,-30);
   delay(500);
   set_motors(40,-40);
   delay(450);
   set_motors(50,50);
   delay(1000);
   set_motors(0,0);
   delay(1);
   j++;
   }

   light=0;
   if (light != prevlight);
   {
     counter++;
     prevlight=0;
   }
 }


  }


}

Pololu3pi robot;
unsigned int sensors[5]; // an array to hold sensor values
unsigned int last_proportional = 0;
long integral = 0;
#include <avr/pgmspace.h>
const char welcome_line1[] PROGMEM = " Pololu";
const char welcome_line2[] PROGMEM = "3\xf7 Robot";
const char demo_name_line1[] PROGMEM = "PID Line";
const char demo_name_line2[] PROGMEM = "follower";


// Data for generating the characters used in load_custom_characters
// and display_readings.  By reading levels[] starting at various
// offsets, we can generate all of the 7 extra characters needed for a
// bargraph.  This is also stored in program space.
const char levels[] PROGMEM = {
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  0b00000,
  0b11111,
  0b11111,
  0b11111,
  0b11111,
  0b11111,
  0b11111,
  0b11111
};

// This function loads custom characters into the LCD.  Up to 8
// characters can be loaded; we use them for 7 levels of a bar graph.
void load_custom_characters()
{
  OrangutanLCD::loadCustomCharacter(levels + 0, 0); // no offset, e.g. one bar
  OrangutanLCD::loadCustomCharacter(levels + 1, 1); // two bars
  OrangutanLCD::loadCustomCharacter(levels + 2, 2); // etc...
  OrangutanLCD::loadCustomCharacter(levels + 3, 3);
  OrangutanLCD::loadCustomCharacter(levels + 4, 4);
  OrangutanLCD::loadCustomCharacter(levels + 5, 5);
  OrangutanLCD::loadCustomCharacter(levels + 6, 6);
  OrangutanLCD::clear(); // the LCD must be cleared for the characters to take effect
}

// This function displays the sensor readings using a bar graph.
void display_readings(const unsigned int *calibrated_values)
{
  unsigned char i;

  for (i=0;i<5;i++) {
    // Initialize the array of characters that we will use for the
    // graph.  Using the space, an extra copy of the one-bar
    // character, and character 255 (a full black box), we get 10
    // characters in the array.
    const char display_characters[10] = { ' ', 0, 0, 1, 2, 3, 4, 5, 6, 255 };

    // The variable c will have values from 0 to 9, since
    // calibrated values are in the range of 0 to 1000, and
    // 1000/101 is 9 with integer math.
    char c = display_characters[calibrated_values[i] / 101];

    // Display the bar graph character.
    OrangutanLCD::print(c);
  }
}

// Initializes the 3pi, displays a welcome message, calibrates, and
// plays the initial music.  This function is automatically called
// by the Arduino framework at the start of program execution.
///////////////////////////////////////////////////////////////////////////////////////


void linefollowsetup()
{
  unsigned int counter; // used as a simple timer

  // This must be called at the beginning of 3pi code, to set up the
  // sensors.  We use a value of 2000 for the timeout, which
  // corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
  robot.init(2000);

  load_custom_characters(); // load the custom characters

  OrangutanLCD::clear();
  OrangutanLCD::printFromProgramSpace(demo_name_line1);
  OrangutanLCD::gotoXY(0, 1);
  OrangutanLCD::printFromProgramSpace(demo_name_line2);


  // Auto-calibration: turn right and left while calibrating the
  // sensors.
  for (counter=0; counter<80; counter++)
  {
    if (counter < 20 || counter >= 60)
      OrangutanMotors::setSpeeds(40, -40);
    else
      OrangutanMotors::setSpeeds(-40, 40);

    // This function records a set of sensor readings and keeps
    // track of the minimum and maximum values encountered.  The
    // IR_EMITTERS_ON argument means that the IR LEDs will be
    // turned on during the reading, which is usually what you
    // want.
    robot.calibrateLineSensors(IR_EMITTERS_ON);

    // Since our counter runs to 80, the total delay will be
    //80*20 = 1600 ms.
    delay(20);
  }
  OrangutanMotors::setSpeeds(0, 0);

  // Display calibrated values as a bar graph.
    // Read the sensor values and get the position measurement.
    unsigned int position = robot.readLine(sensors, IR_EMITTERS_ON);

    // Display the position measurement, which will go from 0
    // (when the leftmost sensor is over the line) to 4000 (when
    // the rightmost sensor is over the line) on the 3pi, along
    // with a bar graph of the sensor readings.  This allows you
    // to make sure the robot is ready to go.
    OrangutanLCD::clear();
    OrangutanLCD::print(position);
    OrangutanLCD::gotoXY(0, 1);
    display_readings(sensors);

    delay(100);

}
////////////////////////////////////////////////////////////////////////

 int x=0;
void linefollower()
{
  while (x<1)
  {
  linefollowsetup();
  x++;
  }
  // Get the position of the line.  Note that we *must* provide
  // the "sensors" argument to read_line() here, even though we
  // are not interested in the individual sensor readings.
  unsigned int position = robot.readLine(sensors, IR_EMITTERS_ON);

  // The "proportional" term should be 0 when we are on the line.
  int proportional = (int)position - 2000;

  // Compute the derivative (change) and integral (sum) of the
  // position.
  int derivative = proportional - last_proportional;
  integral += proportional;

  // Remember the last position.
  last_proportional = proportional;

  // Compute the difference between the two motor power settings,
  // m1 - m2.  If this is a positive number the robot will turn
  // to the right.  If it is a negative number, the robot will
  // turn to the left, and the magnitude of the number determines
  // the sharpness of the turn.  You can adjust the constants by which
  // the proportional, integral, and derivative terms are multiplied to
  // improve performance.
  int power_difference = proportional/20 + integral/10000 + derivative*3/2;

  // Compute the actual motor settings.  We never set either motor
  // to a negative value.
  const int maximum = 70;
  if (power_difference > maximum)
    power_difference = maximum;
  if (power_difference < -maximum)
    power_difference = -maximum;

  if (power_difference < 0)
    OrangutanMotors::setSpeeds(maximum + power_difference, maximum);
  else
    OrangutanMotors::setSpeeds(maximum, maximum - power_difference);
}

void setup()
{
}
void loop()
{
lightfollower();
set_motors(50,-50);
delay(700);
set_motors(0,0);
linefollower();
}