int LMotor=10;
int RMotor=9;
int val = 0;
int off = 0;
int reverse;
int forward;
int circle;
//int move;
int turn;
int aheadY;
int turnX;
void setup() {
pinMode (5,OUTPUT);
pinMode (6,OUTPUT);
pinMode (11,OUTPUT);
pinMode (12,OUTPUT);
pinMode(LMotor,OUTPUT);
pinMode(RMotor,OUTPUT);
pinMode(13,HIGH);
Serial.begin(9600);
}
void loop() {
reverse=pulseIn(4,HIGH);
aheadY =pulseIn(7,HIGH);
turnX = pulseIn(8,HIGH);
forward = map(aheadY, 1000,1882, 35, 255);
circle = map(turnX,1030,1889,-225,225);
turn = map(turnX, 1030,1889,-128,127);
turn = constrain(turn, -128,127);
if (circle < 0){
circle = -1 *circle;
}
if (reverse <1200 )
{
digitalWrite (5,LOW);
delay(1);
digitalWrite(6,HIGH);
delay(1);
digitalWrite (11,LOW);
delay(1);
digitalWrite(12,HIGH);
delay(1);
digitalWrite(13,LOW);
}
if (reverse>1200){
digitalWrite (6,LOW);
delay(1);
digitalWrite(5,HIGH);
delay(1);
digitalWrite (12,LOW);
delay(1);
digitalWrite(11,HIGH);
delay(1);
digitalWrite(13,HIGH);
}
if (forward < 60 && turn < -15){
digitalWrite (6,LOW);
delay(1);
digitalWrite(5,HIGH); //left rotaion
delay(1);
digitalWrite (11,LOW);
delay(1);
digitalWrite(12,HIGH);
analogWrite (LMotor,circle);
analogWrite (RMotor,circle);
Serial.print(" L rota");
}
if (forward < 60 && turn > 15){
digitalWrite (5,LOW);
delay(1);
digitalWrite(6,HIGH); //right rotaion
delay(1);
digitalWrite (12,LOW);
delay(1);
digitalWrite(11,HIGH);
analogWrite (LMotor,circle);
analogWrite (RMotor,circle);
Serial.print(" R rot");
}
//Serial.print(" aheadY ");
// Serial.print(aheadY);
//Serial.print(" turn ");
// Serial.print(turnX);
Serial.print(" forward ");
Serial.print(forward);
Serial.print(" Turn ");
Serial.print(turn);
Serial.print(" circle ");
Serial.print(circle);
// Serial.println(reverse);
if (forward > 60 &&turn >=-16 && turn <=16) // forward
{
analogWrite (LMotor,forward);
analogWrite (RMotor,forward);
Serial.print(" go forward");
}
if (forward <= 60 && turn >=-16 && turn <=16 )
{
analogWrite (LMotor,0); // no effect
analogWrite (RMotor,0);
Serial.print(" stop");
}
if (turn <=-15 && forward > 60) // forward left
{
analogWrite (LMotor,forward );
analogWrite (RMotor,0);
Serial.print(" forward left");
}
if (turn >=15 && forward > 60) // forward right
{
analogWrite (LMotor,0);
analogWrite (RMotor,forward );
Serial.print(" forward left");
}
Serial.println(" ");
}