BlockDuino: Compass Rider - Робот знающий свое направление

понедельник, 21 октября 2013 г.

Compass Rider - Робот знающий свое направление

Продолжаем играться в роботов.

На этот раз прикурочил я к моему роботу компас на микросхеме LSM303, от Sparkfun.

Замечательная штучка - работает по протоколу I2C, а у меня как раз под нее разъемчик появился.

Долго расписываться не буду - робот тупо стоит на месте, но старается из всех сил держать направление. Т.е. возражает от попыток его крутить и поворачивается в нужную сторону. Притом не крутится тупо в одну сторону, а интеллектуально вычисляет кратчайшее расстояние.

Кстати этот компас нужно сначала откалибровать. А то я по первой проигнорировал калибровку, а потом не смог сразу догнать, чего мой робот считает кратчайшим направление в одну сторону 90 градусов, а в другую все 270. А оказалось что на 90 градусов компас показывал все 180!

Показания компаса без (вверху) и с калибровкой (внизу)

Ну, вроде бы все.

Тут код, а там видео-отчет.

Код под спойлером:

// Compass Rider - Робот удерживающий направление 
// Note: робот поворачивается по кратчайшему пути!
// http://blockduino.org/
// http://blockduino.blogger.com/2013/10/compass-rider.html
// Vasyl Yudin
// Oct 2013

#include <Wire.h>   // библиотека для подключения по протоколу I2C
#include <LSM303.h>   // библиотека для работы с микросхемой LSM303
#include <BDMotor.h>     // для BlockDuino и BlockMotor
//#include <AFMotor.h>   // для Arduino и MotorShield

#define SPEED 255        // скорость вращения моторчиков, 255 - максимум
#define LED_PIN 13       // номер порта для ножки индикаторного светодиода

int sensor;              // переменная для хранения показания сенсора
int dir = 180;   // заданное направление
int tol = 20;   // допустимая точность удерживания направления

AF_DCMotor mL(1);        // #1 моторчик левой гусеницы
AF_DCMotor mR(4);        // #2 моторчик правой гусеницы

LSM303 compass;

void setup() {
  Serial.begin(9600);       // Создаем подключение серийного порта
  Wire.begin();      // Подключаем протокол I2C
  compass.init();     // подключаем и 
  compass.enableDefault();  // запускаем компас
  // Значение калибровки. Используйте пример Calibrate для вашего компаса
  compass.m_min.x = -543; compass.m_min.y = -303; compass.m_min.z = -300;
  compass.m_max.x = +195; compass.m_max.y = +408; compass.m_max.z = 312;
  
  pinMode(LED_PIN, OUTPUT); // Объявляем порт индикаторного светодиода
  
  mL.setSpeed(SPEED);    // устанавливаем скорость вращения левого моторчика
  mR.setSpeed(SPEED);    // устанавливаем скорость вращения правого моторчика
}

void loop() { // -------   Основное тело программы
  compass.read();             // считываем показания компаса
  int heading = compass.heading();  // вынимаем из показаний величину направление компаса
  if (abs(heading - sensor)>320) sensor = heading; // отсекаем проблемный переход из 359° в 1°
  sensor = (int)digitalLowPass(sensor, heading, 0.7);  // отфильтровываем показания компаса (сглаживаем)
  int dif = sensor - dir;     // вычисляем разницу между реальным направлением и заданным
  int absdif = abs(dif);
  if (absdif <= tol) Stop();        // если разница меньше допустимой погрешности, то останавливаемся
    else if (((absdif > 180) && (dif != absdif)) || ((absdif < 180) && (dif == absdif))) TurnLeft();  // иначе определяем кратчайшее направление поворота
           else TurnRight();
//  delay(25);
  
} // конец основной программы, начинаем все сначала

//
// Подпрограммы
//
// Цифровой низкочастотный фильт
double digitalLowPass(double last_smoothed, double new_value, double filterVal)
{
  return (new_value * (1 - filterVal)) + (last_smoothed * filterVal);
}

void Stop(){  // подпрограмма Остановка
      mL.run(RELEASE);
      mR.run(RELEASE);
      digitalWrite(LED_PIN, HIGH);
}

void TurnRight(){ // подпрограмма Поворот направо
      mL.run(FORWARD);
      mR.run(BACKWARD);
      digitalWrite(LED_PIN, LOW);
}

void TurnLeft(){ // подпрограмма Поворот налево
      mR.run(FORWARD);
      mL.run(BACKWARD);
      digitalWrite(LED_PIN, LOW);
}