Arduino.ru

Привет всем. Нужна помощь в создании робота. 

Задача робота: Робот должен ехать по прямой линии до объекта. После того как он увидел перед собой ультразвуковым дальномером объект (на расстоянии < 3см), робот должен использовать сервопривод для захвата объекта. После захвата объекта робот возвращается на базу (база вся чёрная) и потом датчиком линии который стоит на подъёмном механизме определяет цвет объекта (чёрный или белый) и в зависимости от цвета ставить объек на лево или на право относительно себя.

#include <Servo.h> 
Servo servo1; 
byte my_speed = 140; 
int delta = 80; 
int cm, duration; 
void setup() { 
pinMode(4, OUTPUT); //выход мотора A1 
pinMode(5, OUTPUT); //выход мотора A2 
pinMode(6, OUTPUT); //выход мотора B1 
pinMode(7, OUTPUT); //выход мотора B2 
servo1.attach(3); // привязываем сервопривод к аналоговому выходу 3
//pinMode(13, OUTPUT); //питание датчика 1 
pinMode(8, INPUT); //сигнал датчика левый 
pinMode(9, INPUT); //сигнал датчика правый 
pinMode(10, INPUT); //сигнал датчика кубов 
//digitalWrite(13, HIGH); //включить датчик 2 
Serial.begin (9600); // подключаем монитор порта 
pinMode(11, OUTPUT); // назначаем trigPin (Pin11), как выход 
pinMode(12, INPUT); // назначаем echoPin (Pin12), как вход 
} 

void loop() { 
// ========== УЛЬТРАЗВУКОМ НАЧИНАЕМ ИЗМЕРЯТЬ РАССТОЯНИЕ 
analogWrite(11, LOW); 
delayMicroseconds(2); 
digitalWrite(11, HIGH); 
delayMicroseconds(10); 
digitalWrite(11, LOW); 
duration = pulseIn(12, HIGH); 
cm = duration / 58; 
Serial.println(cm); 
if (cm < 2) 
CUBE_UP(); // КОМАНДА ОСТАНОВКИ И ПОДНЯТИЯ ЗАХВАТА 
else if (!digitalRead(9) && digitalRead(8)) 
SPIN_RIGHT(); 
else if (!digitalRead(8) && digitalRead(9)) 
SPIN_LEFT(); 
else if (digitalRead(8) && digitalRead(9))
FORWARD();
} 

void FORWARD() { 
digitalWrite(4, LOW); 
digitalWrite(6, LOW); 
analogWrite(5, my_speed); 
analogWrite(7, my_speed); 
} 

void SPIN_LEFT() { 
digitalWrite(4, LOW); 
digitalWrite(6, LOW); 
analogWrite(5, my_speed - delta); 
analogWrite(7, my_speed + delta); 
} 

void SPIN_RIGHT() { 
digitalWrite(4, LOW); 
digitalWrite(6, LOW); 
analogWrite(5, my_speed + delta); 
analogWrite(7, my_speed - delta); 
} 
void CUBE_UP() { 
  if (cm < 2)
{ 
servo1.write(0); 
delay(1500); 
} 
else 
servo1.write(90); 
if (cm < 2){
digitalWrite(4, LOW); 
analogWrite(6, my_speed); 
analogWrite(5, my_speed); 
digitalWrite(7, LOW);   
}
if (!digitalRead(8) && !digitalRead(9) && !digitalRead(10))
{
digitalWrite(4, LOW); 
analogWrite(6, HIGH); 
digitalWrite(5, LOW); 
analogWrite(7, HIGH);
delay(500);
digitalWrite(4, LOW); 
digitalWrite(6, LOW); 
analogWrite(5, HIGH); 
analogWrite(7, HIGH);
delay(750);
servo1.write(90);
digitalWrite(4, LOW); 
analogWrite(6, HIGH); 
analogWrite(5, HIGH); 
digitalWrite(7, LOW);
delay(750);
analogWrite(4, HIGH); 
digitalWrite(6, LOW); 
analogWrite(5, HIGH); 
digitalWrite(7, LOW);
}
else if (!digitalRead(8) && !digitalRead(9) && digitalRead(10))
{
analogWrite(4, HIGH); 
digitalWrite(6, LOW); 
analogWrite(5, HIGH); 
digitalWrite(7, LOW);
delay(500);
digitalWrite(4, LOW); 
digitalWrite(7, LOW); 
analogWrite(5, HIGH); 
analogWrite(6, HIGH);
delay(750);
servo1.write(90);
digitalWrite(4, LOW); 
analogWrite(6, HIGH); 
analogWrite(5, HIGH); 
digitalWrite(7, LOW);
delay(750);
analogWrite(6, HIGH); 
digitalWrite(4, LOW); 
analogWrite(7, HIGH); 
digitalWrite(5, LOW);
}
}