Случайно о фото

Цифровые изображения и их свойства

Давайте поговорим о разрешении изображений. А точнее - цифровых изображений. Термин "Разрешение" так часто и не всегда к месту используют, что создалась немалая путаница. Любит наука измерять, даже больше чем изучать. И разрешению придумали свою мерку. Да вот беда, мерок придумали далеко не одну. "Разрешением" называют настолько различные параметры изображений, что измеряют его теперь в dpi, ppi, lpi, и даже в MP (мегапикселах).

Подробнее ...

R/C: Случайная статья

По многочисленным просьбам я решил описать процесс создания вот такого робота с камерой на гусеничном шасси и управлением по bluetooth с помощью джойстика.

Я всегда мечтал создать робота, которым можно управлять с помощью компьютера и смотреть за его перемещениями от первого лица.
Весь процесс я условно разделил на три части:
1. Сборка робота
2. Программирование боротового микроконтроллера
3. Программирование управления с ПК

Подробнее ...



R/C и Электроника
Строим гусеничного Bluetooth-робота с камерой. Часть 3 Печать E-mail
19.01.2012 12:26

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

(Под катом бонус - 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 lSpeed<0 then lSpeed:=0;
    if rSpeed<0 then rSpeed:=0;
    if lSpeed>MaxSpeed 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, получая возможность принимать репорты от джойстика, расшифровывает значения, устанавливает свойства кнопок и осей и вызывает процедуру обработчика. В нашей программе обработчик выглядит так:
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 Speed<-MaxSpeed then Speed:=-MaxSpeed;
  if Speed>0 then begin
    //Forward
    //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 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 pan<minPan then pan:=minPan;
  if tilt<minTilt then tilt:=minTilt;
  if pan>maxPan 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.
Я попытался снять процесс управления от первого лица, но съемка экрана камерой — неблагодарное занятие, вышло довольно посредственно. Но общий смысл понятен.
 

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


Защитный код
Обновить