Arduino.ru

Что не так сделали, робот уборщик не работает.

1. DFrduino Romeo 1.1

2. Дальномер Sharp 2Y0A22 пин 0

3. ИК датчик линии аналоговые 2 шт пин 1, 2

4. сервоприводы 4 шт.

5. Моторы

#include <Servo.h> 
 Servo myservo0;     //  назначаем вывод 1 сервы
 Servo myservo1;     //   назначаем вывод 2 сервы
 Servo myservo2;     //  назначаем вывод 3 сервы

// Моторы подключаются к клеммам 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 SENSOR_PIN  1
 int i; 
 int j; 
int p[18]; // массив, здесь будем записывать данные с ИК дальномера   
int gp,gp1,gp2,g;
int b; 
int pos = 0;
// Скорость, с которой мы движемся вперёд (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 STATE_BACK       3

#define SPEED_STEP       2

#define FAST_TIME_THRESHOLD     500

int state = STATE_FORWARD;
int currentSpeed = SPEED;
int fastTime = 0;
 
void Forward() 
{
    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 Spin_Right() 
{
    state = STATE_RIGHT;
    fastTime = 0;

    // Замедляем правое колесо относительно левого,
    // чтобы начать поворот
    analogWrite(SPEED_RIGHT, 0);
    analogWrite(SPEED_LEFT, SPEED);

    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
}

void Spin_Left() 
{
    state = STATE_LEFT;
    fastTime = 0;

    analogWrite(SPEED_LEFT, 0);
    analogWrite(SPEED_RIGHT, SPEED);

    digitalWrite(DIR_LEFT, HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
}
void Backward() 
{
    state = STATE_BACK;
    fastTime = 0;

    analogWrite(SPEED_LEFT, SPEED);
    analogWrite(SPEED_RIGHT, SPEED);

    digitalWrite(DIR_LEFT, LOW);
    digitalWrite(DIR_RIGHT, LOW);
}

void motorStop(){ // Подпрограмма остановки мотора 

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

 digitalWrite(DIR_LEFT,LOW); 
 digitalWrite(DIR_RIGHT,LOW); 
 } 

void setup() 
{
  // устанавливаем пин как вывод управления сервой 
  myservo0.attach(6);  // назначаем порты для 1 сервы 
  myservo1.attach(6);  // назначаем порты для 2 сервы
  myservo2.attach(5);  // назначаем порты для 3 сервы
  
  // Настраивает выводы платы 4,5,6,7 на вывод сигналов 
    for(int i = 4; i <= 7; i++)
        pinMode(i, OUTPUT);

        
} 

void loop() 
{ 
    myservo1.write(15); // раскрываем лапки 
   delay(100);   
   myservo2.write(160); // раскрываем лапки 
   delay(100);     
 myservo0.write(0); 
 delay(200); 
 j=0;    
 for (j=0;j<18;j++){  // для j от 0 до 18 
 myservo0.write(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); 
     
     myservo0.write(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); 
   } 
   myservo0.write(0); // убираем голову в сторону, чтобы она не мешала захвату предмета и транспортировке 
   delay(100); 
   motorRun();    
   Forward();    // движение до объекта 
   delay (700); // время движения 
   motorStop(); 
     
   myservo1.write(140); // захват первой лапой 
   delay(500);   
   myservo2.write(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();} 
        
     myservo1.write(15); // раскрываем лапки 
     delay(100);   
     myservo2.write(160); // раскрываем лапки 
     delay(100);       
      
     //digitalWrite(ledPin, HIGH); // нажимаем кнопку стоп 
     //delay(150); 
     //digitalWrite(ledPin, LOW); // отпускаем кнопку стоп 
     //delay(100); 
      
     motorRun(); 
     Backward(); 
     delay(100); 
     Spin_Right(); 
     delay(600); 
   } 
   } 

}