Pololu Robotics & Electronics
Menu
My account Comments or questions? About Pololu Contact Ordering information Distributors

Pololu Forum

Programming maze solving robot to do sharp turns


#1

I am trying to make a line follower and maze solver robot using Arduino and QTR8RC sensors. In that case I can’t rotate my robot on sharp 90 degree or turn around…Here is my code…please help me to improve this code…

#include <Button.h>
#include <QTRSensors.h>

#include <Wire.h>
#define NUMREADINGS 10 

 Button button = Button(1,PULLUP);

QTRSensorsRC qtr((unsigned char[]) {13, 12, 9, 8 ,7, 4, 3, 2}, 8,3500, QTR_NO_EMITTER_PIN);
 unsigned int sensors[8];

 //left Motor
  int inA1 = 6;
  int inA2 = 5;


// Right  motor
  int inB1 = 11; // Pin Number of Dual-H
  int inB2 = 10;

  int pinBattery = 19;
  int pinAudio = 18;

 int voltage;
  int readings[NUMREADINGS];
  float volts = 0;
  int total = 0;
  float average = 0;
  int index = 0;
 
void setup(){
 
  
  Serial.begin(9600);        // Initializes the serial communication
  pinMode(pinAudio, OUTPUT); // Sets pin as audio output
  set_motors(0,0); // While waiting, engines remain standing
 
  voltage = read_battery(); // Reads the battery voltage

  Serial.print("Battery");
  
  Serial.print(voltage);
  Serial.print(" mV");
  delay(2000);

  while(!button.isPressed()){    
  }
  delay(500); // Delay to allow time to take your finger off the button
  // Always wait for a button to be pressed before
  // Your robot might start moving

   
 
  unsigned int counter; // used as a simple counter
  for(counter=0; counter<80; counter++){
    if(counter < 20 || counter >= 60){
      set_motors(80,-80); // Turn right
    }
    else{
      set_motors(-80,80); // Turns to the left
    }
    // This function stores a set of sensor readings , and maintains
    // Information about the maximum and minimum values ​​found
    qtr.calibrate();
    // Since we count up to 80 , the total calibration time
    // Will be 80 * 10 = 800 ms

    delay(10);
  }


  
  set_motors(0,0); // Ensure engines stopped after the process
                   // Calibration
 
}



// Drive engine
void set_motors(int left_speed, int right_speed){
  if(right_speed >= 0 && left_speed >= 0){
    analogWrite(inA1, 0);
    analogWrite(inA2, right_speed);
    analogWrite(inB1, 0);
    analogWrite(inB2, left_speed);
  }
  if(right_speed >= 0 && left_speed < 0){
    left_speed = -left_speed;
    analogWrite(inA1, 0);
    analogWrite(inA2, right_speed);
    analogWrite(inB1, left_speed);
    analogWrite(inB2, 0);
  }
  if(right_speed < 0 && left_speed >= 0){
    right_speed = -right_speed;
    analogWrite(inA1, right_speed);
    analogWrite(inA2, 0);
    analogWrite(inB1, 0);
    analogWrite(inB2, left_speed);
  } 
}


// Check battery voltage
unsigned int read_battery(){

  for (int k = 0; k < NUMREADINGS; k++){ // Resetting the matrix
    readings[k] = 0;
  }
  total -= readings[index];// Initializes Total
  readings[index] = analogRead(pinBattery); // Reads the sensor
  total += readings[index]; // Sum of matrix values
  index = (index + 1); // next value
  if (index >= NUMREADINGS){ // Check if reached the end of the readings
    index = 0; // If yes resets the index
  }
  average = total / NUMREADINGS; // simple average
  
  volts = average * 5000 * 3 / 2 / 1023 * 10;
  return (volts); 
}

// Play Music
void playMusic(){
  // The frequency setting of banknotes
  #define NOTE_B0  31
  #define NOTE_C1  33
  #define NOTE_CS1 35
  #define NOTE_D1  37
  #define NOTE_DS1 39
  #define NOTE_E1  41
  #define NOTE_F1  44
  #define NOTE_FS1 46
  #define NOTE_G1  49
  #define NOTE_GS1 52
  #define NOTE_A1  55
  #define NOTE_AS1 58
  #define NOTE_B1  62
  #define NOTE_C2  65
  #define NOTE_CS2 69
  #define NOTE_D2  73
  #define NOTE_DS2 78
  #define NOTE_E2  82
  #define NOTE_F2  87
  #define NOTE_FS2 93
  #define NOTE_G2  98
  #define NOTE_GS2 104
  #define NOTE_A2  110
  #define NOTE_AS2 117
  #define NOTE_B2  123
  #define NOTE_C3  131
  #define NOTE_CS3 139
  #define NOTE_D3  147
  #define NOTE_DS3 156
  #define NOTE_E3  165
  #define NOTE_F3  175
  #define NOTE_FS3 185
  #define NOTE_G3  196
  #define NOTE_GS3 208
  #define NOTE_A3  220
  #define NOTE_AS3 233
  #define NOTE_B3  247
  #define NOTE_C4  262
  #define NOTE_CS4 277
  #define NOTE_D4  294
  #define NOTE_DS4 311
  #define NOTE_E4  330
  #define NOTE_F4  349
  #define NOTE_FS4 370
  #define NOTE_G4  392
  #define NOTE_GS4 415
  #define NOTE_A4  440
  #define NOTE_AS4 466
  #define NOTE_B4  494
  #define NOTE_C5  523
  #define NOTE_CS5 554
  #define NOTE_D5  587
  #define NOTE_DS5 622
  #define NOTE_E5  659
  #define NOTE_F5  698
  #define NOTE_FS5 740
  #define NOTE_G5  784
  #define NOTE_GS5 831
  #define NOTE_A5  880
  #define NOTE_AS5 932
  #define NOTE_B5  988
  #define NOTE_C6  1047
  #define NOTE_CS6 1109
  #define NOTE_D6  1175
  #define NOTE_DS6 1245
  #define NOTE_E6  1319
  #define NOTE_F6  1397
  #define NOTE_FS6 1480
  #define NOTE_G6  1568
  #define NOTE_GS6 1661
  #define NOTE_A6  1760
  #define NOTE_AS6 1865
  #define NOTE_B6  1976
  #define NOTE_C7  2093
  #define NOTE_CS7 2217
  #define NOTE_D7  2349
  #define NOTE_DS7 2489
  #define NOTE_E7  2637
  #define NOTE_F7  2794
  #define NOTE_FS7 2960
  #define NOTE_G7  3136
  #define NOTE_GS7 3322
  #define NOTE_A7  3520
  #define NOTE_AS7 3729
  #define NOTE_B7  3951
  #define NOTE_C8  4186
  #define NOTE_CS8 4435
  #define NOTE_D8  4699
  #define NOTE_DS8 4978

  // Music
 
  int note[] = {NOTE_C4, NOTE_C4, NOTE_G4, NOTE_C5, NOTE_G4, NOTE_C5};
  int duration[] = {100, 100, 100, 300, 100, 300};

  int starttune[] = {NOTE_C4, NOTE_F4, NOTE_C4, NOTE_F4, NOTE_C4, NOTE_F4, NOTE_C4, NOTE_F4, NOTE_G4, NOTE_F4, NOTE_E4, NOTE_F4, NOTE_G4};
  int duration2[] = {100, 200, 100, 200, 100, 400, 100, 100, 100, 100, 200, 100, 500};

  int error[] = {NOTE_G3, NOTE_C3, NOTE_G3, NOTE_C3, NOTE_G3, NOTE_C3, NOTE_G3, NOTE_C3};
  int duration3[] = {100, 200, 100, 200, 100, 200, 100, 200};
  
  // Loop with quandiade of notes to be played
  //If necessary, change the internal value of the "for" loop
  for(int i=0;i<6;i++){
     tone(pinAudio, note[i], duration[i]); // Emits the sound note / duration
     delay(duration[i]);                   // Pausa 
     noTone(pinAudio);                     // Finalize
  }
}




void follow_segment(){
          int last_proportional=0;
          long integral;

  // This is the main loop - will run forever
  while(1){
    // Gets the line position
    // Here we are not interested in the individual values ​​of each sensor
    unsigned int position = qtr.readLine(sensors);
     
    
  // The proportional term must be 0 when we are on line
    int proportional = ((int)position) - 3500;
        
    // Calculate the derivative term ( change) and the integral term (sum )
    // Position
   
    int derivative = proportional - last_proportional;
     integral += proportional;
      last_proportional = proportional;
    // Calculate the difference between the power of the two engines 
    // M1 - M2 . If a positive number , the robot will turn to
    // Right. If it is a negative number, the robot will turn left
    // And magnetude of numbers determine the sharpness with which will make the curves / turns
    int power_difference = proportional/2.5 + integral/2500 + derivative*3/8;
    
   
   
    
    
// Remembering the last position
  // Calculate the current configuration of the engines. We will never set
    // A motor with a negative value
    const int max = 255;
    const int max_diffrence = 20;
    const int factor_diffrence = 2;

    if(power_difference > max)
        power_difference = max;
    if(power_difference < -max)
        power_difference = -max;

    
    // if diffrence is too much robot skids 

    int left_speed  = max;
    int right_speed = max-power_difference;

    if(power_difference < 0)
    {
        left_speed  = max+power_difference;
        right_speed = max;
    }


    if(left_speed - right_speed > max_diffrence)
    {
        left_speed -= (left_speed - right_speed)/factor_diffrence;
    } 
    else if(right_speed - left_speed > max_diffrence)
    {
        right_speed -= (right_speed - left_speed)/factor_diffrence;
    }

    set_motors(left_speed,right_speed);
  }
  qtr.read(sensors);
  if(sensors[0] < 100  && sensors[1] < 100 && sensors[2] < 100  && sensors[3] < 100 && sensors[4] < 100  && sensors[5] < 100  &&  sensors[6] < 100  && sensors[7] < 100)
            {
                // There is no line visible ahead, and we didn't see any
                  // intersection. Must be a dead end.
                return;
            }

          
        else if((sensors[0] > 200  &&  sensors[1]>200)  || (sensors[6] > 200  && sensors[7]>200 ))
              {
              // Found an intersection.
                return;
                }
  
  }

void turn(unsigned char dir){
  
      switch(dir){
          case 'L':
                    // Turn left.
                    set_motors(-100,100);
                    delay(200);
                    break;
          case 'R':
                    // Turn right.
                    set_motors(100,-100);
                    delay(200);
                    break;
          case 'B':
                    // Turn around.
                    set_motors(100,-100);
                    delay(400);
                    break;
          case 'S':
                    // Don't do anything!
                    break;

                   }
  
  }

char path[100] = "";
unsigned char path_length = 0;

  

unsigned char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right)
{
    // Make a decision about how to turn. The following code
    // implements a left-hand-on-the-wall strategy, where we always
    // turn as far to the left as possible.
    if(found_left)
          return 'L';
    else if(found_straight)
          return 'S';
    else if(found_right)
          return 'R';
    else
          return 'B';
}

void simplify_path()
{
    // only simplify the path if the second-to-last turn was a 'B'
  if (path_length < 3 || path[path_length-2] != 'B')
    return;

  int total_angle = 0;
  int i;
  for (i = 1; i <= 3; i++)
  {
    switch (path[path_length - i])
    {
    case 'R':
      total_angle += 90;
      break;
    case 'L':
      total_angle += 270;
      break;
    case 'B':
      total_angle += 180;
      break;
    }
  }

  // Get the angle as a number between 0 and 360 degrees.
  total_angle = total_angle % 360;

  // Replace all of those turns with a single one.
  switch (total_angle)
  {
  case 0:
    path[path_length - 3] = 'S';
    break;
  case 90:
    path[path_length - 3] = 'R';
    break;
  case 180:
    path[path_length - 3] = 'B';
    break;
  case 270:
    path[path_length - 3] = 'L';
    break;
  }

  // The path is now two steps shorter.
  path_length -= 2;
}

  


void loop() {
 while(!button.isPressed()){
  }
  delay(200);
  playMusic();
 
  delay(250);
  while(1){
            
             follow_segment();
              

              unsigned char found_left=0;
              unsigned char found_straight=0;
              unsigned char found_right=0;
            
                // Now read the sensors and check the intersection type.
              unsigned int sensors[8];
              qtr.read(sensors);

           
            if(sensors[7]> 100  && sensors[6]> 100 && sensors[6]> 100)
              found_left=1;
              
         
              if(sensors[0] > 100  &&  sensors[1] > 100 )
              found_right = 1;
              // Drive straight a bit more - this is enough to line up our
              // wheels with the intersection.
              set_motors(100,100);
              delay(200);
                // Check for a straight exit.
              qtr.read(sensors);
           if(sensors[3] > 500 || sensors[4] > 500)
            found_straight = 1;
            unsigned char dir = select_turn(found_left, found_straight, found_right);
          // Make the turn indicated by the path.
          turn(dir);
          
          path[path_length] = dir;
    path_length++;
    simplify_path();
  }


  while(1){
    set_motors(0,0);

    while(!button.isPressed()){    
  }
  delay(30);
  int i;
    for (i = 0; i < path_length; i++)
    {
      follow_segment();

      // Drive straight while slowing down, as before.
      set_motors(50, 50);
      delay(50);
      set_motors(40, 40);
      delay(200);

      // Make a turn according to the instruction stored in
      // path[i].
      turn(path[i]);
    }
    
    follow_segment();
    }

  
}

#2

Hello.

It looks like your code already has logic for determining and executing the turns. What happens when you run your robot on the maze? Could you post a video showing the behavior of your robot?

-Brandon


#3

during 90 degree turn…one motor is stopped and another is running…but the stopped motor didn’t turn reversly


#4

this is the simple line following video link…


#5

Does the robot turn correctly (one wheel forward, one wheel reverse) when it is in the calibration phase? It looks like your code is effectively setting the speed of each motor to max - power_difference where max is 255 and power_difference is being limited to 255, so left_speed and right_speed can never be negative. You might try adding some debugging using the serial monitor (e.g. printing left_speed and right_speed to see if they ever go negative).

Brandon


#6

Dear BradonM I was changed the previous code and the the turn is ok, but the problem is sometimes the robot skid very much…I cant understand the problem is…sometimes the robot can run in proper way but sometimes is doesn’t…


#7

Tracked robots rely on skidding to turn, so it sounds like what you are describing is probably normal. If you think the skidding is excessive, you might try cleaning the tracks.

If the skidding is causing some inconsistency in the turns, you might consider implementing a similar turning logic as the maze solver example program for the Zumo shield. Instead of turning for a set amount of time, that example turns until the reflectance sensors detect the line again.

Brandon


What is the best way to solve this maze? (coding)