пятница, 5 апреля 2013 г.

Управление обычной радиоуправляемой игрушки с помощью смартфона

Подключив к своей Blockduino блютуз модуль и передав на нее данные с датчиков с моего таба, не смог удержаться, чтобы не попробовать прикрутить все это на какую-нибудь игрушку.
В ход пошел танчик с убитым пультом управления.
На танчике шесть колес: задняя пара приводная, центральня ось сама по себе, а передняя пара рулевая, как в автомобиле, поворачивается с помощью моторчика.
Написал для этой машинки небольшую програмку под arduino. Принцип программы прост, она получает из последовательного порта символьную строку с показаниями акселерометра в виде трех дробных чисел разделенных символом ';' (cделаю отступление: для передачи с Android'а я использовал приложение под названием Amarino);
затем разделяет эту строку еще на три группы символов ориентируясь по точке с запятой и затем выделяет из этих групп знак (минус, если наклон отрицателен) и по три первых цифры; потом эти цифры преобразует в число. 
Первое число - направление движения(приводной мотор вперед/назад), второе -  поворот (поворотный мотор влево/вправо), третье число не используется (в будущем можно применить для пуск/стоп, вкл/выкл фары, стрельба, подача звукового сигнала и т.д.).


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

/*
  Code for BlockDuino w/ BlockMotor
  Control a car via bluetooth by Amarino (Android)
  by Vasyl Yudin
  
  Код для управления машинкой через Блютус с помощью программы Amarino на Android'е
  Управление путем наклона телефона
*/

//#define LCDDEBUG //uncomment for debug to TWI LCD module

#ifdef LCDDEBUG
  #include <Wire.h> 
  #include <LiquidCrystal_I2C.h>
#endif

#include <BDMotor.h>  //для BlockDuino и BlockMotor
//#include <AFMotor.h>  //для Arduino/Freeduino и MotorShield

const int drvMin = 150; // нижний порог чувствительности
const int strMin = 150;
const int drvMax = 510; // верхний порог чувствительности
const int strMax = 510;
const float drvCoeff = 255/(float)drvMax;
const float strCoeff = 255/(float)strMax;

String inStr;
char inChar;
int p1, p2, p3, n1, n2, n3;

AF_DCMotor mStr(1);  // #1 моторчик тяговый | Steering motor
AF_DCMotor mDrv(2);  // #2 моторчик рулевой | Drive motor

#ifdef LCDDEBUG
  LiquidCrystal_I2C lcd(0x27,8,2);  // set the LCD address to 0x27
#endif

void setup(){
  Serial.begin(9600);
#ifdef LCDDEBUG
  lcd.init();
  lcd.print(drvCoeff);
#endif
}

void loop()
{
  // получение пакета с блютуза | Take a data from Blutooth
  if (Serial.available()) {
    delay(100);
    inStr = "";
    while (Serial.available() > 0) {
      inChar = Serial.read(); // читаем символ | read a char
      inStr = inStr + inChar; // складываем в строку | and collect in string
    }

    // обработка пакета и конвертация в 3 числа | convert to 3 numders
    p1 = inStr.indexOf('.');       // находим позицию точки первого числа | find first position of number
    p2 = inStr.indexOf('.', p1+1); // -//- второго числа                         second
    p3 = inStr.indexOf('.', p2+1); // -//- третьего числа                        third
    
    n1 = GetNum(p1); // преобразуем цифровую строку в число | convert chars w/ digits to number
    n2 = GetNum(p2);
    n3 = GetNum(p3);
     
    // обработка комманд моторам 
    // первое число - вперед / назад  | first num - forward/back
    // второе число - лево / право    | second - left/right
    // третье число - стрельба | звук мотора | или что там еще | additional such as motor sound, shoot, LEDs, etc.
    
    // первое число | first num
    if (n1 < -drvMin) { // если точно назад | if really back
      n1 *= -1;
      n1 = constrain(n1, 0, drvMax) * drvCoeff;
      mDrv.setSpeed(n1);
      mDrv.run(BACKWARD); 
      
#ifdef LCDDEBUG
 lcd.clear();lcd.print("bck:");lcd.print(n1);
#endif

    } else if (n1 > drvMin) { // если точно вперед | if really fwd
      n1 = constrain(n1, 0, drvMax) * drvCoeff;
      mDrv.setSpeed(n1);
      mDrv.run(FORWARD);
      
#ifdef LCDDEBUG
 lcd.clear();lcd.print("fwd:");lcd.print(n1);
#endif

    } else { // ну тогда стоим | STOP
      mDrv.run(RELEASE);
    }
    
    //второе число
    if (n2 < -strMin)  { // если точно влево | if really to the left
      n2 *= -1;
      n2 = constrain(n2, 0, strMax);
      mStr.setSpeed(n2*strCoeff);
      mStr.run(BACKWARD);
    } else if (n2 > strMin) { // если точно вправо | if really to the right
      n2 = constrain(n2, 0, strMax);
      mStr.setSpeed(n2*strCoeff);
      mStr.run(FORWARD);
    } else { // ну тогда не рулим | go straight
      mStr.run(RELEASE);
    }
  }
}

// функция для преобразования цифровой строки в число (pos - позиция точки)
int GetNum(int pos){ 
  char c1, c2, c3, sign;     // т.к. число меньше десятки то
  sign = inStr.charAt(pos-2);// второй символ слева от точки может быть минус
  c1 = inStr.charAt(pos-1);  // первый символ слева от точки - это целое число
  c2 = inStr.charAt(pos+1);  // первый символ справа от точки - десятые доли
  c3 = inStr.charAt(pos+2);  // второй символ справа от точки - сотые доли (тысячные и т.д. нам не интересны)
  int resNum = (c1-0x30)*100+(c2-0x30)*10+(c3-0x30); // преобразовываем в целое трехзначное число
  if (sign=='-') resNum *= -1; // учитываем знак
  return resNum;             // в итоге имеем число от -999 до 999
}


Фоток нет, но есть видео как я его раскурочил и собрал: