Changed debounce-time
[Arduino/.git] / 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 */