// Моторы подключаются к клеммам M1+,M1-,M2+,M2-  
// Motor shield использует четыре контакта 6,5,7,4 для управления моторами  
#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 35
   
// Коэффициент, задающий во сколько раз нужно затормозить
// одно из колёс для поворота
#define BRAKE_K 4
   
#define STATE_FORWARD 0
#define STATE_RIGHT 1
#define STATE_LEFT 2
   
int state = STATE_FORWARD;
   
void runForward()  
{
  state = STATE_FORWARD;
   
  // Для регулировки скорости `SPEED` может принимать значения от 0 до 255,
  // чем болше, тем быстрее.  
  analogWrite(SPEED_LEFT, SPEED);
  analogWrite(SPEED_RIGHT, SPEED);
   
  // Если в DIR_LEFT или DIR_RIGHT пишем HIGH, мотор будет двигать соответствующее колесо
  // вперёд, если LOW - назад.
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, HIGH);
}
   
void steerRight()  
{
  state = STATE_RIGHT;
   
  // Замедляем правое колесо относительно левого,
  // чтобы начать поворот
  analogWrite(SPEED_RIGHT, SPEED / BRAKE_K);
  analogWrite(SPEED_LEFT, SPEED);
   
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, HIGH);
}
   
void steerLeft()  
{
  state = STATE_LEFT;
   
  analogWrite(SPEED_LEFT, SPEED / BRAKE_K);
  analogWrite(SPEED_RIGHT, SPEED);
   
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, HIGH);
}
   
   
void setup()  
{
  // Настраивает выводы платы 4,5,6,7 на вывод сигналов  
  for(int i = 4; i <= 7; i++)
  pinMode(i, OUTPUT);
   
  // Сразу едем вперёд
  runForward();
}  
   
void loop()  
{  
  // Наш робот ездит по белому полю с чёрным треком. В обратном случае не нужно
  // инвертировать значения с датчиков
  boolean left = !digitalRead(LEFT_SENSOR_PIN);
  boolean right = !digitalRead(RIGHT_SENSOR_PIN);
   
  // В какое состояние нужно перейти?
  int targetState;
   
  if (left == right) {
  // под сенсорами всё белое или всё чёрное
  // едем вперёд
  targetState = STATE_FORWARD;
  } else if (left) {
  // левый сенсор упёрся в трек
  // поворачиваем налево
  targetState = STATE_LEFT;
  } else {
  targetState = STATE_RIGHT;
  }
   
  if (state == targetState) {
  // мы уже делаём всё что нужно,
  // делаем измерения заново
  return;
  }
   
  switch (targetState) {
  case STATE_FORWARD:
  runForward();
  break;
   
  case STATE_RIGHT:
  steerRight();
  break;
   
  case STATE_LEFT:
  steerLeft();
  break;
  }
   
  // не позволяем сильно вилять на прямой
  delay(50);
}

#define LEFT_SENSOR_PIN 8
#define RIGHT_SENSOR_PIN 9
#define SPEED_LEFT 6
#define SPEED_RIGHT 5  
#define DIR_LEFT 7
#define DIR_RIGHT 4
   
// Для того чтобы убедиться, что именно тормозной путь долог, а не команда остановиться  
// приходит слишком поздно, будем включать светодиод, когда отдаётся команда.
#define LED_PIN 13
   
int currSpeed = 40;
void setup()
{
  for(int i = 4; i <= 7; ++i)
  pinMode(i, OUTPUT);
   
  analogWrite(SPEED_RIGHT, currSpeed);
  digitalWrite(DIR_RIGHT, HIGH);
   
  analogWrite(SPEED_LEFT, currSpeed);
  digitalWrite(DIR_LEFT, HIGH);  
   
  pinMode(LED_PIN, OUTPUT);
}
   
void loop()
{
  if (currSpeed > 120)
  return;
   
  boolean white[] = {
  !digitalRead(LEFT_SENSOR_PIN),  
  !digitalRead(RIGHT_SENSOR_PIN)
  };
   
  if (white[0] && white[1]) {
  // едем пока не упрёмся
  return;
  }
   
  // зажигаем светодиод, останавливаем моторы
  // и наблюдаем
  digitalWrite(LED_PIN, HIGH);
  analogWrite(SPEED_RIGHT, 0);
  analogWrite(SPEED_LEFT, 0);
  delay(5000);
   
  // повторяем эксперимент, увеличивая скорость
  // на 10 пунктов
  currSpeed += 10;
  if (currSpeed > 120)
  return;
   
  digitalWrite(LED_PIN, LOW);
  analogWrite(SPEED_RIGHT, currSpeed);
  analogWrite(SPEED_LEFT, currSpeed);  
}

#define SPEED_LEFT 6
#define SPEED_RIGHT 5  
#define DIR_LEFT 7
#define DIR_RIGHT 4
   
void go(int speed, bool reverseLeft, bool reverseRight, int duration)
{
  analogWrite(SPEED_LEFT, speed);
  analogWrite(SPEED_RIGHT, speed);
  digitalWrite(DIR_LEFT, reverseLeft ? LOW : HIGH);  
  digitalWrite(DIR_RIGHT, reverseRight ? LOW : HIGH);  
  delay(duration);  
}
   
void setup()  
{
  for(int i = 4; i <= 7; ++i)
  pinMode(i, OUTPUT);
}  
   
void loop()  
{  
  // Задержка 5 секунд после включения питания  
  delay(5000);  
   
  for (int i = 200; i <= 1000; i += 100) {
  // Несколько сотен мс вперёд  
  go(50, false, false, 200);
  go(0, false, false, 0);
   
  // Задержка 5 секунд
  delay(5000);  
  }
   
  // Остановка до ресета или выключения питания  
  go(0, false, false, 0);
   
  // Приехали
  while (true)
  ;  
}

// Моторы подключаются к клеммам M1+,M1-,M2+,M2-  
// Motor shield использует четыре контакта 6,5,7,4 для управления моторами  
#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 2
   
#define FAST_TIME_THRESHOLD 500
   
int state = STATE_FORWARD;
int currentSpeed = SPEED;
int fastTime = 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, HIGH);
  digitalWrite(DIR_RIGHT, HIGH);
}
   
void steerRight()  
{
  state = STATE_RIGHT;
  fastTime = 0;
   
  // Замедляем правое колесо относительно левого,
  // чтобы начать поворот
  analogWrite(SPEED_RIGHT, 0);
  analogWrite(SPEED_LEFT, SPEED);
   
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, HIGH);
}
   
void steerLeft()  
{
  state = STATE_LEFT;
  fastTime = 0;
   
  analogWrite(SPEED_LEFT, 0);
  analogWrite(SPEED_RIGHT, SPEED);
   
  digitalWrite(DIR_LEFT, HIGH);
  digitalWrite(DIR_RIGHT, HIGH);
}
   
   
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, LOW);
  digitalWrite(DIR_LEFT, LOW);
   
  delay(duration);
}
   
   
void setup()  
{
  // Настраивает выводы платы 4,5,6,7 на вывод сигналов  
  for(int i = 4; i <= 7; i++)
  pinMode(i, OUTPUT);
   
  // Сразу едем вперёд
  runForward();
}  
   
void loop()  
{  
  // Наш робот ездит по белому полю с чёрным треком. В обратном случае не нужно
  // инвертировать значения с датчиков
  boolean left = !digitalRead(LEFT_SENSOR_PIN);
  boolean right = !digitalRead(RIGHT_SENSOR_PIN);
   
  // В какое состояние нужно перейти?
  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:
  runForward();
  break;
   
  case STATE_RIGHT:
  steerRight();
  break;
   
  case STATE_LEFT:
  steerLeft();
  break;
  }
   
}