97Views4Replies

Author Options:

Help for realising a linefollower tankbot Answered

Hello,

I want to realize a linefollowing tank bot.

I got this following parts:

- Arduino Mega 2560
- Arduino MotorShield Rev03
- Pololu QTR-8RC Array
- Pololu RP5 Tankchassis (6V DC Motors)

Now all parts are together and the code must be completed.
Therefore I found a really nice PID-based tutorial for linefollower:
http://letsmakerobots.com/blog/enigmerald/pid-tutorials-line-following

I create a program, but after calibrating the sensors, the bot doesn't drive on the line. It makes no sense were it drives.
Most time it do 90° turns and so on. Do you find a better solution for my code?

[code]
#include <QTRSensors.h>
#include <Servo.h> 

///////////////////////////
// define constants   //
///////////////////////////

#define AUTO_CALIB  false      // Autokalibrierung aktivieren
#define DEBUG_ENABLE   true               // false=keine Serial Ausgaben
#define NUM_SENSORS    8                  // Anzahl der verwendeten Sensoren
#define TIMEOUT        2500               // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN    QTR_NO_EMITTER_PIN         // Default: kein Emitterpin definiert
#define MOTOR_SPEED    100               // PWM Signal 0-255 (Basismotorgeschwindigkeit)
#define MOTOR_MAX_SPEED 255       // PWM Signal für Maximalgeschwindigkeit
#define KP     0.2                    // KP-Wert für PID Regler
#define KD     5                     // KD-Wert für PID Regler ( Beachte: Kp < Kd)
#define SERVO_PIN1  11       // PWM Ausgang für Servo1
#define SERVO_PIN2  12       // PWM Ausgang für Servo2
#define CALIB_LED   13       // LED Ausgang für Kalibrierungsphase


//////////////////////////
// define variables     //
//////////////////////////

// Zuweisung der Sensoren 0-7 an die digitalen IO's 22-29
QTRSensorsRC qtrrc((unsigned char[]) {22, 23, 24, 25, 26, 27, 28, 29}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];


int MotorLeftSpeed=3;                  // PWM pin for speed control left
int MotorRightSpeed=11;                // PWM pin for speed control right
int MotorLeftDirection=12;            // digital pin for direction control left
int MotorRightDirection=13;            // digital pin for direction control right
int MotorLeftBrake=9;                  // digital pin for brake control left
int MotorRightBrake=8;                 // digital pin for brake control right
int lastError = 0;




/////////////////////
// Initialisierung //
/////////////////////


void setup()
{
 
  // Initialisierung verschiedner IOs
  pinMode(CALIB_LED, OUTPUT);    // Definiert LED Ausgang für Kalibierungsphase
 
 
  // Initialisierung linker Motor
  pinMode(MotorLeftDirection, OUTPUT);      // Definiert Ausgang für den linken Motor
  pinMode(MotorLeftBrake, OUTPUT);          // Definiert Bremse für den linken Motor
  pinMode(MotorLeftSpeed, OUTPUT);   // Definiert Geschwindigkeitsausgang für linken Motor

  // Initialisierung rechter Motor
  pinMode(MotorRightDirection, OUTPUT);     // Definiert Ausgang für den rechten Motor
  pinMode(MotorRightBrake, OUTPUT);         // Definiert Bremse für den rechten Motor
 
  // Initialisierung Servo Motoren
  servo1.attach(SERVO_PIN1);     // Zuweisung PWM PIN für Servo1
  servo2.attach(SERVO_PIN2);     // Zuweisung PWM PIN für Servo2

  if (DEBUG_ENABLE)
  {
   Serial.begin(9600);
   Serial.print("Debug Modus aktiviert:\n");  
  }
 
  delay(500);
 
  digitalWrite(CALIB_LED, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode

  for (int i = 0; i < 400; i++)   // make the calibration take about 10 seconds
  {

    qtrrc.calibrate();           // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  }

delay(20);      // The 20 ms delay in the calibration code is intended to make the routine take a reasonable amount of time.
 
  digitalWrite(CALIB_LED, LOW);     // turn off Arduino's LED to indicate we are through with calibration

 
    if (DEBUG_ENABLE)
   {
   // print the calibration minimum values measured when emitters were on
  
   for (int i = 0; i < NUM_SENSORS; i++)
   {
  Serial.print(qtrrc.calibratedMinimumOn[i]);
  Serial.print(' ');
   }
   Serial.println();
  
   // print the calibration maximum values measured when emitters were on
   for (int i = 0; i < NUM_SENSORS; i++)
   {
  Serial.print(qtrrc.calibratedMaximumOn[i]);
  Serial.print(' ');
   }
   Serial.println();
   Serial.println();
  }
  delay(1000);
 
}



///////////////////
// Main Program  //
///////////////////


void loop()
{

 
// read calibrated sensor values and obtain a measure of the line position from 0 to 5000
// To get raw sensor values, call:
// qtrrc.read(sensorValues); instead of unsigned int position = qtrrc.readLine(sensorValues);
// Variable positon zeigt die genaue Position (0-7000) an
 
unsigned int position = qtrrc.readLine(sensorValues); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.
int error = position - 3500;  // 3500 is the desired position

  if (DEBUG_ENABLE)
  {
   // print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and
   // 1000 means minimum reflectance, followed by the line position
   for (unsigned char i = 0; i < NUM_SENSORS; i++)
   {
    Serial.print(sensorValues[i]);
    Serial.print('\t');
   }
   Serial.print(position);    // comment this line out if you are using raw values
  }

int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;

int rightMotorSpeed = MOTOR_SPEED + motorSpeed;
int leftMotorSpeed  = MOTOR_SPEED - motorSpeed;


 
  if (rightMotorSpeed > MOTOR_MAX_SPEED ) rightMotorSpeed = MOTOR_MAX_SPEED;    // Verhindert dass die Maximalgeschwindigkeit überschritten wird
if (leftMotorSpeed > MOTOR_MAX_SPEED ) leftMotorSpeed  = MOTOR_MAX_SPEED;    // Verhindert dass die Maximalgeschwindigkeit überschritten wird
if (rightMotorSpeed < 0) rightMotorSpeed  = 0;          // Verhindert einen negativen Geschwindigkeitswert
if (leftMotorSpeed < 0) leftMotorSpeed   = 0;          // Verhindert einen negativen Geschwindigkeitswert



 
{
  // Ansteuerung der Motoren durch PID Berechnung
  digitalWrite(MotorRightBrake, LOW);     // Motorbremse rechts deaktivieren
  digitalWrite(MotorLeftBrake, LOW);     // Motorbremse links deaktivieren
  digitalWrite(MotorRightDirection, HIGH);   // Motor rechts im Uhrzeigersinn  --> fährt vorwärts
  digitalWrite(MotorLeftDirection, LOW);    // Motor links gegen den Uhrzeigersinn  --> fährt vorwärts
  analogWrite(MotorRightSpeed, rightMotorSpeed);  // Motorgeschwindigkeit rechts in Abhängigkeit des PID Reglers
  analogWrite(MotorLeftSpeed, leftMotorSpeed);  // Motorgeschwindigkeit links in Abhängigkeit des PID Reglers
}
 
 
   
 
}
[/code]

Discussions

0
None
julianpe

3 years ago

This is the robot. I just started with a straight black line to try the forward move.

IMG_20151114_200839.jpg
0
None
icengjulianpe

Reply 3 years ago

I do not read German. What are the sensors are you using ?... How many optical sensors do you have ?... Have you meter reading and hand movement verified they all work ?...

BTW, I do not see where or how a PID Proportional-Integral-Derivative algorithm is useful in this type of line follower.... This link will show you many line follow ideas.

https://www.google.com/search?q=line+sensors&biw=1...

2line-following-robot-with-2-sensors.jpg3sensors.jpg3lightsensor3diagram.jpg
0
None
VigneshW8

3 years ago

the values above are just for example--you need to test for yourself to see what value your sensor reads when it it centered over edge of your line.

0
None
Kiteman

3 years ago

The problem may also be hardware - it would help if you posted photos of your build, the sensor-head and the line you are wanting to follow. Maybe even a video of the aberrant behaviour?