Arduino Carmer Pan/Tilt 제어 with Python

Pan/Tilt 제어 대해 알아  보자. 

Pan/Tilt는 그대 생각하는 것이다. 좌우위아래를 움직여 주는 장치이다. 

위와 같이 움직이는 모든 것을 Pan/Tile 장치라는 것이다. 

내가 제어하는 아래 사진과 같고, Alliexpree에서 구매를 했다. 플라스틱으로 되어 있는 것도 있늗데.  역시 steel이 최고다.

아래 바닥은  고정하기 위해  굴러 다니는 것을 붙였다. 

카메라는 USB to Camera 이고 시판 되고 있는 것이다.  사용하지 않는 집에 있는  것을  껍데기만 벗겨서 위아 같이 하였다. 

servo가 동작을 할때 회전력이 있어. 바닥을 고정하지 않으면 Pan/Tilt 장비가 넘어지거나 위치가 틀어지니. 잘 고정한다. 

 

Pan/Tilt with Carma operating by servo

HW는  Arduino UNO 를 사용하였고  GPIO 는 2개 port (8번, 9번)을 사용하였다. 

별도 전원은 사용하지 않고  기본 전원인 USB 5V을 사요하였다. 

통신은 serial 115200bps 이다.  

RaspPI 로 바로 동작해도 되지만, Arduino UNO 를 사용하였고, HOST PC에서 UART로 Arduino를 통해 제어를 할 것이다. (Python을 사용하여 , 확정성을 위해서, 작업성을 위해서)

카메라는 PC에서 인식하고 python onenCV2로 제어를 할 것이다. 

 

여기서는 Arduino UNO  코드만 다룬다. 

코드는 아래와 같다. (source code도 첨부하였다)

인터넷에서 찾은 코드를 약간 반영하긴 했지만.  기본 코드는  file->Examples->0.4communication->SerialEvent 예제와  Servo 제어 예제 프로그램이다. 

 

사용 방법은  방향기 w,s,a,d 를 사용하여  상하좌우를 움직이고, 

h (home)  초기 위치로 이동하고 r(reset)은 최대 동작을 하고 home 위치로 이동이다. 

사용되는 명령어는 아래 4개 이고, 추가 필요한 기능은 추가하면 된다.

 

SET_UNIT:숫자[0~90];         <= wrtite, key 한개에 대한 이동 각 step

TILT_KEY:[w,s,a,d,r,h];         <= write, unit_angle 값 만큼  각 이동

POS?                                     <= quary, return servo motor  position 값

UNIT_ANGLE?                      <= quary, return servo motor step 값 

/*
Serial Data Packet 
Main   Format : Packet1;Packet2;packet3;.....
Packet Format : tag:data

example:
SET_UNIT:10;TILT_KEY:www;
SET_UNIT:5;TILT_KEY:www;TILT_KEY:dddd;

Note: 
명령어 끝에는 세미콜론(;) 을 반드시 붙여야 한다. 
determine(구문자)는 세미콜론(;), 콜론(:)  두 가지 사용한다. 
모든 값은 문자열(String) 이다.
그러므로, int, float 값을 사용하려면, String 을 int, float 값으로 변환한다.
SCPI 통신과 유사한 data format이다.
몇 가지 기능을 추가하면,   packet format 확장이 가능하다.
Packet Format은 반드시 Tag:data 유지해야 한다. 
data 가 여러 개가 존재 한다면, determine은 콤마(,)로 정의하고 Parsing 하는 함수를 추가한다.

Packet 의 최대 길이는 128Bytes이고, Tag 최대 길이는 20Bytes이다. 응용 방법에 따라 길이를 조정한다. 
Data 저장을 point로 하면 garbage data 가 사용되므로, Malloc을 사용한다. 
*/

#include <Servo.h>

Servo sv1;  // Body, Horizontal
Servo sv2;  // Head, Vertical

#define MAX_ANGLE 180
#define MIN_ANGLE 0

#define BODY_RESET_POSITION 90
#define HEAD_RESET_POSITION 135


int ang1 = BODY_RESET_POSITION;  // Body, Horizontal
int ang2 = HEAD_RESET_POSITION;  // Head, Vertical
int unitAngle = 1;

String promptStr = "OK";      // a String to end Prompt
String inputString = "";      // a String to hold incoming data
bool stringComplete = false;  // whether the string is complete

#define MAX_PACKETS_LEN 128
#define MAX_TAGS_LEN 20

void setup() {
  sv1.attach(8);
  sv2.attach(9);
  Serial.begin(115200);
  while (!Serial) {  // 시리얼 통신 포트가 연결되기 전까지 대기
  }

  // reserve 200 bytes for the inputString:
  inputString.reserve(MAX_PACKETS_LEN);
}

void ResetPosition() {
  int PostInit[4][2] = { { MIN_ANGLE, MAX_ANGLE }, { MAX_ANGLE, MIN_ANGLE }, { MAX_ANGLE, MAX_ANGLE }, { MIN_ANGLE, MIN_ANGLE } };
  for (int i = 0; i < 4; i++) {
    sv1.write(PostInit[i][0]);
    sv2.write(PostInit[i][1]);
    delay(500);
  }

  ang1 = BODY_RESET_POSITION;  // Body
  ang2 = HEAD_RESET_POSITION;  // Head

  sv1.write(ang1);
  sv2.write(ang2);

  // Serial.println("Ready");
  // sprintf(buffer, "body: %d\nhead: %d",ang1,ang2);
  // Serial.println(buffer);
}

void loop() {
  // print the string when a newline arrives:
  if (stringComplete) {

    // Serial.println(inputString);
    inputString.replace("\n", "");  // "\n"을 빈 문자열로 대체
    inputString.replace("\r", "");  // "\r"을 빈 문자열로 대체

    char searchString = ';';
    int foundIndex = inputString.indexOf(searchString);
    if (inputString.length() > 0 and foundIndex > 0) {
      // Call the parseString method with the delimiter ","

      // First Format = [Packet;Packet1;Packet2;....]
      char *pkgStrings[MAX_TAGS_LEN];
      int pkgCount = parseString(inputString, searchString, pkgStrings);
      for (int i = 0; i < pkgCount; i++) {
        inputString = String(pkgStrings[i]);
        inputString.trim();

        char searchString = ':';
        int foundIndex = inputString.indexOf(searchString);
        if (inputString.length() > 0 and foundIndex > 0) {
          // Second Format = [Tag:Tag1:...: data,data1,data2,...]
          char *tagStrings[MAX_TAGS_LEN];
          int tagCount = parseString(inputString, ':', tagStrings);

          /* Tag Data를 Parsing하여 명령어 수행 */
          String val = tagStrings[tagCount - 1];
          /* SET_UNIT: Angle Unit */
          if (strcmp(tagStrings[0], "SET_UNIT") == 0) {
            unitAngle = val.toInt();
            if (unitAngle < 1) {
              unitAngle = 5;
            }
          }

          /* TILT_KEY: [w,d,s,a,h,r]
            Tilt control에 사용되는 Key값(char) 사용 
          */
          if (strcmp(tagStrings[0], "TILT_KEY") == 0) {
            char *input = val.c_str();
            pan_tile_key_control(input, val.length());
          }

          // parseString() 에서 Malloc을 사용했기에 Memory를 release를 반드시 해야 함
          for (int j = 0; j < tagCount; j++) { /*Serial.println(tagStrings[j]);*/
            free(tagStrings[j]);
          }
        } else {
          Serial.println("Tag: " + inputString);
        }
        free(pkgStrings[i]);  // parseString() 에서 Malloc을 사용했기에 Memory를 release를 반드시 해야 함
      }
      Serial.println(promptStr);
    } else {
      // Serial.println("Normal Data: " + inputString);

      /* POS?
        Tilt control의 Servo 위치를 가져 온다. 
      */
      char *cmpStr = inputString.c_str();
      if (strcmp(cmpStr, "POS?") == 0) {
        // Convert the sensor value to a string and Concatenate a string before printing
        Serial.println("HorPos: " + String(ang1) + "HerPos: " + String(ang2));
      }

      if (strcmp(cmpStr, "UNIT_ANGLE?") == 0) {
        Serial.println("unit: " + String(unitAngle));
      }
      Serial.println(promptStr);
    }
    // clear the string:
    inputString = "";
    stringComplete = false;
  }
}


/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

int parseString(String input, char delimiter, char **tagStrings) {
  /*
  String input : 입력 문자열
  char delimiter: 구문자
  char **tagStrings : 구문 문자열 저장
  return 구문 문자열 개수
  */
  // Tokenize the input string using the specified delimiter
  int startIndex = 0;
  int endIndex = input.indexOf(delimiter);
  int tagCount = 0;

  while (endIndex != -1) {
    // Extract the substring between startIndex and endIndex
    String tag = input.substring(startIndex, endIndex);

    // Print the extracted tag
    // Serial.println(tag);

    // Memory Malloc 으로 사용해야 함(Data 보호)
    tagStrings[tagCount] = malloc(strlen(tag.c_str()) + 1);  // + 1 for the null terminator
    if (tagStrings[tagCount] != NULL)                        // malloc() returns a NULL if you are out of memory
    {
      strcpy(tagStrings[tagCount], tag.c_str());
      ++tagCount;
    }

    // Update the startIndex and endIndex for the next iteration
    startIndex = endIndex + 1;
    endIndex = input.indexOf(delimiter, startIndex);
  }

  // Process the last tag (substring after the last delimiter)
  String tag = input.substring(startIndex);
  if (tag.length() > 0) {
    // Serial.println(tag);
    // Memory Malloc 으로 사용해야 함(Data 보호)
    tagStrings[tagCount] = malloc(strlen(tag.c_str()) + 1);  // + 1 for the null terminator
    if (tagStrings[tagCount] != NULL)                        // malloc() returns a NULL if you are out of memory
    {
      strcpy(tagStrings[tagCount], tag.c_str());
      ++tagCount;
    }
  }

  return tagCount;
}


void pan_tile_key_control(char *input, int len) {

  for (int i = 0; i < len; i++) {
    if (*input == 'a' or *input == '4') {
      ang1 += unitAngle;
      if (ang1 > MAX_ANGLE) {
        ang1 = MAX_ANGLE;
      }
      sv1.write(ang1);
    } else if (*input == 's' or *input == '2') {
      ang2 += unitAngle;
      if (ang2 > MAX_ANGLE) {
        ang2 = MAX_ANGLE;
      }
      sv2.write(ang2);
    } else if (*input == 'd' or *input == '6') {
      ang1 -= unitAngle;
      if (ang1 < unitAngle) {
        ang1 = unitAngle;
      }
      sv1.write(ang1);
    } else if (*input == 'w' or *input == '8') {
      ang2 -= unitAngle;
      if (ang2 < MIN_ANGLE) {
        ang2 = MIN_ANGLE;
      }
      sv2.write(ang2);
    } else if (*input == 'h') {
      ang1 = BODY_RESET_POSITION;  // Body
      ang2 = HEAD_RESET_POSITION;  // Head
      sv1.write(ang1);
      sv2.write(ang2);
    } else if (*input == 'r') {
      ResetPosition();
    }
    // Serial.println(*input);
    input++;
  }
}

Servo_2DOF_PanTilt.zip

0.05MB