LineFollowing/LineFollowing.pde

   1 /*
   2 Linefollowing code by Gabriel Fornaeus
   3 ######
   4 More information at http://hax0r.se
   5 */
   6
   7 // Header file for musical lulz
   8 #include "pitches.h"
   9 // Servo control
  10 #include <Servo.h>
  11
  12 // For the Pololu Qik motor controller
  13 #include <CompactQik2s9v1.h>
  14 #include <NewSoftSerial.h>
  15 // Setup pins for the motor controller
  16 #define rxPin 8
  17 #define txPin 9
  18 #define rstPin 10
  19
  20 NewSoftSerial mySerial = NewSoftSerial(rxPin,txPin);
  21 CompactQik2s9v1 motor = CompactQik2s9v1(&mySerial,rstPin);
  22
  23 Servo sensorServo; // Create object to control the front servo with the sensor
  24 // Pololu QTR sensors
  25 #include <PololuQTRSensors.h>
  26 #define NUM_SENSORS   3     // Number of sensors used(actually five, but we'll skip the outer two)
  27 #define TIMEOUT       2500  // Waits for 2500 us for sensor outputs to go low
  28
  29 // Sensors 0 through 5 are connected to digital pins 4 through 6, respectively
  30 PololuQTRSensorsRC qtrrc((unsigned char[]) { 4 , 5, 6, },
  31   NUM_SENSORS, TIMEOUT);
  32 unsigned int sensorValues[NUM_SENSORS];
  33
  34 void setup()
  35 {
  36         // Output pins
  37         pinMode(8, OUTPUT);
  38         pinMode(9, OUTPUT);
  39         pinMode(10, OUTPUT);
  40         pinMode(13, OUTPUT);
  41         pinMode(12, OUTPUT);
  42         // Input pins
  43         pinMode(4, INPUT);
  44         pinMode(5, INPUT);
  45         pinMode(6, INPUT);
  46
  47         // Don't want the servo to move about, even though we'e not using it
  48         sensorServo.attach(2);
  49         sensorServo.write(3);
  50         // Setup motors
  51         mySerial.begin(9600);
  52         Serial.begin(9600);
  53         motor.begin();
  54         delay(200);
  55         motor.stopBothMotors();
  56
  57   digitalWrite(13, HIGH);
  58
  59   delay(300);
  60   int i;
  61   for (i = 0; i < 80; i++)
  62         {
  63             if (i < 20 || i >= 60)
  64                 {
  65                 motor.motor0Forward(75);
  66                 motor.motor1Reverse(75);
  67                 }
  68                 else
  69                 {
  70                 motor.motor1Forward(75);
  71                 motor.motor0Reverse(75);
  72                 }
  73     qtrrc.calibrate();  // Reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  74         }
  75         // Stop when the calibration is done
  76         motor.stopBothMotors();
  77         // Play start noise( add delay later)
  78         digitalWrite(13, LOW);
  79         tone(12, NOTE_E3, 500);
  80         delay(1000);
  81         tone(12, NOTE_E3, 500);
  82         delay(1000);
  83         tone(12, NOTE_E3, 500);
  84         delay(1000);
  85         tone(12, NOTE_A3, 1000);
  86         delay(1010);
  87         motor.stopBothMotors();
  88 }
  89
  90
  91 void loop()
  92 {
  93         unsigned int position = qtrrc.readLine(sensorValues);
  94
  95         // We're far to the right of the line, turn left
  96         if (position < 1000)
  97         {
  98                 //Serial.println("right of line");
  99                 digitalWrite(13, LOW);
 100                 motor.motor0Forward(127);
 101                 motor.motor1Reverse(127);
 102         }
 103         // We're sort of centered, drive forward
 104         if(position < 2000)
 105         {
 106                 digitalWrite(13, HIGH);
 107                 motor.motor0Forward(127);
 108                 motor.motor1Forward(127);
 109         }
 110         // We're to the left, turn right
 111         else
 112         {
 113                 //Serial.println("left of line");
 114                 digitalWrite(13, LOW);
 115                 motor.motor0Reverse(127);
 116                 motor.motor1Forward(127);
 117         }
 118
 119 }
 120
 121 /*
 122 // With PID control
 123 void followSegment()
 124 {
 125   int lastProportional = 0;
 126   long integral=0;
 127
 128   while(1)
 129   {
 130     // Normally, we will be following a line.  The code below is
 131     // similar to the 3pi-linefollower-pid example, but the maximum
 132     // speed is turned down to 60 for reliability.
 133
 134     // Get the position of the line.
 135         unsigned int position = qtrrc.readLine(sensorValues);
 136
 137     // The "proportional" term should be 0 when we are on the line.
 138     int proportional = ((int)position) - 2000;
 139
 140     // Compute the derivative (change) and integral (sum) of the
 141     // position.
 142     int derivative = proportional - lastProportional;
 143     integral += proportional;
 144
 145     // Remember the last position.
 146     lastProportional = proportional;
 147
 148     // Compute the difference between the two motor power settings,
 149     // m1 - m2.  If this is a positive number the robot will turn
 150     // to the left.  If it is a negative number, the robot will
 151     // turn to the right, and the magnitude of the number determines
 152     // the sharpness of the turn.
 153     int powerDifference = proportional/20 + integral/10000 + derivative*3/2;
 154
 155     // Compute the actual motor settings.  We never set either motor
 156     // to a negative value.
 157     const int maximum = 80; // the maximum speed
 158     if (powerDifference > maximum)
 159         {
 160       powerDifference = maximum;
 161         }
 162     if (powerDifference < -maximum)
 163         {
 164       powerDifference = -maximum;
 165         }
 166     if (powerDifference < 0)
 167         {
 168       motor.motor0Forward(maximum + powerDifference);
 169       motor.motor1Forward(maximum + powerDifference);
 170         }
 171     else
 172     {
 173       motor.motor0Forward(maximum - powerDifference);
 174       motor.motor1Forward(maximum - powerDifference);
 175         }
 176
 177     // We use the inner three sensors (1, 2, and 3) for
 178     // determining whether there is a line straight ahead, and the
 179     // sensors 0 and 4 for detecting lines going to the left and
 180     // right.
 181
 182     if (sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100)
 183     {
 184       // There is no line visible ahead, and we didn't see any
 185       // intersection.  Must be a dead end.
 186       return;
 187     }
 188     else if (sensors[0] > 200 || sensors[4] > 200)
 189     {
 190       // Found an intersection.
 191       return;
 192     }
 193
 194   }
 195 }
 196 */