Nonfinishism studious, заметки с тегом: ServoMotor

Учим робота ориентироваться в пространстве. Робот, объезжающий препятствия.

Fri, 30 Aug 2013 11:26:52 +0300

Обычно говорят «Скоро сказка сказывается, да не скоро дело делается». В нашем случае все с точностью до наоборот. Этого робота мы закончили (ну или почти закончили) делать в мае, а «сказка» появляется только сейчас, почти через три месяца.
Долго ли, коротко ли, пришло время научить робота принимать какие-то осмысленные решения самостоятельно. Пока робот умеет только обнаруживать препятствия и объезжать их слева. Необходимо, чтобы он объезжал препятствие либо слева, либо справа, в зависимости от того, с какой стороны больше места для маневра. Если же робот заезжает в тупик (т. е. невозможно проехать ни вперед, ни вправо, ни влево), он должен задним ходом выехать из тупика.

Из всего вышеописанного следует, что нам необходимо не только контролировать расстояние до препятствия впереди, но и расстояние до препятствия слева и справа.
Вариантов несколько:

установить ультразвуковые сенсоры впереди, слева и справа,
сделать так, чтобы сеносор мог вращаться и смотреть в разные стороны.

Второй вариант немного сложнее, но кажется более правильным.
Для того, чтобы сенсор мог смотреть в разные стороны мы использовали сервопривод вот такой.

Сервопривод закрепили в передней части робота, на него поставили сенсор. Таким образом сенсор может смотреть вперед, поворачиваться на 90 градусов направо и налево.

Для подсоединения сервопривода используется трехконтактная колодка, как и для сенсора (плюс, минус и сигнал). Для подключения использовали Sensor Shield (на фотографии это хорошо видно).

Для того, чтобы робот смог объезжать препятствия был придуман такой алгоритм:

Робот все время мониторит свободное пространство перед собой.
Как только расстояние до препятствия становится меньше определенного минимального значения, робот останавливается.
Сенсор поворачивается на 90 градусов налево и измеряется расстояние до препятствия, затем на 90 градусов направо и тоже измеряет расстояние.
Робот должен повернуться в том направлении, где расстояние до ближайшего препятствия больше, при этом оно должно быть больше определенного минимального значения.
После завершения маневра поворота робот продолжает движение прямо, и алгоритм исполняется с пункта 1.
В случае если и справа и слева расстояние до препятствия меньше определенного минимального значения, робот включается задний ход. Алгоритм исполняется с пункта 3.

Для управления сервоприводом мы использовали стандартную библиотеку Arduino. В этом месте выяснился непрятный момент, что стандартная библиотека для сервопривода конфликтует с библиотекой IRremote.h, которую мы использовали для работы с инфракрасным приемником. Они обе используют один таймер (9-й пин), а второй подключить не прадставляется возможным (так как 3-й пин занят двигателем). В общем испробовали разные библиотеки для сервопривода, которые не используют таймер, но как-то нормально ни одна не заработала. В результате поисков мы набрели на очень интересную библиотеку, которая позволяет работать с разными устройствами, в т.ч. и с ИК приемником (http://www.zbotic.com).

Вот так это работает:

Здесь уже робота поджидают испытания по сложнее:

Программа:

#include "Device.h"
#include "PCInterrupt.h"
#include "irController.h"
#include "myHardware.h"

#include <Servo.h> 
#include <ArduMoto.h>
#include <NewPing.h>
int key;
IRController controller;

const int mindist=15;
const int pingpin=4;  // Arduino pin tied to trigger pin on the ultrasonic sensor.
const int maxdist=400 ;// Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
Servo servoping;
NewPing sonar(pingpin, pingpin, maxdist); // NewPing setup of pins and maximum distance.

ArduMoto Moto;  //создаем объект для работы с моторами

const float kB=0.55 ; //коэффициент для мотора B, так как он крутится быстрее
const float kA=1;
const float kAll=0.7;//общий коэффициент
const int delayServo=200;
int distcmAhead;
int distcmRight;
int distcmLeft;


void setup()
{

  servoping.attach(A0);
  Moto.begin();//подключаем моторы
  Moto.setSpeed('A',0);//останавливаем моторы на всякий случай
  Moto.setSpeed('B',0);
  servoping.write(90);
  Serial.begin(9600);
  int res = controller.begin(IR_PIN, OTHER_DEVICE);
  forward(0);
}

void stopall(){
  forward(0);
}

int wheretogo()
{
  if( (distcmLeft>=distcmRight)&&(distcmLeft>mindist)||(distcmLeft=0))//можем ли мы ехать налево
  {
    return 1;//go left     
  }

  else {
    if( (distcmLeft<distcmRight)&&(distcmRight>mindist)||(distcmRight=0))////можем ли мы ехать направо
    {
      return 2;//go right
    }
    else{
      return 3;//go back
    }
  }
}

int lookahead()
{
  servoping.write(90);
  delay(delayServo);
  int distcm=sonar.ping_cm();
  return distcm;
}

int lookleft(){
  servoping.write(180);
  delay(delayServo*2);
  int distcm=sonar.ping_cm();
  return distcm;

}
int lookright(){
  servoping.write(0);
  //SoftwareServo::refresh();
  delay(delayServo*2);
  int distcm=sonar.ping_cm();
  return distcm;
}

void left(int velocity)
{
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',-velocity*kAll*kB);
}

void right(int velocity)
{
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',velocity*kAll*kB);
}


void forward(int velocity)
{
  Serial.println('go forward');
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',velocity*kAll*kB);
}

void backward(int velocity)
{
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',-velocity*kAll*kB);
}
void forwardleft(int velocity)
{
  Moto.setSpeed('A',velocity*kAll*kA);
  Moto.setSpeed('B',0.4*velocity*kAll*kB);
}
void forwardright(int velocity)
{
  Moto.setSpeed('B',velocity*kAll*kB);
  Moto.setSpeed('A',0.4*velocity*kAll*kA);
}
void backwardright(int velocity){
  Moto.setSpeed('B',-velocity*kAll*kB);
  Moto.setSpeed('A',-0.4*velocity*kAll*kA);
}
void backwardleft(int velocity){
  Moto.setSpeed('A',-velocity*kAll*kA);
  Moto.setSpeed('B',-0.4*velocity*kAll*kB);
}

void loop() {     
  int dir;
  distcmAhead=lookahead();
  if( (distcmAhead<=mindist)&&(distcmAhead>0)){
    stopall();
label1:   
    distcmLeft=lookleft();
    distcmRight=lookright();
    dir=wheretogo();
    switch (dir){
    case 1://едем налево
      left(100);
      delay(500);
      forward(100);
      break;
    case 2://едем направо
      right(100);
      delay(500);
      forward(100);
      break;
    case 3:
      backward(100);  
      do{
        distcmLeft=lookleft();
        distcmRight=lookright();
      }
      while( (distcmLeft<mindist)&&(distcmRight<mindist)&&(distcmRight>0)&&(distcmLeft>0));
      goto label1; 
    }
  } 


  key = controller.read();
  if (key>=0){//если получили сигнал то надо что-то сделать
    switch (key){
    case  16128://едем вперед
      forward(100);
      break;
    case 16192:
      backward(100);
      break;
    case   14726:
      forward(0);
      break;
    case  15427 :
      forwardleft(100);
      break;
    case   14662:
      forwardright(100);
      break;
    case   16256:
      backwardright(100);
      break;
    case  10200   :
      backwardleft(100);
      break;
    case  15746 :
      left(100);
      break;
    case  14471  :
      right(100);
      break;
    }
  }
}

Использование сервопривода.

Sun, 10 Mar 2013 13:24:38 +0300

Сервопривод — это такие типы двигателей, которые не крутятся все время по кругу, а поворачиваются на определённый угол и останавливаются до тех пор, пока им не поступит другая команда повернуться. Сервопривод обычно поворачиваются только на 180 градусов (половина круга). Если присоединить к такому мотору вырезанную из картона шкалу, можно использовать его в качестве указателя чего-нибудь, например, настроения.

Аналогично тому, как мы использовали ШИМ для управления светодиодами в опыте с «лампой-хамелеоном», сервопривод ожидает поступления определенного количества импульсов, чтобы понять, на какой угол ему надо повернуться. С аналогового выхода Arduino импульсы всегда поступают через один и тот же временной интервал, но продолжительность их меняется от 1000 до 2000 микросекунд. Программу, которая генерирует такие импульсы написать не очень сложно самим, но среда разработки Arduino уже включает в себя специальную библиотеку для управления моторами. Поскольку сообщество разработчиков для Arduino достаточно велико, существует великое множество дополнительных программ и библиотек для разных сенсоров, актуаторов и прочих устройств, которые могут взаимодействовать с Arduino. И мы этим тоже будем пользоваться.

Схема, которую мы будем собирать выглядит так:

Один контакт потенциометр подсоединяем на плюс, второй — на минус, подвижный контакт — к аналоговому входу Arduino. При вращении ручки потенциометр, напряжение между подвижным контактом (входом Arduino) и минусом будет изменяться, это изменение мы будем «читать» и управлять вращением сервопривод.

Сервопривод тоже имеет три контакта: два — питание (плюс и минус), третий для управления, его подсоединяем с выходу Arduino.

Когда сервопривод начинает движение, ток, проходящий через него, значительно больше, чем во время самого движения, из-за этого происходить сильное падение напряжения во всей схеме. Если подключить два конденсатора параллельно сервопривод и потенциометр, можно это сгладить. Такие конденсаторы называют «развязывающие» или разделяющие, так как они отделяют изменения, произведенные какими-либо компонентами, от остальной схемы.

Итак, макет готов:

Теперь пишем программу:

#include<Servo.h> //используем библиотеку для управления сервоприводом
Servo motor;      //наш мотор
int potens=A0;    //пин потенциометра 
int motorpin=9;   //пин мотора
int potensVal=0;  //значение потенциометра
int angle;        //угол поворота сервопривода

void setup(){ 
  motor.attach(motorpin); //подключаем мотор
  Serial.begin(9600);
}

void loop(){
  potensVal=analogRead(potens);//читаем положение потенциометра
  Serial.print(«potensVal: „);
  Serial.print(potensVal);
  angle=map(potensVal,0,1023,0,179);//преобразуем в градусы
  Serial.print(“, angle: „);
  Serial.println(angle);
  motor.write(angle);//поворачиваем мотор
  delay(15);
}

Небольшой комментарий по преобразованию значения, считанного с потенциометр, в градусы. С аналогового входа Arduino можно считать значение от 0 до 1023, в зависимости от напряжения на нем. В случае с потенциометром мы задействует все эти значения. На аналоговый выход Arduino мы может подать значение от 0 до 254, а в случае с сервоприводом, в команду движения мы вообще можем передавать значения только до 179. То есть нам надо пропорционально преобразовать значения из интервала [0;1023] в интервал [0;179]. У Arduino для этого есть специальная функция map(), в качестве первого параметра мы передаем значение, которое надо преобразовать, в качестве второго и третьего — границы первого интервала, четвертое и пятое — границы второго интервала. В результате функция возвращает преобразованное значение, которое уже можно использовать. После команды поворота добавлена небольшая пауза, чтобы сервопривод успел повернуться. Фу… вроде все.

На консоль мы выводим значение, прочитанное с потециометра и уже преобразованное значение (угол поворота сервопривод).

Теперь мы решили вместо потенциометр поставить в схему термосенсор, и поворачивать сервопривод в зависимости от внешней температуры. Немного изменили программу:

void loop(){
  tempVal=analogRead(tempPin);
  float v=(tempVal/1024.0)*5.0;
  float t=(v-0.5)*100;
  Serial.print(» temperature: «);
  Serial.print(t);
  angle=map( t, 15, 35,0,179);
  Serial.print(«, angle: „);
  Serial.println(angle);
  motor.write(angle);
  delay(1000);
}

Преобразовываем значение, считанное с термосенсор, в градусы, как мы это уже делали. В функции map() мы указали интервал от 15 до 35, потому что именно в этих пределах будет меняться температура в нашем опыте, и сервопривод будет поворачиваться на все 180 градусов при изменении температуры.

angle