Программируем управление с ПК

В предыдущих сериях:
Часть 1
Часть 2

Ну что, все уже заказали запчасти и собрали роботов? Пора робота оживить.
Сегодня мы разберем программную начинку.
Вариант, который я предлагаю максимально прост. Не стоит ждать от него уникальных способностей. Его задача — просто ехать работать. Отказоустойчивость, плавность управления и дополнительные функции — это простор для творчества, который я оставляю каждому, чтобы не лишать этого удовольствия. Код весьма простой и оттого далеко не оптимальный и не защищенный и вообще не красивый. Если есть предложения по его улучшению — предлагайте свои варианты, прямо куски кода с пояснением зачем и почему так будет лучше.
Неконструктивная критика того, что сделано плохо — не особо нужна 🙂 Я и так знаю про недостатки. А вот если что-то непонятно- спрашивайте, поясню.

Разделим задачу на простые этапы:
Бортовой контроллер

  • Управление моторами гусениц для движения вперед/назад и поворотов
  • Управление сервоприводами камеры
  • Прием по bluetooth и исполнение команд движения и управления сервоприводами камеры

ПК/Ноутбук

  • Вычисление скорости моторов для задания направления движения
  • Передача управляющих пакетов по bluetooth
  • Подключение джойстика для удобства управления

Управление моторами гусениц для движения вперед/назад и поворотов
Поскольку у нас используется готовый MotorShield а не голый H-bridge или L293D/L298N, то ничего особенно сложного изобретать не придется. Мы вспользуемся библиотекой AFMotor. Если у вас Motorshield V3 и вам нужна шина SPI — возьмите модифицированный вариант.
Комментарии я по привычке чаще всего пишу по-английски, так проще и короче.
Объявляем переменные для управления моторами. к 4му порту подключен правый мотор, к 3му — левый.

AF_DCMotor rMotor(4);  //Right motor
AF_DCMotor lMotor(3);  //Left motor

в зависимости от заданного направления и скорости коммандуем моторам вращаться (для левого мотора):


      switch (lDirection){
        case 0:
          lMotor.run(RELEASE);
          break;
        case 1:
          lMotor.run(FORWARD);
          lMotor.setSpeed(lSpeed);
          break;
        case 2:
          lMotor.run(BACKWARD);
          lMotor.setSpeed(lSpeed);
          break;
      } 

Для правого мотора то же самое:


      switch (rDirection){
        case 0:
          rMotor.run(RELEASE);
          break;
        case 1:
          rMotor.run(FORWARD);
          rMotor.setSpeed(rSpeed);
          break;
        case 2:
          rMotor.run(BACKWARD);
          rMotor.setSpeed(rSpeed);
          break;
      }

lDirection (или rDirection) принимает значения:
0 — остановить мотор
1 — вращение вперед
2 — вращение назад.

Управление сервоприводами камеры

Для управления сервоприводами объявляем два объекта panServo(отвечает за вращение камеры) и tiltServo (отвечает за наклон). Так как сервопривод механический и поворачивается не моментально, то введем переменную для задержки, требуемой приводу для отработки команды на поворот (15 мсек вполне достаточно)

Servo panServo, tiltServo;
long interval = 15;    // interval at which to control servo
long previousMillis = 0;   
unsigned long currentMillis;

previousMillis и currentMillis используются для того, чтобы в цикле управления не ждать тупо, когда отработает серво. Проверяем — если со времени последней команды не прошло 15 мсек, то командовать сервой бесполезно — она еще занята.
Кусок, отвечающий за вращение камеры:

      //Rotate camera
      currentMillis = millis();
      if(currentMillis - previousMillis > interval) {
        previousMillis = currentMillis;
        if (lastPan!=pan) panServo.write(pan);	     // tell pan servo to go to position
        if (lastTilt!=tilt) tiltServo.write(tilt);   // tell tilt servo to go to position
        lastPan=pan;
        lastTilt=tilt;
      }

Прием по bluetooth и исполнение команд движения и управления сервоприводами камеры

Bluetooth модуль с точки зрения Arduino — просто последовательный (UART) порт.
Поэтому мы будем в цикле опрашивать проверять — пришло ли что-то от компьютера. Если в буфере что-то нашлось, то ищем в потоке начало пакета — байт $FF (крайние положения сервоприводов и значения скоростей двигателей 255 практически бесполезны — сервы упираются раньше, а скорость 250-255 не отличается, поэтому в потоке такое значение будет встречаться крайне редко и это позволит нам выловить начало пакета, можно увеличить надежность, усложнив алгоритм, но нам вполне хватит и этого).
Обнаружив заголовок, принимаем байт, в котором закодировано направление двигателей по 2 бита на двигатель. Затем считываем скорости двигателей — по 1 байту на двигатель (lSpeed, rSpeed) и положения сервоприводов камеры (pan, tilt).

  if (Serial.available()>0) {
    Header=Serial.read();
    //If header found then get and process Cmd
    if (Header==255){
      while(Serial.available()<5){};

      Direction=Serial.read();
      lSpeed=Serial.read();
      rSpeed=Serial.read();
      pan=Serial.read();
      tilt=Serial.read();

Дальше выделяем направления для правого и левого двигателей

      lDirection=Direction & 0x03;
      rDirection=(Direction & 0x0C) >> 2;

и если направление или скорость изменились с последней принятой команды, то устанавливаем скорости двигателей и вращаем камеру.

Основной рабочий цикл
void loop() {

  if (Serial.available()>0) {
    Header=Serial.read();
    //If header found then get and process Cmd
    if (Header==255){
      while(Serial.available()<5){};       Direction=Serial.read();       lSpeed=Serial.read();       rSpeed=Serial.read();       pan=Serial.read();       tilt=Serial.read();              lDirection=Direction & 0x03;       rDirection=(Direction & 0x0C) >> 2;
      //Left
     if ((lastlDir!=lDirection) or (lastlSpeed!=lSpeed)){
      switch (lDirection){
        case 0:
          lMotor.run(RELEASE);
          break;
        case 1:
          lMotor.run(FORWARD);
          lMotor.setSpeed(lSpeed);
          break;
        case 2:
          lMotor.run(BACKWARD);
          lMotor.setSpeed(lSpeed);
          break;
      }
      lastlDir=lDirection;
      lastlSpeed=lSpeed;
     }

      //Right
     if ((lastrDir!=rDirection) or (lastrSpeed!=rSpeed)){
      switch (rDirection){
        case 0:
          rMotor.run(RELEASE);
          break;
        case 1:
          rMotor.run(FORWARD);
          rMotor.setSpeed(rSpeed);
          break;
        case 2:
          rMotor.run(BACKWARD);
          rMotor.setSpeed(rSpeed);
          break;
      }
      lastrDir=rDirection;
      lastrSpeed=rSpeed;
     }

      //Rotate camera
      currentMillis = millis();
      if(currentMillis - previousMillis > interval) {
        previousMillis = currentMillis; 
        if (lastPan!=pan) panServo.write(pan);			// tell pan servo to go to position
        if (lastTilt!=tilt) tiltServo.write(tilt);		// tell tilt servo to go to position
        lastPan=pan;
        lastTilt=tilt;
      }

    }
  }
}
Как видите, проще уже практически некуда 🙂
Скачать скетч можно со страницы проекта в Google code.

Исполнять команды мы шасси научили. Теперь надо научиться отправлять их.
Кому лень разбираться в программировании или неохота ставить Delphi, могут скачать скомпилированный вариант

(работает с джойстиком Logitech Extreme 3D Pro или китайским геймпэдом EasyTouch).
imageimage
С остальными идем дальше 🙂

Нам понадобится:

  • Delphi 2010 (можно и Delphi 7, просто пару строк подправить нужно в файле проекта)
  • Компонент TComPort из открытой ComPort Library (у меня установлена 4.11с)
  • Компоненты TjvHIDDevice, TjvHIDDeviceController из JEDI VCL. Я использую v3.38, вы можете скачать посвежее. Ставьте целиком, пригодится

Вычисление скорости моторов для задания направления движения

Движение вперед и назад сложностей не вызывает — просто задаем одинаковые скорости левого и правого моторово и одинаковое направление.
Для поворотов в движении вводим понятие Steer — значение отклонения от прямого движения. Скорости двигателей вычисляем для движения вперед и назад так:

  if Speed>0 then begin
    //Forward
    //Left/Right turn
    lSpeed:=Speed-Steer;
    rSpeed:=Speed+Steer;
    if lSpeedMaxSpeed then lSpeed:=MaxSpeed;
    if rSpeed>MaxSpeed then rSpeed:=MaxSpeed;
  end else begin
    //Backward
    //Left/Right turn
    lSpeed:=Speed+Steer;
    rSpeed:=Speed-Steer;
    if lSpeed>0 then  lSpeed:=0;
    if rSpeed>0 then  rSpeed:=0;
    if lSpeed<(-MaxSpeed) then  lSpeed:=-MaxSpeed;
    if rSpeed<(-MaxSpeed) then  rSpeed:=-MaxSpeed;
  end;

То есть при движении вперед из скорости левого двигателя отклонение вычитаем, к скорости правого — прибавляем. Получается эффект подтормаживания одной из гусениц и шасси плавно поворачивает, не останавливаясь полностью. При движении назад знаки просто меняются.
Ну и проверяем, не вышла ли скорость за максимально допустимые значения. В частности, это пригодится тем, у кого питание моторов напряжением выше чем им положено — просто ограничьте максимальную скорость и моторы будут целы.
Примеры по управлению:
ехать вперед — направление обоим моторам «1», скорость одинаковую
ехать назад — направление обоим моторам «2», скорость одинаковую
для поворота в вдвижении влево/вправо задаем направление одинаковое, скорости разные. Поворачивает в сторону, скорость которой меньше.
для поворота на месте — скорость одинаковая, направление моторов разное — развернется вокруг центра.
остановка — обоим моторам направление «0»

Передача управляющих пакетов по bluetooth

При добавлении bluetooth модуля в ПК образуется 2 виртуальных COM порта — один входящий, один исходящий.
Для подключения к роботу нужно всего навсего открыть исходящий порт. Можно определить в списке портов в настройках bluetooth или методом перебора — при подключении к правильному программа не будет ругаться и светодиод на модуле перестает мигать — соединение установлено, можно считать, что мы подключены напрямую к роботу.

procedure TfTank.bConnectClick(Sender: TObject);
begin
  if Tank.Connected then begin
    Tank.Disconnect;
    bConnect.Caption:='Connect';
  end else begin
    Tank.Port:=cbPort.Text;
    Tank.Connect;
    bConnect.Caption:='Disconnect';
    MessageBeep(MB_ICONINFORMATION);
  end;

end;

Для удобства я написал небольшой класс TRCTank, который реализует все действия по связи с роботом.

  TRCTank=class
  private
    fPort:string;
    ComPort:TComPort;
    Cmd, lastCmd:TControlPacket;
    fConnected:Boolean;
    function isConnected: boolean;
  protected
  public
    constructor Create;
    destructor  Destroy;override;

    procedure   Connect;
    procedure   Disconnect;

    procedure   SendCommand(lDir,left, rDir, right, pan, tilt:Byte);

    property    Port:string read fPort write fPort;
    property    Connected:boolean read isConnected;
  end;

Connect и Disconnect по сути просто открывают/закрывают порт ну и проверяют, текущее состояние, чтобы не пытаться открыть открытый или закрыть закрытый порт.

Чтобы послать команду роботу формируем заголовок, который будет ловить робот (у нас байт с кодом 255). А затем записываем команды в том порядке, как их ждет робот. Получается такая структура

  TControlPacket=record
    Header,
    Direction,
    lSpeed,     //left motor speed
    rSpeed  :Byte;//right motor speed
    pan,
    tilt    :Byte; //Camera pan & tilt
  end;

В функции отправки команды единственный момент, который стоит упомянуть — упаковка направлений для обоих двигателей в один байт смещением на 2 бита. Остальное очевидно.

procedure TRCTank.SendCommand;
begin
  if not fConnected then Exit;

  Cmd.Header:=255;
  Cmd.Direction:=lDir + rDir shl 2;
  Cmd.lSpeed:=left;
  Cmd.rSpeed:=right;
  Cmd.pan:=pan;
  Cmd.tilt:=tilt;

  if (lastCmd.Direction=Cmd.Direction) and
    (lastCmd.lSpeed=Cmd.lSpeed) and
    (lastCmd.rSpeed=Cmd.rSpeed) and
    (lastCmd.pan=Cmd.pan) and
    (lastCmd.tilt=Cmd.tilt) then Exit;

  ComPort.Write(cmd, SizeOf(cmd));
  lastCmd:=Cmd;
end;

Подключение джойстика для удобства управления

К сожалению, документации по работе с HID устройствами толковой в интернете не так много. В итоге я перебрал кучу устаревших кодов, которые отправляют к работе еще через midi порт или рассматривают джойстики как устройство с 2мя осями и 4 кнопками. Меня такой вариант не устраивал. Информации по компоненту TjvJoystick вообще нигде не было, поэтому наткнулся я на него случайно. А жаль, к этому моменту я уже написал свой компонент 🙂 Так что если не поленитесь разобраться, то можете воспользоваться готовым компонентом из JEDI VCL.
Я же работаю с HID устройством напрямую и анализирую Report от него побайтово. Зато доступны все оси джойстика (у EasyTouch их 4) и все кнопки (10-12 у моих джойстиков).
Работает это так: с помощью компонента TjvHIDDeviceController на форме мы получаем список HID устройств в системе и выводим в комбобокс. Выбранный девайс отдаем объекту класса TRjoystick вызовом SelectJoystickByID(VID, PID: Word); (Выбирается по VendorID и ProductID — их можно посмотреть например в диспетчере устройств системы).
Класс TRjoystick выполняет checkout, получая возможность принимать репорты от джойстика, расшифровывает значения, устанавливает свойства кнопок и осей и вызывает процедуру обработчика. В нашей программе обработчик выглядит так:

Обработчик OnJoyData
procedure TfTank.OnJoyData;
var
  Hat:THatPosition;
  CenterCamera:Boolean;
begin
  Hat:=hCenter;
  CenterCamera:=False;

  //Easy touch joystick
  if (joyPID=6) and (joyVID=121) then begin
    scrPitch.Position:=TREasyTouchJoystick(Joy).rZ;
    scrAileron.Position:=TREasyTouchJoystick(Joy).Z;
    scrRudder.Position:=TREasyTouchJoystick(Joy).X;
    scrThrottle.Position:=TREasyTouchJoystick(Joy).Y;
    cbFire.Checked:=TREasyTouchJoystick(Joy).Btn1;
    cbAltFire.Checked:=TREasyTouchJoystick(Joy).Btn10;
    Hat:=TREasyTouchJoystick(Joy).Hat;
    CenterCamera:=TREasyTouchJoystick(Joy).Btn2;

    Speed:=Round(((TREasyTouchJoystick(Joy).rZ)-127)*2);
    Steer:=Round((TREasyTouchJoystick(Joy).Z)-127)*2;
  end;

  //Logitech Extreme 3D Pro
  if (joyPID=49685) and (joyVID=1133) then begin
    scrPitch.Position:=TRLogitechExtreme(Joy).Pitch;
    scrAileron.Position:=TRLogitechExtreme(Joy).Aileron;
    scrRudder.Position:=TRLogitechExtreme(Joy).Rudder;
    scrThrottle.Position:=TRLogitechExtreme(Joy).Throttle;
    cbFire.Checked:=TRLogitechExtreme(Joy).Btn1;
    cbAltFire.Checked:=TRLogitechExtreme(Joy).Btn2;
    Hat:=TRLogitechExtreme(Joy).Hat;
    CenterCamera:=TRLogitechExtreme(Joy).Btn1;

    Speed:=(TRLogitechExtreme(Joy).Pitch div 8)-255;   //4096 to -256..256
    Steer:=(TRLogitechExtreme(Joy).Aileron div 4)-127; //1024 to -127..128
  end;

  ApplyDeadZone(Speed,DeadX);
  ApplyDeadZone(Steer,DeadY);
  if Speed>MaxSpeed then Speed:=MaxSpeed;
  if Speed0 then begin
    //Forward
    //Left/Right turn
    lSpeed:=Speed-Steer;
    rSpeed:=Speed+Steer;
    if lSpeedMaxSpeed then lSpeed:=MaxSpeed;
    if rSpeed>MaxSpeed then rSpeed:=MaxSpeed;
  end else begin
    //Backward
    //Left/Right turn
    lSpeed:=Speed+Steer;
    rSpeed:=Speed-Steer;
    if lSpeed>0 then  lSpeed:=0;
    if rSpeed>0 then  rSpeed:=0;
    if lSpeed<(-MaxSpeed) then  lSpeed:=-MaxSpeed;
    if rSpeed<(-MaxSpeed) then  rSpeed:=-MaxSpeed;
  end;

  scrLeft.Position:=-lSpeed;
  scrRight.Position:=-rSpeed;

  if (cbAltFire.Checked) and (bConnect.Caption='Connect') then bConnect.OnClick(Self);

  case Hat of
    hUp: Inc(Tilt);
    hUpRight:begin
        Inc(Tilt);Dec(pan);
      end;
    hRight: Dec(pan);
    hRightDown: begin
        Dec(Pan); Dec(tilt);
      end;
    hDown: Dec(Tilt);
    hLeftDown: begin
        Inc(pan);Dec(tilt);
      end;
    hLeft: Inc(pan);
    hLeftUp: begin
        Inc(pan);Inc(tilt);
      end;
    hCenter: if CenterCamera then begin
       pan:=panCenter;
       tilt:=tiltCenter;
    end;
  end;

  //Limit Pan&Tilt range
  if panmaxPan then pan:=maxPan;
  if tilt>maxTilt then tilt:=maxTilt;

  //Show info
  lJoy.Caption:='S:'+IntToStr(Speed)+' D:'+InttoStr(Steer)+' L:'+InttoStr(lSpeed)+' R:'+InttoStr(rSpeed);
  lhat.Caption:=THatPosString[Integer(Hat)];
  //Show camera position on sliders
  scrPan.Position:=pan;
  scrTilt.Position:=tilt;

  //Send command to tank
  Command2Tank;
end;
Сначала приводим сырые значения координат осей к диапазону скоростей -256..256 и направлений -127..128.
Поскольку при линейном управлении на малых значениях скоростей моторам не хватит сил сдвинуть с места робота, то вводим небольшие мертвые зоны (опытным путем) — двигаться будет только начиная с некоторого значения скорости. (ApplyDeadZone(Speed,DeadX); ApplyDeadZone(Steer,DeadY);)
После того, как учли руль направления, проверяем, что скорости не вылезли за диапазон, показываем на форме ползунками наглядно скорости моторов.
Затем в зависимости от положения шляпы меняем направление камеры или центруем ее, также проверяем ограничения (сервы упираются механически обычно раньше, чем достигают цифровых пределов управления). отображаем положение камеры на другой паре ползунков, выводим скорости и отправляем команду танку.

procedure TfTank.Command2Tank;
begin
  lDir:=0;
  rDir:=0;

  //prepare rDir, lDir data based on tracks speed
  case  lSpeed of
   0:lDir:=0; //stop
   1..255:  lDir:=1; //forward
   -255..-1:lDir:=2; //backward
  end;

  case rSpeed of
   0:rDir:=0; //stop
   1..255:  rDir:=1; //forward
   -255..-1:rDir:=2; //backward
  end;

  Tank.SendCommand(lDir,Abs(lSpeed),rDir,Abs(rSpeed), pan, tilt);
end;

В коде есть еще различные эксперименты, куски, отвечающие за сохранение выбранных порта и джойстика, сохранение и загрузку пределов управления и центровки камеры, есть возможность вернуть управление не джойстиком, а непосредственно дергая за ползунки скорости и направления. Но это не касается основной задачи управления. Можете пользоваться кодом как вам больше нравится, даже можете «устроить BolgenOS» если захочется 🙂
Скачать исходники R BT RC Tank можно на сайте проекта в Google code.
Я попытался снять процесс управления от первого лица, но съемка экрана камерой — неблагодарное занятие, вышло довольно посредственно. Но общий смысл понятен.

Программируем управление с ПК: 14 комментариев

  1. Хороший проект. Поздравляю. Есть планы куда двигаться дальше в этом направлении? Если OpenCV, то от Delphi придется отказаться. Это отдельная тема для разговора.
    Хочу остановиться еще на управлении двигателями при помощи джойстика. Сам совсем недавно оптимизировал свой алгоритм. Мне кажется, что твой не оптимален по управлению. Попробуй вот этот:

    float X=paxis->at(0)/32768.0; // значения X джойстика
    float Y=-paxis->at(1)/32768.0; // значение Y джойстика
    int joyMotorL=0,joyMotorR=0;
    float fMotorL=Y,fMotorR=Y;
    float fDelta=2*X; // дельта рассогласования по тракам
    if(fDelta>0)
    {
    fMotorR=fMotorL-fDelta;
    if(fMotorR<-1.0)
    {
    fMotorL+=abs(fMotorR+1);
    fMotorR=-1.0;
    }
    }else
    {
    fMotorL=fMotorR-abs(fDelta);
    if(fMotorL<-1.0)
    {
    fMotorR+=abs(fMotorL+1);
    fMotorL=-1.0;
    }

    }
    joyMotorL=(int)255*fMotorL; // довожу до int и размерности годной для отправки роботу. Направление — знак этого числа: 60 вперед, -255 полный назад
    joyMotorR=(int)255*fMotorR;

    Удачи

    • Кроме OpenCV есть и другие библиотеки. На Delphi есть VisionLab. Она без исходников (исходники платные), но довольно удобная. C OpenCV я только попробовал проигрывать видео и загружать картинки, обработку не пробовал вообще.

      Насчет алгоритма управления — вполне возможно, сейчас это шасси вообще разобранное лежит, я переделал игрушечный танк под управление от МК, выпаяв его электронику — у него шасси получше.
      А вообще прямо сейчас немного не до него — занят мультикоптерами. Попозже вернусь к нему.
      Спасибо )

    • Да ладно, есть же бесплатная версия делфей.. а компоненты можно в рантайме инициализировать (насколько я помню в панель добавить их нельзя, только стандартные, могу ошибаться).
      А проект очень интересный, даже сам загорелся о чем-то подобном, пока без видео правда.

      • Да что вы? и где она? http://store.embarcadero.ru/catalog/rubric/24
        Бесплатные версии с запретом ставить компоненты были очень давно, еще Delphi2006.
        Даже академические лицензии стоят около $100, а Starter версия так и $200.
        Есть триал.
        У меня вообще складывается ощущение, что большинство рассуждающих о Delphi со временм Delphi 7 — 2006 ничего не пробовали.
        Поверьте, я был шокирован, когда увидел насколько изменилась Delphi 2010 по сравнению с Delphi 7. И в плане удобства разработки и в плане надежности и в плане развития языка- да-да, он постоянно развивается и сейчас превосходит мои потребности и знания о нем. Функционально это уже гораздо более мощный продукт. Там и 64бит и кроссплатформенность. Но и денег он стоит немалых.

        • Хм, действительно, турбо делфи эксплорер прикрыли лавочку, как ни жаль 🙁
          Спасибо, не знал.
          Кстати до сих пор сижу на делфи 7 и пока что мне все нравится, да и программы пишу только для себя.

          • В Delphi 7 все бы ничего, но нет некоторых важных вещей и есть проблема — в win7 x64 и старше проблема с отрисовкой — пропадают кнопки и другие элементы управления. Это бага в VCL, которая была исправлена в более новых версиях. На XP и младше баг не проявляется.

  2. Добрый день. Хочу построить похожего робота под другой геймпад. При компиляции делфи ругается на RProfiler.dcu и TRtimer. Помогите разобраться…

  3. Сейчас другое…
    Этот PAS точно от этого проекта?
    C:\arduino\R BT RC Tank1\RProfiler.pas(5,1): error E2029: E2029 ‘UNIT’ expected but ‘<' found
    C:\arduino\R BT RC Tank1\RProfiler.pas(8,19): error E2038: E2038 Illegal character in input file: '"' (#$22)
    C:\arduino\R BT RC Tank1\RProfiler.pas(8,32): error E2038: E2038 Illegal character in input file: '"' (#$22)
    0 Warning(s)
    34 Error(s)
    Time Elapsed 00:00:00.27

    • Точно, только версия для Delphi7. Проблема скорее всего в кодировке файла. в новых версиях UTF-8

  4. В дельфи 7 проект совсем не открывается. Она виснет. Если открывать только RProfiler.pas , то выдаются те же ошибки. Можете выложить готовый RProfiler.dcu или подскажите что исправить…

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *

Лимит времени истёк. Пожалуйста, перезагрузите CAPTCHA.