#include <QTRSensors.h>
#define NUM_SENSORS 8
#define TIMEOUT 2500
#define EMITTER_PIN QTR_NO_EMITTER_PIN
QTRSensorsRC qtrrc((unsigned char[]) {2, 4, 5, 6, 7, 8, 9,10},NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS]; // array with individual sensor reading values
unsigned int line_position=0;
int pwm_a = 3;
int pwm_b = 11;
int dir_a = 12;
int dir_b = 13;
int calSpeed = 85;
float error=0;
float PV =0 ;
float kp = 1;
int m1Speed=0;
int m2Speed=0;
void setup()
{
pinMode(pwm_a, OUTPUT);
pinMode(pwm_b, OUTPUT);
pinMode(dir_a, OUTPUT);
pinMode(dir_b, OUTPUT);
analogWrite(pwm_a, 0);
analogWrite(pwm_b, 0);
delay(2000);
for (int i = 0; i <= 100; i++) // begin calibration cycle to last about 2.5 seconds (100*25ms/call)
{
if (i==0 || i==60)
{
digitalWrite(dir_a, LOW);
analogWrite(pwm_a, calSpeed);
digitalWrite(dir_b, HIGH);
analogWrite(pwm_b, calSpeed);
}
else if (i==20 || i==100)
{
digitalWrite(dir_a, HIGH);
analogWrite(pwm_a, calSpeed);
digitalWrite(dir_b, LOW);
analogWrite(pwm_b, calSpeed);
}
qtrrc.calibrate();
}
line_position = qtrrc.readLine(sensorValues);
while (sensorValues[6] < 200)
{
line_position = qtrrc.readLine(sensorValues);
}
while (line_position > 4350)
{
line_position = qtrrc.readLine(sensorValues);
}
analogWrite(pwm_b, 0);
analogWrite(pwm_a, 0);
delay(1000);
}
void loop() // main loop
{
unsigned int line_position = qtrrc.readLine(sensorValues);
follow_line(line_position);
}
void follow_line(int line_position)
{
switch(line_position)
{
// Line has moved off the left edge of sensor
// This will make it turn fast to the left
case 7000:
digitalWrite(dir_a, HIGH);
analogWrite(pwm_a, 200);
digitalWrite(dir_b, LOW);
analogWrite(pwm_b, 200);
break;
case 0:
digitalWrite(dir_a, LOW);
analogWrite(pwm_a, 200);
digitalWrite(dir_b, HIGH);
analogWrite(pwm_b, 200);
break;
default:
error = (float)line_position - 3500;
PV = kp * error;
if (PV > 55)
{
PV = 55;
}
if (PV < -55)
{
PV = -55;
}
m1Speed = 200 - PV;
m2Speed = 200 + PV;
digitalWrite(dir_a, LOW);
analogWrite(pwm_a, m1Speed);
digitalWrite(dir_b, LOW);
analogWrite(pwm_b, m2Speed);
break;
}
}