i found this tutorial it's nearest thing for what i wanna do
#define motorL1 8
#define motorL2 9
#define motorR1 10
#define motorR2 11
#define PwmLeft 5
#define PwmRight 6
#define SensorR 2
#define SensorL 3
#define Sensor3 A0
#define Sensor4 A1
#define TableA A4
#define TableB A2
#define TableC A5
#define TableD A3
int OriginalSpeed = 200;
int TableCount = 0;
int TableCheck = 0;
int RFCheck = 10;
void setup()
{
Serial.begin (9600);
pinMode(motorR1, OUTPUT);
pinMode(motorR2, OUTPUT);
pinMode(motorL1, OUTPUT);
pinMode(motorL2, OUTPUT);
pinMode(PwmLeft, OUTPUT);
pinMode(PwmRight, OUTPUT);
pinMode(SensorL, INPUT);
pinMode(SensorR, INPUT);
pinMode(Sensor3, INPUT);
pinMode(Sensor4, INPUT);
pinMode(TableA, INPUT);
pinMode(TableB, INPUT);
pinMode(TableC, INPUT);
pinMode(TableD, INPUT);
MotorsStop();
analogWrite(PwmLeft, 0);
analogWrite(PwmRight, 0);
delay(2000);
// Serial.println("fghfg");
}
void loop() {
MotorsForward();
if((digitalRead(Sensor3) == LOW) && (TableCheck == 0)){TableCount++; TableCheck = 1;}
if((digitalRead(Sensor3) == HIGH) && (TableCheck == 1)){TableCheck = 2;}
if((digitalRead(Sensor3) == LOW) && (TableCheck == 2)){TableCount++; TableCheck = 3;}
if((digitalRead(Sensor3) == HIGH) && (TableCheck == 3)){TableCheck = 4;}
if((digitalRead(Sensor3) == LOW) && (TableCheck == 4)){TableCount++; TableCheck = 5;}
if((digitalRead(Sensor3) == HIGH) && (TableCheck == 5)){TableCheck = 0;}
if(digitalRead(TableA) == HIGH){RFCheck = 1;}
if(digitalRead(TableB) == HIGH){RFCheck = 2;}
if(digitalRead(TableC) == HIGH){RFCheck = 3;}
if(digitalRead(TableD) == HIGH){RFCheck = 4;}
if(RFCheck == TableCount){Table1();}
PIDController();
}
void MotorsBackward()
{
digitalWrite(motorL1, HIGH);
digitalWrite(motorL2, LOW);
digitalWrite(motorR1, HIGH);
digitalWrite(motorR2, LOW);
}
void MotorsForward()
{
digitalWrite(motorL1, LOW);
digitalWrite(motorL2, HIGH);
digitalWrite(motorR1, LOW);
digitalWrite(motorR2, HIGH);
}
void MotorsStop()
{
digitalWrite(motorL1, HIGH);
digitalWrite(motorL2, HIGH);
digitalWrite(motorR1, HIGH);
digitalWrite(motorR2, HIGH);
}
void MotorsLeft()
{
analogWrite(PwmLeft, 0);
analogWrite(PwmRight, 0);
digitalWrite(motorR1, HIGH);
digitalWrite(motorR2, HIGH);
digitalWrite(motorL1, LOW);
digitalWrite(motorL2, HIGH);
}
void MotorsRight()
{
analogWrite(PwmLeft, 0);
analogWrite(PwmRight, 0);
digitalWrite(motorR1, LOW);
digitalWrite(motorR2, HIGH);
digitalWrite(motorL1, HIGH);
digitalWrite(motorL2, HIGH);
}
void Motors180()
{
analogWrite(PwmLeft, 0);
analogWrite(PwmRight, 0);
digitalWrite(motorL1, HIGH);
digitalWrite(motorL2, LOW);
digitalWrite(motorR1, LOW);
digitalWrite(motorR2, HIGH);
}
void PIDController()
{
if(digitalRead(SensorL) == HIGH){analogWrite(PwmRight, 250);analogWrite(PwmLeft, 0);}
if(digitalRead(SensorR) == HIGH){analogWrite(PwmLeft, 250);analogWrite(PwmRight,0);}
if((digitalRead(SensorL) == LOW) && (digitalRead(SensorR) == LOW)){analogWrite(PwmRight, 0);analogWrite(PwmLeft, 0);}
}
void Table1()
{
TableCount = 0;
MotorsRight();
delay(4000);
while(digitalRead(SensorR) == HIGH);
while(digitalRead(SensorL) == HIGH);
while(1)
{
MotorsForward();
PIDController();
if(digitalRead(Sensor3) == LOW){break;}
}
MotorsStop();
delay(1000);
Motors180();
delay(2000);
while(digitalRead(Sensor3) == HIGH);
while(digitalRead(Sensor3) == LOW);
while(digitalRead(SensorL) == HIGH);
//delay(500);
while(1)
{
MotorsForward();
PIDController();
if(digitalRead(Sensor3) == LOW){break;}
}
//delay(1000);
MotorsLeft();
delay(4000);
while(digitalRead(SensorL) == HIGH);
while(digitalRead(SensorR) == LOW);
while(1)
{
MotorsForward();
PIDController();
if(digitalRead(Sensor3) == LOW){break;}
}
MotorsStop();
delay(1000);
}