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