LDR-робот

Сообщение **Mr.Kubikus** » 15 фев 2013, 12:12

Мы тут собрали робота, который умеет отслеживать источник света и объезжает препятствия, возникающие на пути во время движения к источнику света. Назвали его LDR-робот. LDR - это фоторезистор на английском.

: ldr-robot-000.JPG (52.49 КБ) 9264 просмотра

: ldr-robot-001.JPG (64.34 КБ) 9264 просмотра

: ldr-robot-10.JPG (40.13 КБ) 9264 просмотра

Программа для LDR-робота

Код: Выделить всё

#define SRV_PORT    5      // Port  number for servo
#define SRV_INITPOS 112    // Initial position for servo

#define MOTOR_LEFT_PWM   3 // Ports for motor control
#define MOTOR_RIGHT_PWM  11
#define MOTOR_LEFT_DIR   12 
#define MOTOR_RIGHT_DIR  13

#define DBWIDTH     10     // Deadband width for tracking controller
                           
#include <Servo.h> 
 
Servo myservo;  // Create servo object to control a servo 
                // a maximum of eight servo objects can be created 
 
int pos;        // variable to store the servo position 
 
void setup() 
{ 
  pos = SRV_INITPOS;
  
  pinMode(MOTOR_LEFT_PWM, OUTPUT);
  pinMode(MOTOR_RIGHT_PWM, OUTPUT);

  pinMode(MOTOR_LEFT_DIR, OUTPUT);
  pinMode(MOTOR_RIGHT_DIR, OUTPUT);

  digitalWrite(MOTOR_LEFT_DIR, HIGH);
  digitalWrite(MOTOR_RIGHT_DIR, HIGH);
    
  myservo.attach(SRV_PORT);    // Attaches the servo 
  myservo.write(SRV_INITPOS);  // Tell servo to go to init position
  
  delay(20);                   // Waits 15ms for the servo to reach the position 
  
  scanDistance();
} 
 
void loop() 
{ 
  
  avoidObstacle();                           // Scan for obstacles

  int sensor1Value = 1023 - analogRead(A3);  // Read sensor LDR Left
  int sensor2Value = 1023 - analogRead(A4);  // Read sensor LDR Right

  int diff = sensor1Value - sensor2Value;    // Calculate difference

    if (diff > DBWIDTH/2) {
      setMotorL(10);
      setMotorR(90);
    } else if (diff < -DBWIDTH/2) {
      setMotorL(90);
      setMotorR(10);
    } else {
      setMotorL(70);
      setMotorR(70);
    } 
  
  delay(10);
} 

void avoidObstacle() {
  int sensor3Value = 0;

  sensor3Value = analogRead(A2);  // Read distance sensor
 
  if (sensor3Value < 400) {
    return;
  }
  
  setMotorL(0);
  setMotorR(0);
  delay(500);
  setMotorL(-50);
  setMotorR(100); 
  while (sensor3Value > 350) {
    delay(10);
    sensor3Value = analogRead(A2);
  }
  delay(100);
  setMotorL(0);
  setMotorR(0);
  delay(500);
  setMotorL(100);
  setMotorR(80);
  delay(300); 
  setMotorL(0);
  setMotorR(0);
  delay(500);  
}

void scanDistance() {
  int pos = SRV_INITPOS;
  for(pos = SRV_INITPOS; pos < 180; pos += 1)  // Goes from 0 degrees to 180 degrees 
  {                                            // in steps of 1 degree 
    myservo.write(pos);                        // Tell servo to go to position in variable 'pos' 
    delay(5);                                  // Waits 15ms for the servo to reach the position 
  } 
  for(pos = 180; pos>=30; pos-=1)               
  {                                
    myservo.write(pos);                        
    delay(5);                                  
  } 
  for(pos = 30; pos<=SRV_INITPOS; pos+=1)    
  {                                
    myservo.write(pos);                        
    delay(5);                                  
  } 
}

А вот и описание алгоритма:

Программа из архива

ldr_robot_5.zip: (1.42 КБ) 1338 скачиваний

Подключение датчиков

ИК Дальномер – вход A2
Левый фоторезистор – вход A3
Правый фоторезистор – вход A4

Описание

После запуска программы однократно выполняется подпрограмма setup(). В этой подпрограмме выполняется инициализация переменных и настройка микроконтроллера.
После того, как все предварительные операции в подпрограмме setup() успешно выполнены, управление переходит к подпрограмме loop(). После завершения этой подпрограммы она запускается снова. Таким образом, мы имеем дело с бесконечной программой.

В подпрограмме loop() мы сначала запускаем подпрограмму avoidObstacle() в которой реализован алгоритм выявления препятствий на пути движения и выполнение маневра объезда. Затем выполняется часть программы, которая направляет робота на источник света.

Алгоритм движения на источник света начинается со строки 44. Сначала мы считываем в переменные sensor1Value и sensor2Value результаты аналого-цифрового преобразования сигналов на входах A3, A4. К этим входам подключены левый и правый фоторезисторы. Затем мы вычитаем sensor1Value - sensor2Value чтобы определить с какой стороны интенсивность света выше. Результат вычитания записываем в переменную diff.

Для выбора направления движения переменная diff сравнивается с опорными значениями, которые задаются как DBWIDTH/2 и -DBWIDTH/2. Если diff > DBWIDTH/2, то моторы включаются так, чтобы робот поворачивал влево (см. строку 49). Если diff < -DBWIDTH/2, то робот должен поворачивать вправо (см. строку 52). Если diff > -DBWIDTH/2 и diff < DBWIDTH/2, то робот движется прямо (см. строку 55).

Управление моторами выполняется через подпрограммы setMotorL() и setMotorR(), которые находятся в файле motor.ino.