Arduino.ru

Господа, помогите правильно задать работу функции роботу. Все перепробовал, а нужного результата так и не получил. Необходимо, что бы робот по команде, начал движение по линии, до этого робот двигается под управление со смартфона. У меня получается, что робот просто едет вперед, когда я даю ему команду ехать по линии. 

int in1 = 9;
int in2 = 8;
int in3 = 7;
int in4 = 6;
int ENA = 10;
int ENB = 5;
int ABS = 130;
char getstr = 'S';

void _mForward(int a)
{
  digitalWrite(ENA, HIGH);
  digitalWrite(ENB, HIGH);
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  Serial.println("go forward!");
  delay (a * 50);
}
void _mBack(int b)
{
  digitalWrite(ENA, HIGH);
  digitalWrite(ENB, HIGH);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  Serial.println("go back!");
  delay (b * 50);
}
void _mleft(int c)
{
  analogWrite(ENA, ABS);
  analogWrite(ENB, ABS);
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  Serial.println("go left!");
  delay(c * 50);
}
void _mright(int d)
{
  analogWrite(ENA, ABS);
  analogWrite(ENB, ABS);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  Serial.println("go right!");
  delay (d * 50);
}
void _mStop(int s)
{
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  Serial.println("Stop!");
  delay (s * 50);
}
void _go(int g)
{
  int num1,num2,num3;
  num1=digitalRead(11);    
  num2=digitalRead(4);
  num3=digitalRead(2); 
   if((num1==0)&&num2&&num3) 
   {
    _mleft(3);          //The sensor detected that right car turn left immediately when it meets black line 
    delay(2);
    while(1){
    num2=digitalRead(2);         //Cycle to judge degree of intermediate sensor,
    if(num2==1)
    {  _mleft(3);                     //If num2==1 does not go to the middle position, continue to turn left.
     delay(2);}
     else
     break;                      //Detection of num2==0 instructions turned over, out of the loop, detection of three sensors’ statusand then make appropriate action
   }                                       //The following and so on
   }    

  else if(num2&&num1&&(num3==0))
   {
    _mright(3);
    delay(2);
    while(1)
    {
     num2=digitalRead(2);
    if(num2==1){
      _mright(3);  
     delay(2);}
     else
      break;
    }
   }
  else
  {
    _mForward(3); 
    delay(2);
  } 
  delay (g * 50);
}
void setup()
{
  Serial.begin(9600);
  pinMode (in1, OUTPUT);
  pinMode (in2, OUTPUT);
  pinMode (in3, OUTPUT);
  pinMode (in4, OUTPUT);
  pinMode (ENA, OUTPUT);
  pinMode (ENB, OUTPUT);
}

void loop() {

  if (Serial.available() > 0)// В случае если в "Серийном порту" есть информация
  {
    getstr = Serial.read(); // Считываем инвормацию с "Серийного порта"
    switch (getstr)// Отрабатывем в соотвествии с поступившей командой
    {
      case 'F':
        _mForward (3); // едем ВПЕРЕД
        break;
      case 'B':
        _mBack (3); // едем НАЗАД
        break;
      case 'R':
        _mright (3); // поворачиваем ВПРАВО
        break;
      case 'L':
        _mleft (3); // поворачиваем ВЛЕВО
        break;
      case 'V':
      _go(3);
      break;
      case 'S':
        _mStop (3);
    }
  }
}