RoboCraft

Магазин

Arduino + Tamiya+TSOP, Часть 4 Движение по линии

Появилось время продолжить работу. (Начало 1 , 2 , 3 )
Делаем два режима:
1) уже существующий — управление движением с ИК пульта (в будущем переход на двусторонний обмен с Android-планшетом по Bluetooth hc05)
2) автономный режим (пока делаю движение по линии)

Из четырех подключенных к роботу датчиков черное-белое
OpenRobotics OR-BWSENS
решил использовать два передних.
Датчики решил расположить следующим образом

Для получения данных с датчиков использовалась библиотека PololuQTRSensors,
которая предназначена для работы с датчиками линии Polulu(аналоговыми и цифрвыми),
состоящими из нескольких датчиков черное — белое.
Подключение библиотеки

#include <QTRSensors.h>

Далее создание экземпляра объекта для цифровых

QTRSensorsRC qtrrc((unsigned char[]) {B1,...,Bn},NUM_SENSORS, TIMEOUT,EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

B1,…,Bn- пины Arduino для подключения сенсоров
NUM_SENSORS — количество сенсоров
TIMEOUT — время ожидания разрядки конденсатора датчика
(Из инструкции для датчиков OR-BWSENS
«Принцип работы датчика:
Замыкаем на 500мкс (для версии 1 датчика — на 2500мкс) сигнальную линию датчика на землю, разряжая конденсатор;
Делаем сигнальную линию входом для МК (т.е. перестаём её подтягивать к земле или к питанию) и ждём сколько-то времени, пока через открытый, в зависимости от отражающей способности поверхности в спектре ИК-излучения и расстояния до поверхности, оптодатчик зарядится конденсатор;
Замеряем что на выходе с датчика — 0 или 1.» )
EMITTER_PIN — QTR_NO_EMITTER_PIN

Далее в процедуре setup() — калибровка датчика

  for (i = 0; i < 400; i++)
  {
    qtrrc.calibrate();
  }

И в основном цикле — считывание показаний датчиков

  unsigned char i;
  for (i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(sensorValues[i] * 10 / 1001);
    Serial.print(' ');
  }

Черная линия выдавала показания — 24 (sensorValues[i] * 10 / 1001)
белый лист — 0-2
Остается только реализовать движение по черной линии
Добавил 2 режима работы робота
— внешнее управление;
— автономный (движение по линии)
Для этого добавил для ИК-пульта две клавиши

#define  EXT 16          //"1" - переход во внешнее управление
#define  LOCAL 1040      //"3" - автономный режим

И два псевдо-кода (при наезде на черную линию
передним левым и передним правым датчиком)

#define  LOCAL_LEFT 9997     // для поворота по QTR
#define  LOCAL_RIGHT 9998    // для поворота по QTR

И весь код и далее видео

// ******************************
// подключение библиотеки irremote
#include <IRremote.h>
//вход ИК приемника
int RECV_PIN = 2;
IRrecv irrecv(RECV_PIN);
decode_results results;
unsigned long ir_dt, old_ir;
long ir_kod;
unsigned long ir_time1, ir_time2;
// ******************************
// коды клавиш ИК пульта (marmitek)
#define FORWARD 1936
#define BACK  3984
#define SPEED_UP 144       //ch+
#define SPEED_DOWN 2192    //ch-
#define LEFT 3472
#define RIGHT 1424
#define CIRCLE_LEFT 3216   //vol+
#define CIRCLE_RIGHT 1168  //vol-
#define STOP 2320          //0

#define  EXT 16          //1
#define  LOCAL 1040          //3
#define  LOCAL_LEFT 9997     // для поворота по QTR
#define  LOCAL_RIGHT 9998    // для поворота по QTR

int local=0;
// состояние моторов робота
struct POZ    // структура для хранения статусов моторов
{
  int poz1;      // позиция для 1 мотора
  int poz2;      // позиция для 2 мотора
  int direction12;      // направление до поворота
  int qtr[4];      //
  long millis1[4];  // для датчиков черное-белое фиксация появления белого
};
// моторы
struct MOTOR    // структура для хранения номеров pin-ов, к которым подключены моторчики
{
  int in1;      // INPUT1
  int in2;      // INPUT2
  int enable;   // ENABLE1
};
// определяем порты, к которым подключены моторчики
MOTOR MOTOR1 = { 13, 12, 5 };
MOTOR MOTOR2 = { 7, 8, 6 };
// начальная позиция робота
// 1,2 мотор - выключены
POZ POZ12={7,7,7,{0,0,0,0},{0,0,0,0}};
// массив возможных состояний моторов
// max down -> min down -> stop -> min up -> max up
int arrpoz[9][17]={
 // in1 первого мотора
 {0,0,0,0,0,0,0,0,1,1,1,1,1,1,1},
 // in2 первого мотора
 {1,1,1,1,1,1,1,0,0,0,0,0,0,0,0},
 // скорости (enable1) первого мотора
 {250,230,210,190,170,150,130,0,130,150,170,190,210,230,250},
 // in3 второго мотора
 {0,0,0,0,0,0,0,0,1,1,1,1,1,1,1},
 // in4 второго мотора
 {1,1,1,1,1,1,1,0,0,0,0,0,0,0,0},
 // скорости (enable2) второго мотора
 {250,230,210,190,170,150,130,0,130,150,170,190,210,230,250},
 // скорость++
 {0,-1,-1,-1,-1,-1,-1,0,1,1,1,1,1,1,0},
 // скорость--
 {-1,-1,-1,-1,-1,-1,-1,0,1,1,1,1,1,1,1},
 // кружение влево-вправо
 {14,12,10,8,6,4,2,0,-2,-4,-6,-8,-10,-12,-14}
 };
// ******************************
// датчики черное-белое
#include <QTRSensors.h>

#define NUM_SENSORS   4     // кол-во сенсоров
#define TIMEOUT       2500  // 2500мс
#define EMITTER_PIN   QTR_NO_EMITTER_PIN     //

// выводы для датчиков
// перед левый 9
// перед правый 4
// зад левый 11
// зад правый 10
QTRSensorsRC qtrrc((unsigned char[]) {4,9,10,11}, NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

//long millis1=0;

void setup()
 {
  // ***** последовательный порт
  Serial.begin(9600);
  // *****
  delay(500);
  int i;
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);    // turn on LED to indicate we are in calibration mode
  for (i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  {
    qtrrc.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  }
  digitalWrite(13, LOW);     // turn off LED to indicate we are through with calibration
  for (i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
   // print the calibration maximum values measured when emitters were on
  for (i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtrrc.calibratedMaximumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
  delay(1000);


  // ***** включить приемник
  irrecv.enableIRIn();
  // прерывания для ИК
  ir_time1=0;ir_time2=0;
  attachInterrupt(0, get_ir_kod, FALLING);
  // ***** настраиваем выводы для моторов
  pinMode(MOTOR1.in1, OUTPUT);
  pinMode(MOTOR1.in2, OUTPUT);
  pinMode(MOTOR2.in1, OUTPUT);
  pinMode(MOTOR2.in2, OUTPUT);
  // начальная позиция - робот стоит
  go12(POZ12.poz1,POZ12.poz2);
 }

void loop()
 {
  // обработка кода нажатия
  if(ir_kod>0)
   {
   ir_go(ir_kod);
   //Serial.println(ir_kod);
   ir_kod=0;
   }
  // если управление local - проверка датчиков
  if(local==1)
    {
    unsigned int position = qtrrc.readLine(sensorValues);
    qtrrc.read(sensorValues);
    for (int i = 0; i < NUM_SENSORS; i++)
      {
      int qtrv=0;
      if(sensorValues[i]*10/1001==24) qtrv=1;
      if(POZ12.qtr[i]!=qtrv)
        {POZ12.qtr[i]=qtrv;POZ12.millis1[i]=millis();}
      //Serial.print(sensorValues[i] * 10 / 1001);Serial.print(' ');
      //Serial.print(millis()-POZ12.millis1[i]);Serial.print(' ');
      }
    //Serial.println(position);
    //if(POZ12.qtr[0]==1 && (millis()-POZ12.millis1[0])>200)
    if(POZ12.qtr[0]==1)
      ir_kod=LOCAL_RIGHT;
    //else if(POZ12.qtr[1]==1 && (millis()-POZ12.millis1[1])>200)
    else if(POZ12.qtr[1]==1)
      ir_kod=LOCAL_LEFT;
    //else if((millis()-POZ12.millis1[0])>1000 && (millis()-POZ12.millis1[1])>1000)
    else
      ir_kod=LOCAL;
    //else ;
    if(ir_kod>0)
      {detachInterrupt(0);local=0;
       ir_go(ir_kod);
       local=1;attachInterrupt(0, get_ir_kod, FALLING);}
   }
 delay(100);
 }

// получить код переданный с ИК пульта
void get_ir_kod()
 {
 detachInterrupt(0);    // отключить прерывание 0
 if (irrecv.decode(&results))
  {
  if (results.value > 0 && results.value < 0xFFFFFFFF)
    {
    ir_dt = results.value;
    ir_time2=millis();
    ir_kod=ir_dt;
    Serial.println(ir_kod);
    }
    irrecv.resume();
  }
  //attachInterrupt(0, get_ir_kod, CHANGE);
  attachInterrupt(0, get_ir_kod, FALLING);
 }
// переустановка моторов
void go12(int poz1,int poz2)
 {
  digitalWrite(MOTOR1.in1, arrpoz[0][poz1]);
  digitalWrite(MOTOR1.in2, arrpoz[1][poz1]);
  analogWrite(MOTOR1.enable, arrpoz[2][poz1]);
  digitalWrite(MOTOR2.in1, arrpoz[3][poz2]);
  digitalWrite(MOTOR2.in2, arrpoz[4][poz2]);
  analogWrite(MOTOR2.enable, arrpoz[5][poz2]);
//Serial.print(arrpoz[0][poz1]);Serial.print(" ");
//Serial.print(arrpoz[1][poz1]);Serial.print(" ");
//Serial.print(arrpoz[2][poz1]);Serial.print(" ");
//Serial.print(arrpoz[3][poz2]);Serial.print(" ");
//Serial.print(arrpoz[4][poz2]);Serial.print(" ");
//Serial.print(arrpoz[5][poz2]);Serial.print("\n");
 }

// обработка кода нажатия
void ir_go(long kod)
  {
   Serial.print(kod);
   Serial.print("\n");
   if(kod!=EXT && local==1)
     return;
   switch(kod)
     {
     case LOCAL : // внешнее управление включить
       Serial.print("local\n");
        POZ12.direction12=10;POZ12.poz1=10;POZ12.poz2=10;
        go12(10,10);local=1;
       break;
     case LOCAL_LEFT : //
       Serial.print("local_left\n");
        POZ12.direction12=8;POZ12.poz1=7;POZ12.poz2=8;
        go12(POZ12.poz1,POZ12.poz2);
       break;
     case LOCAL_RIGHT : // по датчику вправо
       Serial.print("local\n");
        POZ12.direction12=8;POZ12.poz1=8;POZ12.poz2=7;
        go12(POZ12.poz1,POZ12.poz2);
       break;
     case EXT : // внешнее управление включить
       Serial.print("external\n");
       local=0;
       break;
     //case 1936 : // направление вперед
     case FORWARD : // направление вперед
        Serial.print("forward\n");
        POZ12.direction12=max(POZ12.direction12,8);
        POZ12.poz1=POZ12.direction12;
        POZ12.poz2=POZ12.direction12;
        go12(POZ12.poz1,POZ12.poz2);
       break;
     //case 3984 : // направление назад
     case BACK : // направление назад
        Serial.print("back\n");
        POZ12.direction12=min(POZ12.direction12,6);
        POZ12.poz1=POZ12.direction12;
        POZ12.poz2=POZ12.direction12;
        go12(POZ12.poz1,POZ12.poz2);
        break;
      //case 144 : // скорость++
      case SPEED_UP : // скорость++
        Serial.print("speed++\n");
        POZ12.direction12=POZ12.direction12+arrpoz[6][POZ12.direction12];
        POZ12.poz1=POZ12.direction12;
        POZ12.poz2=POZ12.direction12;
        go12(POZ12.poz1,POZ12.poz2);
        break;
      //case 2192 :  // скорость--
      case SPEED_DOWN :  // скорость--
       Serial.print("speed--\n");
        POZ12.direction12=POZ12.direction12-arrpoz[7][POZ12.direction12];
        POZ12.poz1=POZ12.direction12;
        POZ12.poz2=POZ12.direction12;
        go12(POZ12.poz1,POZ12.poz2);
        break;
      //case 3472 : // влево
      case LEFT : // влево
        Serial.print("left\n");
        if(POZ12.direction12>7)
          {
          POZ12.poz1=POZ12.poz1-1;POZ12.poz1=max(POZ12.poz1,0);
          POZ12.poz2=POZ12.direction12;
          }
        else if(POZ12.direction12<7)
          {
          POZ12.poz1=POZ12.poz1+1;POZ12.poz1=min(POZ12.poz1,14);
          POZ12.poz2=POZ12.direction12;
          }
        go12(POZ12.poz1,POZ12.poz2);
       break;
     //case 1424 : // вправо
     case RIGHT : // вправо
        Serial.print("right\n");
        if(POZ12.direction12>7)
          {
          POZ12.poz2=POZ12.poz2-1;POZ12.poz2=max(POZ12.poz2,0);
          POZ12.poz1=POZ12.direction12;
          }
        else if(POZ12.direction12<7)
          {
          POZ12.poz2=POZ12.poz2+1;POZ12.poz2=min(POZ12.poz2,14);
          POZ12.poz1=POZ12.direction12;
          }
        go12(POZ12.poz1,POZ12.poz2);
       break;
     //case 1168 : // кружение вправо
     case CIRCLE_RIGHT : // кружение вправо
        Serial.print("circle_right\n");
        POZ12.poz1=POZ12.direction12;
        POZ12.poz2=POZ12.direction12+arrpoz[8][POZ12.direction12];
        go12(POZ12.poz1,POZ12.poz2);
        break;
     //case 3216 : // кружение влево
     case CIRCLE_LEFT : // кружение влево
        Serial.print("circle_left\n");
        POZ12.poz1=POZ12.direction12+arrpoz[8][POZ12.direction12];
        POZ12.poz2=POZ12.direction12;
        go12(POZ12.poz1,POZ12.poz2);
        break;
     //case 2320 : // стоп
     case STOP : // стоп
        Serial.print("stop\n");
        POZ12.poz1=7;
        POZ12.poz2=7;
        POZ12.direction12=7;
        go12(POZ12.poz1,POZ12.poz2);
        break;
     default:
        break;
     }
  }

Видео движения по линии

Продолжение следует ….

,

victoruni

0 комментариев на «“Arduino + Tamiya+TSOP, Часть 4 Движение по линии”»

  1. Хороший проект:)
    Когда-то сам тоже делал движение по линии, только без МК, на ИК фотодиодах и фототранзисторах. Даже без колес, тупо моторы примотал скотчем к изогнутой железке, валы упирал в пол (тогда был линолиум). Результат был так себе, но худо-бедно работало.

  2. делал как раз под свою задумку —
    линия между датчиками и ширина линии значительно < расстояния между датчиками
    на соревнованиях (робофест) там ширина линии 5 см — алгоритм другой нужен
    но эта конструкция очень медленная
    я для другого:
    перевоз «грузов» из одного места в другое
    Надеюсь закончить, почти все есть
    сервы, захват механический, датчики sharp аналоговый и цифровой,
    не хватает пока одного цифрового 5 см

    • Ну на 5см и самому датчик изваять не сложно

Добавить комментарий

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Arduino Bluetooth CraftDuino DIY Google IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение