Inclination Rider - Едем по наклонной

Как и обещал, подключил акселерометр к управлению роботом.

Inclination Rider - Робот поворачивается в сторону небольшого наклона и едет вперед при бо'льшем наклоне.

Алгоритм прост:

• если наклон по Х(право-лево) и Y(нос-корма) меньше допустимого, то стоим;
• если нос вниз и левый борт чуток внизу, то направо;
• если нос вниз и правый борт чуток внизу, то налево;
• если левый борт внизу, то направо;
• если правый борт внизу, то налево;
• а если нос вверх в два раза больше допустимого, то едем вперед;

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

// Inclination Rider - Робот поворачивается в сторону небольшого наклона
// и едет вперед при бóльшем наклоне
// http://blockduino.org
// http://blockduino.blogger.com/2013/10/nclination-rider.html
// 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 x, y;                // переменные для хранения показания сенсора
int tol = 100;           // величина предельного наклона 

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

LSM303 compass;

void setup() {
  Serial.begin(9600);    // Создаем подключение серийного порта
  Wire.begin();          // запускаем протокол I2C
  compass.init();     // подключаем и 
  compass.enableDefault();  // запускаем компас
  compass.read();           // считываем первое значение
  x = (int)compass.a.x;     // по х и y
  y = (int)compass.a.y;     // 
  
  pinMode(LED_PIN, OUTPUT); // Объявляем порт индикаторного светодиода
  
  mL.setSpeed(SPEED);    // устанавливаем скорость вращения левого моторчика
  mR.setSpeed(SPEED);    // устанавливаем скорость вращения правого моторчика
}

void loop() { // Основное тело программы
  compass.read();   // считываем и сразу же сглаживаем показания акселерометра
  x = digitalLowPass(x, (int)compass.a.x, 0.75);
  y = digitalLowPass(y, (int)compass.a.y, 0.75);
  
  if (abs(y)<tol && abs(x)<tol) Stop();    // если наклон в пределах, то стоим
  else if (y<-tol && x<-tol/2) TurnLeft(); // если нос внизу и правый бок внизу, то крутимся влево
  else if (y<-tol && x>tol/2) TurnRight(); // если нос внизу и левый бок внизу, то крутимся вправо
  else if (x<-tol) TurnLeft();             // если завалились на правый бок, то крутимся влево
  else if (x>tol) TurnRight();             // если завалились на левый бок, то крутимся вправо
  else if (y>2*tol) Run();                 // если нос поднят в два допуска, то едем вперед
  
  delay(100);
} // конец основной программы, начинаем все сначала

//
// Подпрограммы
//
//Digital low pass filter
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);
      Serial.println("Stop");
}

void Run(){  // подпрограмма Ход вперед
      mL.run(FORWARD);
      mR.run(FORWARD);
      digitalWrite(LED_PIN, LOW);
      Serial.println("Run");
}

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

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

Видео отчет: