Управление гусеничным роботом со смартфона

  В продолжение предыдущей статьи решил поуправлять небольшим шасси на гусеничном ходу купленном на Pololu.

  Для этого необходимо:

• гусеничное шасси с двумя моторчиками;
• плата BlockDuino;
• плата BlockMotor;
• модуль Bluetooth;

и по необходимости:

• плата BlockProto (если есть желание подпаять модуль Bluetooth);
• и шлейф для параллельного соединения блоков (без шлейфа блоки подключаются последовательно);

Для управления использовал программу Amarino с настройкой для передачи данных датчика ускорения.

!!! К сожаления с GooglePlay исчезла программа Amarino, поэтому в ближайшее время протестирую другие программы

Код для приема данных и управления моторчиками переделан из предыдущего поста, поэтому он слегка сыроват:

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

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

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

const int drvMin = 150; // нижний порог чувствительности
const int strMin = 25 ;
const int drvMax = 200; // верхний порог чувствительности
const int strMax = 200;
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 mLft(1);  // #1 моторчик левого трака | Left track
AF_DCMotor mRht(2);  // #2 моторчик правого трака | Right track

void setup(){
  Serial.begin(9600);
}

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;
      mRht.setSpeed(n1);
      mRht.run(BACKWARD); 
      mLft.setSpeed(n1);
      mLft.run(BACKWARD); 

    } else if (n1 > drvMin) { // если точно вперед | if really fwd
      n1 = constrain(n1, 0, drvMax) * drvCoeff;
      mRht.setSpeed(n1);
      mRht.run(FORWARD);
      mLft.setSpeed(n1);
      mLft.run(FORWARD);

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

// функция для преобразования цифровой строки в число (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
}

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

Видеоотчёт: