LineFollowingPID/LineFollowingPID.pde

   1 /*
   2 Linefollowing code by Gabriel Fornaeus
   3 ######
   4 More information at http://hax0r.se
   5 */
   6
   7 // For the Pololu Qik motor controller
   8 #include <CompactQik2s9v1.h>
   9 #include <NewSoftSerial.h>
  10 // Setup pins for the motor controller
  11 #define rxPin 3
  12 #define txPin 4
  13 #define rstPin 5
  14
  15 NewSoftSerial mySerial = NewSoftSerial(rxPin,txPin);
  16 CompactQik2s9v1 motor = CompactQik2s9v1(&mySerial,rstPin);
  17
  18 // Pololu QTR sensors
  19 #include <PololuQTRSensors.h>
  20 #define NUM_SENSORS   3     // Number of sensors used(actually five, but we'll skip the outer two)
  21 #define TIMEOUT       2500  // Waits for 2500 us for sensor outputs to go low
  22
  23 // Sensors 0 through 5 are connected to digital pins 4 through 6, respectively
  24 PololuQTRSensorsRC qtrrc((unsigned char[]) {  8, 9, 10, },
  25   NUM_SENSORS, TIMEOUT);
  26 unsigned int sensorValues[NUM_SENSORS];
  27
  28 // Variables for PD control
  29
  30 // Proportional constant
  31 float KP = 0.1;
  32 // Derivative constant
  33 float KD = 5.05;
  34
  35 // Base motor speeds
  36 int M0 = 101;
  37 int M1 = 98;
  38
  39 int lastError = 0;
  40
  41 void setup()
  42 {
  43         // Output pins
  44         pinMode(3, OUTPUT);
  45         pinMode(4, OUTPUT);
  46         pinMode(5, OUTPUT);
  47         pinMode(11, OUTPUT);
  48         pinMode(12, OUTPUT);
  49         // Input pins
  50         pinMode(8, INPUT);
  51         pinMode(9, INPUT);
  52         pinMode(10, INPUT);
  53
  54         // Setup motors
  55         mySerial.begin(9600);
  56         motor.begin();
  57         delay(200);
  58         motor.stopBothMotors();
  59
  60   digitalWrite(13, HIGH);
  61
  62   delay(300);
  63   int i;
  64   for (i = 0; i < 80; i++)
  65         {
  66             if (i < 20 || i >= 60)
  67                 {
  68                 motor.motor0Forward(70);
  69                 motor.motor1Reverse(75);
  70                 }
  71                 else
  72                 {
  73                 motor.motor0Reverse(70);
  74                 motor.motor1Forward(75);
  75                 }
  76     qtrrc.calibrate();  // Reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  77         }
  78         // Stop when the calibration is done
  79         motor.stopBothMotors();
  80         // End of calibration period
  81         digitalWrite(13, LOW);
  82
  83         motor.stopBothMotors();
  84 }
  85
  86
  87 void loop()
  88 {
  89         // Get the position of the line
  90         int position = qtrrc.readLine(sensorValues,QTR_EMITTERS_ON);
  91
  92         /* Compute the "error" from the line position.
  93            Error will be 0 when we're dead center over the line, -1000 when we're to the left and 1000
  94            if it's to the right */
  95         int error = position - 1000;
  96
  97         // Set motor speed based on proportional and derivative PD terms
  98         int motorSpeed = KP * error + KD * (error - lastError);
  99         lastError = error;
 100
 101         int m0Speed = constrain((M0 + motorSpeed),0,127);
 102         int m1Speed = constrain((M1 - motorSpeed),0,127);
 103
 104         motor.motor0Forward(m0Speed);
 105         analogWrite(11, m0Speed);
 106         motor.motor1Forward(m1Speed);
 107         analogWrite(12, m1Speed);
 108 }