Arduino.ru

#include

//int ledPin = 13; // Кнопка включения мп3
//int ledPin1 = 12; // кнопка перемотки мп3
int i;
int j;
int p[18]; // массив, здесь будем записывать данные с ИК дальномера
int gp,gp1,gp2,g;
int b;
void setup() {
{
Serial.begin(9600);
lms.begin(LMS_SERVOS);
}
pinMode(ledPin,OUTPUT);
pinMode(ledPin1,OUTPUT);
pinMode(3,OUTPUT); // Motor A скорость
pinMode(7,OUTPUT); // Motor A направление
pinMode(11,OUTPUT); // Motor B скорость
pinMode(8,OUTPUT); // Motor B направление
pinMode(2,OUTPUT); // Motor A стоп, пуск
pinMode(4,OUTPUT); // Motor B стоп, пуск
}
void Forward() { // Подпрограмма движения робота вперед
digitalWrite(3,HIGH);
digitalWrite(7,LOW);
analogWrite(11,240);// здесь скоростью вращения колеса регулируем прямолинейность движения
digitalWrite(8,LOW);
}
void Backward(){ // Подпрограмма движения робота назад
digitalWrite(3,HIGH);
digitalWrite(7,HIGH);
digitalWrite(11,HIGH);
digitalWrite(8,HIGH);
}
void motorStop(){ // Подпрограмма остановки мотора

digitalWrite(4,HIGH);
digitalWrite(2,HIGH);
}
void motorRun(){ // Подпрограмма запуска мотора

digitalWrite(2,LOW);
digitalWrite(4,LOW);
}
void Spin_Left(){ // Вращение в лево
digitalWrite(11,HIGH);
digitalWrite(8,LOW);
digitalWrite(3,HIGH);
digitalWrite(7,HIGH);
}
void Spin_Right(){ // Вращение в право
digitalWrite(3,HIGH);
digitalWrite(7,LOW);
digitalWrite(11,HIGH);
digitalWrite(8,HIGH);
}
void loop()
{
lms.servoWrite(2,15); // раскрываем лапки
delay(100);
lms.servoWrite(4,160); // раскрываем лапки
delay(100);
lms.servoWrite(1,0);
delay(200);
j=0;
for (j=0;j<18;j++){ // для j от 0 до 18
lms.servoWrite(1,j*10); // вращение сервы по 10 градусов

gp=analogRead(0); // считываем данные с ИК радара
delay(50);
p[j]=gp; // записываем это значение в массив
Serial.println(p[j]); // выводим на экран

gp2=(analogRead(2)); // считываем значение датчиков плоскости
gp1=(analogRead(1));
g=min(gp1,gp2); // выбираем минимальное значение из двух
if (g<4){ // Если значение близко к 0, то отходим назад и разворачиваемся
motorRun();
Backward();
delay(500);
Spin_Left();
delay(600);
motorStop();
}

if (p[j]<160){ // если значение с дальномера меньше 150, (примерно 25 см.), то шаг вперед
motorRun();
Forward();
delay (50);
motorStop();}
else { // Иначе
digitalWrite(ledPin1, HIGH); // нажимаем кнопку перемотка
delay(100);
digitalWrite(ledPin1, LOW); // отпускаем кнопку перемотка
delay(100);

lms.servoWrite(1,90); // поворачиваем голову вперед
delay(200);

gp=analogRead(0); // считываем данные с ИК радара
Serial.println(gp);
while(gp<160){ // поворачиваемся, пока не заметим предмет
motorRun();
if (j>9){ // если предмет с лева, то
Spin_Left();// поворот в сторону объекта
}
else { // иначе поворот вправо
Spin_Right();
}
delay (10);
motorStop();
gp=analogRead(0); // считываем данные с ИК радара
delay(50);
}

gp=analogRead(0); // считываем данные с ИК радара
delay(50);
while(gp<400){ //Двигаемся вперед пока предмет не будет на расстоянии 10 см.
motorRun();
Forward(); // движение до объекта
delay (50); // время движения
motorStop();
gp=analogRead(0); // считываем данные с ИК радара
delay(50);
}
lms.servoWrite(1,0); // убираем голову в сторону, чтобы она не мешала захвату предмета и транспортировке
delay(100);
motorRun();
Forward(); // движение до объекта
delay (700); // время движения
motorStop();

lms.servoWrite(2,140); // захват первой лапой
delay(500);
lms.servoWrite(4,15); // захват второй лапой
delay(100);

gp2=(analogRead(2));
gp1=(analogRead(1));
g=min(gp1,gp2);
while(g>4) // движение после захвата до конца плоскости.
{
gp2=(analogRead(2));
gp1=(analogRead(1));
Serial.println(gp2);
g=min(gp1,gp2);
motorRun();
Forward();
delay(5);
motorStop();}

lms.servoWrite(2,15); // раскрываем лапки
delay(100);
lms.servoWrite(4,160); // раскрываем лапки
delay(100);

digitalWrite(ledPin, HIGH); // нажимаем кнопку стоп
delay(150);
digitalWrite(ledPin, LOW); // отпускаем кнопку стоп
delay(100);

motorRun();
Backward();
delay(100);
Spin_Right();
delay(600);
}
}
}