Arduino.ru

#define SPEED_LEFT       6
#define SPEED_RIGHT      5 
#define DIR_LEFT         7
#define DIR_RIGHT        4
#define LEFT_SENSOR_PIN  8
#define RIGHT_SENSOR_PIN 9
 
// Скорость, с которой мы движемся вперёд (0-255)
#define SPEED            100
 
// Скорость прохождения сложных участков
#define SLOW_SPEED       35
 
#define BACK_SLOW_SPEED  30
#define BACK_FAST_SPEED  50
 
// Коэффициент, задающий во сколько раз нужно затормозить
// одно из колёс для поворота
#define BRAKE_K          4
 
#define STATE_FORWARD    0
#define STATE_RIGHT      1
#define STATE_LEFT       2
 
#define SPEED_STEP       7
 
#define FAST_TIME_THRESHOLD     75
 
int state = STATE_FORWARD;
int currentSpeed = SPEED;
int fastTime = 0;

//моя вставка
int sensorPin1 = A2;    // select the input pin for the potentiometer
int sensorPin2 = A3;
int sensorPin3 = A4;
int sensorPin4 = A5;
int sensorPin5 = A6;    // select the input pin for the potentiometer
int sensorPin6 = A7;
int sensorPin7 = A0;
int sensorPin8 = A1;
int ledPin = 13;      // select the pin for the LED
int sensorValue1 = 0;  // variable to store the value coming from the sensor
int sensorValue2 = 0;
int sensorValue3 = 0;
int sensorValue4 = 0;
int sensorValue5 = 0;  // variable to store the value coming from the sensor
int sensorValue6 = 0;
int sensorValue7 = 0;
int sensorValue8 = 0;
int qtr1 = 0;
int qtr2 = 0;
int qtr3 = 0;
int qtr4 = 0;
int qtr5 = 0;
int qtr6 = 0;
int qtr7 = 0;
int qtr8 = 0;

int qtrposition = 0;
int qtrpositionERROR = 0;
int label = 0; 
int L = 0;
int R = 0;
int stat=0;
// конец моей вставки




 
void runForward() 
{
    state = STATE_FORWARD;
 
    fastTime += 1;
    if (fastTime < FAST_TIME_THRESHOLD) {
        currentSpeed = SLOW_SPEED;
    } else {
        currentSpeed = min(currentSpeed + SPEED_STEP, SPEED);
    }
 
    analogWrite(SPEED_LEFT, currentSpeed);
    analogWrite(SPEED_RIGHT, currentSpeed);
 
    digitalWrite(DIR_LEFT, LOW);
    digitalWrite(DIR_RIGHT, LOW);
}
 
void steerRight() 
{
    state = STATE_RIGHT;
    fastTime = 0;
 
    // Замедляем правое колесо относительно левого,
    // чтобы начать поворот
    analogWrite(SPEED_RIGHT, 0);
    analogWrite(SPEED_LEFT, SPEED);
 
    digitalWrite(DIR_LEFT, LOW);
    digitalWrite(DIR_RIGHT, LOW);
}
 
void steerLeft() 
{
    state = STATE_LEFT;
    fastTime = 0;
 
    analogWrite(SPEED_LEFT, 0);
    analogWrite(SPEED_RIGHT, SPEED);
 
    digitalWrite(DIR_LEFT, LOW);
    digitalWrite(DIR_RIGHT, LOW);
}
 
 
void stepBack(int duration, int state) {
    if (!duration)
        return;
 
    // В зависимости от направления поворота при движении назад будем
    // делать небольшой разворот 
    int leftSpeed = (state == STATE_RIGHT) ? BACK_SLOW_SPEED : BACK_FAST_SPEED;
    int rightSpeed = (state == STATE_LEFT) ? BACK_SLOW_SPEED : BACK_FAST_SPEED;
 
    analogWrite(SPEED_LEFT, leftSpeed);
    analogWrite(SPEED_RIGHT, rightSpeed);
 
    // реверс колёс
    digitalWrite(DIR_RIGHT, HIGH);
    digitalWrite(DIR_LEFT, HIGH);
 
    delay(duration);
}
 
 
void setup() 
{
    // Настраивает выводы платы 4,5,6,7 на вывод сигналов 
    for(int i = 4; i <= 7; i++)
        pinMode(i, OUTPUT);
 Serial.begin(9600);
    // Сразу едем вперёд
    runForward();
} 
 
void loop() 
{ 
    //моя вставка
sensorValue8 = analogRead(sensorPin8);   
 sensorValue1 = analogRead(sensorPin1);
 sensorValue2 = analogRead(sensorPin2);
 sensorValue3 = analogRead(sensorPin3);
 sensorValue4 = analogRead(sensorPin4);   
 sensorValue5 = analogRead(sensorPin5);
 sensorValue6 = analogRead(sensorPin6);
 sensorValue7 = analogRead(sensorPin7);

         // qtr1
  
 if (sensorValue1 < 250)      // qtr1
 
{
  qtr1 = 0; // действие A
 
}
if (sensorValue1 > 600)
{
   qtr1 = 1; // действие B
  
}

         // qtr2
 
 if (sensorValue2 < 250)      // qtr1
 
{
  qtr2 = 0; // действие A
 
}
if (sensorValue2 > 600)
{
   qtr2 = 1; // действие B
 
}

        // qtr3
 
 if (sensorValue3 < 250)      // qtr1
 
{
  qtr3 = 0; // действие A
 
}
if (sensorValue3 > 600)
{
   qtr3 = 1; // действие B
  
}

         // qtr4
 
 if (sensorValue4 < 250)      // qtr1
 
{
  qtr4 = 0; // действие A
 
}
if (sensorValue4 > 600)
{
   qtr4 = 1; // действие B
 
}


      // qtr5
  
 if (sensorValue5 < 250)      // qtr1
 
{
  qtr5 = 0; // действие A
 
}
if (sensorValue5 > 600)
{
   qtr5 = 1; // действие B
  
}

         // qtr6
 
 if (sensorValue6 < 250)      // qtr1
 
{
  qtr6 = 0; // действие A
 
}
if (sensorValue6 > 600)
{
   qtr6 = 1; // действие B
 
}

        // qtr7
 
 if (sensorValue7 < 250)      // qtr1
 
{
  qtr7 = 0; // действие A
 
}
if (sensorValue7 > 600)
{
   qtr7 = 1; // действие B
  
}

         // qtr8
 
 if (sensorValue8 < 250)      // qtr1
 
{
  qtr8 = 0; // действие A
 
}
if (sensorValue8 > 600)
{
   qtr8 = 1; // действие B
 
}
      
      
 
      
    // Наш робот ездит по белому полю с чёрным треком. В обратном случае не нужно
    // инвертировать значения с датчиков
 if (qtr8==0 ,qtr7==0 )
{
 L=0;
}
else 
{
  L=1;
}
 if (qtr2==0 ,qtr1==0 )
{
 R=0;
}
else 
{
  R=1;
}   
 
    
    
    boolean left = R;
    boolean right = L;
 
    // В какое состояние нужно перейти?
    int targetState;
 
    if (left == right) {
        // под сенсорами всё белое или всё чёрное
        // едем вперёд
        targetState = STATE_FORWARD;
    } else if (left) {
        // левый сенсор упёрся в трек
        // поворачиваем налево
        targetState = STATE_LEFT;
    } else {
        targetState = STATE_RIGHT;
    }
 
    if (state == STATE_FORWARD && targetState != STATE_FORWARD) {
        int brakeTime = (currentSpeed > SLOW_SPEED) ?
            currentSpeed : 0;
        stepBack(brakeTime, targetState);
    }
 
    switch (targetState) {
        case STATE_FORWARD:
        Serial.println("STATE_FORWARD");
            runForward();
            break;
 
        case STATE_RIGHT:
        Serial.println("STATE_RIGHT");
            steerRight();
            break;
 
        case STATE_LEFT:
        Serial.println("STATE_LEFT");
            steerLeft();
            break;
    }
Serial.print("L=");    
 Serial.print(L); 
Serial.print("     "); 
Serial.print("R=");
 Serial.println(R);
Serial.print("currentSpeed=");    
 Serial.println(currentSpeed); 
 Serial.print("fastTime=");
 Serial.println(fastTime);
 
 
 
 

 
 
 
 
 
 
 
 
 
 
 Serial.println(stat);

}

написал код для перемещения по черной линии, но на теперь на поворотах останавливаеться , и долго думает , что делать?