line follow robot - code

This is a code I wrote in Processing for a line follow robot

I used processing https://processing.org
an arduino board and a robotkit
here you can see the robot "in action":

This is the code:

const int motor1Pin = 9; //
const int motor2Pin = 11; //
int sensorPin2 = 2; //sensor 1
int sensorPin3 = 3; //sensor 2
int sensorPin4 = 4; //sensor 3
int sensorPin5 = 5; //sensor 4 // 220 or 1k resistor connected to this pin
int sDigi2=0; //for each sensor
int sDigi3=0;
int sDigi4=0;
int sDigi5=0;
int x1=0;
int x2=100;
int x3=150;
int x4=200;
int x5=250
;
long result = 0;
void setup() //
{
Serial.begin(9600);
Serial.println("start"); //
}
void loop() //
{
Serial.println( "-----------------");
Serial.println( "-- sensor 1 ----");
Serial.println( RCtime(sensorPin2) );
Serial.println( "-- sensor 2 ----");
Serial.println( RCtime(sensorPin3) );
Serial.println( "-- sensor 3 ----");
Serial.println( RCtime(sensorPin4) );
Serial.println( "-- sensor 4 ----");
Serial.println( RCtime(sensorPin5) );
Serial.println( "-----------------");
// delay(5000);
//--------------------- if for digital assignment sDigi2 -----------------------------
if(RCtime(sensorPin2) > 50) //black
{
sDigi2 = 1;
}
if(RCtime(sensorPin2) < 50) //white
{
sDigi2 = 0;
}
//--------------------- if for digital assignment sDigi3 -----------------------------
if(RCtime(sensorPin3) > 50) //black
{
sDigi3 = 1;
}

if(RCtime(sensorPin3) < 50) //white
{
sDigi3 = 0;
}
//--------------------- if for digital assignment sDigi4 -----------------------------
if(RCtime(sensorPin4) > 50) //black
{
sDigi4 = 1;
}
if(RCtime(sensorPin4) < 50) //white
{
sDigi4 = 0;
}
//--------------------- if for digital assignment Digi5 -----------------------------
if(RCtime(sensorPin5) > 50) //black
{
sDigi5 = 1;
}
if(RCtime(sensorPin5) < 50) //white
{
sDigi5 = 0;
}
//------------------- 0000--------------------------------------
if ((sDigi2 == 0) && (sDigi3 == 0) && (sDigi4 == 0) && (sDigi5 == 0))
{
analogWrite(motor1Pin, x1);
analogWrite(motor2Pin, x1);
}
//------------------- 1000--------------------------------------
if ((sDigi2 == 1) && (sDigi3 == 0) && (sDigi4 == 0) && (sDigi5 == 0))
{
analogWrite(motor1Pin, x5);
analogWrite(motor2Pin, x2);
}
//------------------- 1100--------------------------------------
if ((sDigi2 == 1) && (sDigi3 == 1) && (sDigi4 == 0) && (sDigi5 == 0))
{
analogWrite(motor1Pin, x5);
analogWrite(motor2Pin, x3);
}
//------------------- 0110--------------------------------------
if ((sDigi2 == 0) && (sDigi3 == 1) && (sDigi4 == 1) && (sDigi5 == 0))
{

analogWrite(motor1Pin, x2);
analogWrite(motor2Pin, x2);
}
//------------------- 0011--------------------------------------
if ((sDigi2 == 0) && (sDigi3 == 0) && (sDigi4 == 1) && (sDigi5 == 1))
{
analogWrite(motor1Pin, x3);
analogWrite(motor2Pin, x5);
}
//------------------- 0001--------------------------------------
if ((sDigi2 == 0) && (sDigi3 == 0) && (sDigi4 == 0) && (sDigi5 == 1))
{
analogWrite(motor1Pin, x2);
analogWrite(motor2Pin, x5);
}
else Serial.println( "-- wrong assignment ----");
}
long RCtime(int sensPin)
{
long result = 0;
pinMode(sensPin, OUTPUT); // make pin OUTPUT
digitalWrite(sensPin, HIGH); // make pin HIGH to discharge capacitor - study the
schematic
delay(1); // wait a ms to make sure cap is discharged
pinMode(sensPin, INPUT); // turn pin into an input and time till pin goes low
digitalWrite(sensPin, LOW); // turn pullups off - or it won't work
while(digitalRead(sensPin))
{ // wait for pin to go low
result++;
}
return result; // report results
}