Учим робота ориентироваться в пространстве. Робот, объезжающий препятствия.
Обычно говорят «Скоро сказка сказывается, да не скоро дело делается». В нашем случае все с точностью до наоборот. Этого робота мы закончили (ну или почти закончили) делать в мае, а «сказка» появляется только сейчас, почти через три месяца.
Долго ли, коротко ли, пришло время научить робота принимать какие-то осмысленные решения самостоятельно. Пока робот умеет только обнаруживать препятствия и объезжать их слева. Необходимо, чтобы он объезжал препятствие либо слева, либо справа, в зависимости от того, с какой стороны больше места для маневра. Если же робот заезжает в тупик (т. е. невозможно проехать ни вперед, ни вправо, ни влево), он должен задним ходом выехать из тупика.
Из всего вышеописанного следует, что нам необходимо не только контролировать расстояние до препятствия впереди, но и расстояние до препятствия слева и справа.
Вариантов несколько:
- установить ультразвуковые сенсоры впереди, слева и справа,
- сделать так, чтобы сеносор мог вращаться и смотреть в разные стороны.
Второй вариант немного сложнее, но кажется более правильным.
Для того, чтобы сенсор мог смотреть в разные стороны мы использовали сервопривод вот такой.
Сервопривод закрепили в передней части робота, на него поставили сенсор. Таким образом сенсор может смотреть вперед, поворачиваться на 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;
}
}
}
