the full code is here
#include <QTRSensors.h>
#include <SoftwareSerial.h>
#include <EEPROM.h>
int P, D, value_send, line, max_speed, s_left, s_right, minim, use_break;
int last_pos, diff, derivative, pos;
int menu = 0;
int remote_stop = 0;
int mleft = 3; //PWM control for motor outputs 1 and 2 is on digital pin 3
int mright = 11; //PWM control for motor outputs 3 and 4 is on digital pin 11
byte remote;
QTRSensorsAnalog qtra((unsigned char[]) {0, 1, 2, 3, 4, 5}, 6);
unsigned int sensors[6];
void setup()
{
TCCR2B = (TCCR2B & 0xF8) | 8; //4 is default
P = EEPROM.read(1);
D = EEPROM.read(2);
max_speed = EEPROM.read(3);
value_send = EEPROM.read(4);
use_break = EEPROM.read(5);
minim = EEPROM.read(6);
minim = -1*minim;
pinMode(mleft, OUTPUT);
pinMode(mright, OUTPUT);
pinMode(12, OUTPUT);
pinMode(13, OUTPUT);
pinMode(2, OUTPUT);
Serial.begin(9600);Serial.println();Serial.println();
Serial.print("!Welcome to INSANITY PI Human machine interface! ");Serial.println();Serial.println();
Serial.print("Booting INSANITY PI...");Serial.println();delay(250);
Serial.print("Please be patient...");delay(250);Serial.println();
Serial.print("Waiting Bluetooth connection stabilisation... ");delay(500);Serial.print("OK");Serial.println();
Serial.print("Database loading... ");delay(250);Serial.print("OK");Serial.println();
Serial.print("Accesing database... ");delay(125);Serial.print("OK");Serial.println();
Serial.print("Reading stored values... ");delay(125);Serial.print("OK");Serial.println();
Serial.print("Restoring PID values... ");delay(125);Serial.print("OK");Serial.println();
Serial.print("Restoring speed values... ");delay(125);Serial.print("OK");Serial.println();delay(125);
Serial.print("Stored values of PID are as following:");Serial.println();
show_data();Serial.begin(9600);
Serial.print("Insanity PI is now ONLINE");Serial.println();Serial.println();
Serial.print("Send any key to continue... ");
while(Serial.available()==0);
Serial.println();Serial.println();
Serial.end();
while(menu!=10)
{
switch(menu)
{
case 0:
Serial.begin(9600);
Serial.print("Please select options: ");Serial.println();
Serial.print(" 1. Configure PID");Serial.println();
Serial.print(" 2. Race track");Serial.println();
Serial.print(" 3. Calibrate Sensors");Serial.println();
Serial.print(" 4. Set maximum speed");Serial.println();
Serial.print(" 5. Show database values");Serial.println();
Serial.print(" 6. USE/NOT USE breaking system");Serial.println();
Serial.print(" 7. Enable/Disable DEBUG MODE");Serial.println();
Serial.print(" 8. Set breaker speed");Serial.println();
if(value_send == 1)
{
Serial.print(" 9. Race in DEBUG mode");Serial.println();
}
Serial.println();
while(Serial.available()==0);
menu = Serial.parseInt();
Serial.end();
break;
case 1:
pid_set();
menu = 0;
break;
case 2:
menu = 10;
break;
case 3:
Serial.begin(9600);
Serial.print("Calibration starts in... 3... ");delay(1000);
Serial.print("2... ");delay(1000);
Serial.print("1... ");delay(1000);Serial.println();
Serial.print("Calibration started!");Serial.println();
int i;
for (int i = 0; i <= 80; i++)
{
if(i < 20 || i >= 60)
{
go(60,-60);
}
else
{
go(-60,60);
}
qtra.calibrate();
}
go(0,0);
Serial.print("Calibration done succesfully!");Serial.println();
Serial.print("Minimum treshold for each sensor is: ");Serial.println();
for (i = 0; i < 6; i++)
{
Serial.print(qtra.calibratedMinimumOn[i]);Serial.print("\t");
}
Serial.println();
Serial.print("Maximum treshold for each sensor is: ");Serial.println();
for (i = 0; i < 6; i++)
{
Serial.print(qtra.calibratedMaximumOn[i]);Serial.print("\t");
}
Serial.println();Serial.println();
Serial.end();
menu = 0;
break;
case 4:
Serial.begin(9600);
Serial.print("Actual maximum speed is: ");Serial.print(max_speed);Serial.println();
Serial.print("Please input maximum: ");
while(Serial.available()==0);
max_speed = Serial.parseInt();
Serial.print(max_speed);
EEPROM.write(3,max_speed);
Serial.println();
Serial.end();
menu = 0;
break;
case 5:
show_data();
Serial.println();Serial.println();
Serial.end();
menu = 0;
break;
case 6:
Serial.begin(9600);
Serial.print("Currently ");
if(use_break==0)
{
Serial.print("NOT ");
}
Serial.print("USING breaking system!");Serial.println();
Serial.print("Please input 0(OFF)/1(ON): ");
while(Serial.available()==0);
use_break = Serial.parseInt();
EEPROM.write(5, use_break);
if(use_break == 1)
{
minim = EEPROM.read(6);
minim = -minim;
Serial.println();Serial.print("Breaking system has been enabled");Serial.println();
}
else
{
Serial.println();Serial.print("Breaking system has been disabled");Serial.println();
minim = 0;
}
menu = 0;
break;
case 7:
Serial.begin(9600);
Serial.print("To recieve actual position and speed informations send 0(OFF)/1(ON): ");
while(Serial.available()==0);
value_send = Serial.parseInt();
EEPROM.write(4,value_send);
Serial.println();
Serial.print("DEBUG MODE HAS BEEN ");
if(value_send==0)
{
Serial.print("DISABLED");
}
else
{
Serial.print("ENABLED");
}
Serial.println();Serial.println();
menu = 0;
Serial.end();
break;
case 8:
Serial.begin(9600);
Serial.print("Breaking speed is: ");Serial.print(minim);Serial.println();
Serial.print("Please input breaking speed: ");
while(Serial.available()==0);
minim = Serial.parseInt();
EEPROM.write(6,minim);
minim = -1*minim;
Serial.print(minim);
Serial.println();
Serial.end();
menu = 0;
break;
case 9:
Serial.begin(9600);
value_send = 1;
menu = 10;
break;
}
}
digitalWrite(12, LOW);
digitalWrite(13, HIGH);
analogWrite(mleft, max_speed);
analogWrite(mright, max_speed);
}
void loop()
{
while(remote_stop==1)
{
Serial.print("STOPPED");Serial.println();
Serial.print("Please reset INSANITY PI!");Serial.println();Serial.println();
go(-max_speed, -max_speed);
delay(5);
go(0, 0);
while(1);
}
line = qtra.readLine(sensors);
pos = line-2500;
derivative = pos - last_pos;
last_pos = pos;
diff = pos/P + derivative*D/2;
if(diff<0)
{
s_left = max_speed;
s_right = max_speed + diff;
}
else
{
s_left = max_speed - diff;
s_right = max_speed;
}
s_left = constrain(s_left, minim, max_speed);
s_right = constrain(s_right, minim, max_speed);
go(s_left, s_right);
if(value_send==1)
{
Serial.print(pos);Serial.print("\t\t");Serial.print(s_left);Serial.print("\t\t");Serial.print(s_right);Serial.println();
}
}
void pid_set()
{
Serial.end();Serial.begin(9600);
Serial.println();
Serial.print("Previsious data for PID were:");Serial.println();
Serial.print("kP: ");Serial.print(P);Serial.print(" kD: ");Serial.print(D);Serial.println();
Serial.print("Setting up for SPEED track... ");Serial.println();
Serial.print(" kP = ");
while(Serial.available()==0);
P = Serial.parseInt();
Serial.print(P);
EEPROM.write(1,P);
Serial.end();
Serial.begin(9600);
Serial.print(" kD = ");
while(Serial.available()==0);
D = Serial.parseInt();
Serial.print(D);
EEPROM.write(2, D);
Serial.println();Serial.println();
Serial.end();
menu = 0;
}
void go(int speedLeft, int speedRight)
{
if (speedLeft > 0)
{
digitalWrite(12, LOW);
analogWrite(mleft, speedLeft);
}
else
{
digitalWrite(12, HIGH);
analogWrite(mleft, -speedLeft);
}
if (speedRight > 0)
{
digitalWrite(13, HIGH);
analogWrite(mright, speedRight);
}
else
{
digitalWrite(13, LOW);
analogWrite(mright, -speedRight);
}
}
void show_data()
{
Serial.begin(9600);
Serial.print("PID: ");Serial.print("\t\t");Serial.print("kP:");Serial.print(P);Serial.print("\t\t");Serial.print("kD:");Serial.print(D);Serial.print("\t\t");Serial.print("speed:");Serial.print(max_speed);Serial.println();
if(use_break==0)
{
Serial.print("NOT ");
}
Serial.print("USING breaking system");
if(use_break==1)
{
Serial.print(" with breaker speed of ");Serial.print(minim);Serial.println();
}
Serial.println();
Serial.print("DEBUG mode is ");
if(value_send==0)
{
Serial.print("DISABLED");
}
else
{
Serial.print("ENABLED");
}
Serial.println();Serial.println();
Serial.end();
}
void serialEvent()
{
remote = Serial.read();
if(remote == 's');
remote_stop=1;
}
and right now is working… got 4th place in a competition…