Arduino.ru

Вечер добрый. Может кто сможет помочь с программой. Идея такая, что трехостный робокар должен проехать по прямой линии и остановиться через заданное расстояние. Написал кое какую программу которая выравнивает движение. В программировании не силен.

Первая проблема. Когда я подключаю ардуину к компу, заливаю скетч в котором указываю что если расстояние больше 100 см, то стоп. По монитору порта, благодаря данным поступающих со счетчиков импульсов, отслеживаю пройденное расстояние. Все работает просто отлично. Но когда я отключаюсь от пк, ставлю машинку на пол и включаю, она вместо 100 см проезжает 130-170. Причем очень частым является то, что после остановки машинка может рывком проехать еще некоторое расстояние. 

Вторая проблема возникает в части кода отвечающем за корректировку движения. Алгоритм следующий: сравниваем количество импульсов на правом и левом колесе. Если на правом колесе импульсов больше, то мы останавливаем его, тем самым левое колесо проезжает недостающее расстояние, и аналогично с левым колесом. Т.е. в теории должно произойти следующее: если мы рукой остановим одно из колес, второе тоже должно остановиться и сигнал должен подаваться только на то колесо, которое мы остановили. На деле происходит следующее: остановив левое колесо, правое колесо останавливается, но если остановить правое колесо, то левое продолжает крутиться, а так быть не должно. 

Мне кажется что все дело в моих дуратских циклах, но иначе у меня никак не получилось реализовать.

Если кто сможет, помогите подправить код.

Использую 

Arduino Mega 2560

Motor shield L293D

 

Код программы:

#include <AFMotor.h> // Подключаем библиотеку для управления двигателями 

//Создаем объекты для двигателей 
AF_DCMotor motor1(1); //канал М1 на Motor Shield — левый 
AF_DCMotor motor2(2); //канал М2 на Motor Shield — правый 

enum { ENC_PIN1 = 22, ENC_PIN2 = 23 }; // пины с энкодеров
// все во float потому что иначе неправильно считалось расстояние
float contL=0.00; // импульсы с левого колеса
float contP=0.00; // импульсы с правого колеса
float contS=0.00; //разница между количеством импульсов на колесах
float L;//расстояние пройденное левым колесом
float P;//расстояние пройденное правым колесом
float N=20.00; // число дырок на диске энкодера
boolean kflagL=LOW; 
boolean kflagP=LOW;

void setup() { 
 pinMode(ENC_PIN1, INPUT);
 digitalWrite(ENC_PIN1, HIGH);
 pinMode(ENC_PIN2, INPUT);
 digitalWrite(ENC_PIN2, HIGH);
 
 Serial.begin(9600);

go();}

void loop() { 

//************ Для левого колеса ***************
if (digitalRead(ENC_PIN1)==HIGH)
    {
     kflagL=HIGH;
    }
  
if(digitalRead(ENC_PIN1)==LOW && kflagL==HIGH)
{
  contL=contL+1;
     Serial.println("Dlya LEVOGO kolesa:");
     Serial.print(contL);
     L=(contL/N)*3.14*8;
     Serial.print("  |  ");   
     Serial.println(L);
     stop();
  kflagL=LOW;
}

//************* Для правого колеса ****************
if (digitalRead(ENC_PIN2)==HIGH)
    {
     kflagP=HIGH;
    }
  
if(digitalRead(ENC_PIN2)==LOW && kflagP==HIGH)
{
     contP=contP+1;
     Serial.println("Dlya PRAVOGO kolesa:");
     Serial.print(contP);
     P=(contP/N)*3.14*8;
     Serial.print("  |  ");   
     Serial.println(P);
     stop();
  kflagP=LOW;
}

//********************************************
// Часть кода отвечающая за стабилизацию движения
 
contS=contL-contP;
if (contS==0.00){ go();}
if (contS<0.00){right();} else {left;} 
if (contS>0.00){left();} else {right;}

//********************************************
}

void stop(){
  if(L>=100.00 or P>=100) //условие отвечающее за остановку через заданное расстояние
{
motor1.setSpeed(0); 
motor2.setSpeed(0);
motor1.run(RELEASE); 
motor2.run(RELEASE);
  }
}

void go(){
// Движение вперед 
motor1.run(FORWARD); 
motor2.run(FORWARD); 
//Устанавливаем скорость 100% (0-255) 
motor1.setSpeed(255); 
motor2.setSpeed(255); 
} 

void left(){
       motor1.setSpeed(0); 
       motor2.setSpeed(255);
}

void right(){ 
      motor1.setSpeed(255); 
      motor2.setSpeed(0);
}