[Arduino] 아두이노 코딩봇 만들기 (6) - 코딩봇 펌웨어 작성

이제 코딩봇의 펌웨어를 만들 차례입니다.

그러기에 앞서 제품의 이름을 먼저 지어주어야 합니다.

회사에서 사내 테스트 툴을 만들때에는 별별 이상한 이름으로 지어서

사용하는 사람들이 이름을 말할 때 부끄러워 하게 하는 편이었는데,

이 녀석은 따님이 쓸꺼라 평범하게 이름을 붙여줍니다.

간단히 올로와 따님의 이름중 한 글자씩 따서 올빈봇으로 지었습니다.

 

펌웨어는 블록코딩 프로그램 (Mblock)에서 시리얼을 통해 받는 명령어를

CM-50 모듈에 전달하는 역할 및 각 센서의 값들을 시리얼을 통해 전송해주는 역할을 하게 됩니다.

먼저 CM-50에 전달할 Instruction packet을 만들어 주는 것 부터 시작해 봅니다.

 

Instuction packet은 지난번 포스트에서 살펴본 바 아래와 같은 구조로 구성이 됩니다.

200(0xC8)의 ID를 가진 device에 80(0x50)번 주소에 1을 Write(0x03) 해주는 packet

그냥 간단하게 만들 수 있으면 좋겠지만, CRC 계산하는데 로직이 들어가 주어야 합니다.

CRC 계산하는 방법 역시 Dynamixel Protocol 메뉴얼에 자세히 나와 있긴 하지만

C/C++을 손 놓은지가 10년은 훌쩍 넘어가다 보니 머리가 아파와서 이미 있는 걸 사용하기로 합니다.

 

이전 포스트에서 살짝 소개한 Dynamixel2Arduino 코드에서 필요한 부분을 가져 옵니다.

https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/blob/master/src/dxl_c/protocol.h

 

Instruction packet에 대한 구조체가 아래와 같이 선언이 되어 있고

이 구조체의 내용을 채우는 함수들이 아래와 같이 정의되어 있습니다.

이제 펌웨어를 본격적으로 작성합니다.

아래에 나오는 코드들의 완성본은 아래 깃허브에서 확인 가능합니다.

https://github.com/reitn/OlbinBot

GitHub - reitn/OlbinBot: Olbin Coding bot based on Arduino and Mblock

 

protocl.h 를 이용하여 아두이노 스케치 코드에 명령어를 전달해 주는 클래스를 하나 만듭니다.

우선은 헤더파을을 아래와 같이 작성을 하고,

//// Olbin.h
#ifndef _OLBIN_H_
#define _OLBIN_H_

#include "protocol.h"
enum MOTOR_DIRECTION
{
    CW,
    CCW
};

enum ADDRESS
{
    MOTOR1 = 136,
    MOTOR2 = 138,
};

class Olbin
{
    public:
        Olbin(int protocol_version = 2, uint16_t malloc_buf_size = 256);
        InfoToMakeDXLPacket_t get_command(uint8_t id, uint16_t addr, const uint8_t *p_data, uint16_t data_length);
        InfoToMakeDXLPacket_t command_set_motor_speed(int motorIdx, MOTOR_DIRECTION direction, int32_t speed);
        

    private:
        int protocol_version;
        bool is_buf_malloced_;
        uint8_t *p_packet_buf_;
        uint16_t packet_buf_capacity_;
        InfoToMakeDXLPacket_t info_tx_packet_;
        InfoToParseDXLPacket_t info_rx_packet_;

        DXLLibErrorCode_t last_lib_err_;
};


#endif

내용을 아래와 같이 채워 줍니다.

//// olbin.cpp

#include "olbin.h"


Olbin::Olbin(int protocol_version, uint16_t malloc_buf_size)
{
    this->protocol_version = protocol_version;
    if(malloc_buf_size > 0)
    {
        p_packet_buf_ = new uint8_t[malloc_buf_size];
        if(p_packet_buf_ != nullptr)
        {
            packet_buf_capacity_ = malloc_buf_size;
            is_buf_malloced_ = true;
        }
    }
    info_tx_packet_.is_init = true;
    info_rx_packet_.is_init = true;
}

InfoToMakeDXLPacket_t  
Olbin::get_command(uint8_t id, uint16_t addr, const uint8_t *p_data, uint16_t data_length)
{
    bool ret = false;
    DXLLibErrorCode_t err = DXL_LIB_OK;
    uint8_t param_len = 0;
    param_len = 2;
    begin_make_dxl_packet(&info_tx_packet_, id, protocol_version,
        DXL_INST_WRITE, 0, p_packet_buf_, packet_buf_capacity_);
    add_param_to_dxl_packet(&info_tx_packet_, (uint8_t*)&addr, param_len);
    param_len = data_length;
    add_param_to_dxl_packet(&info_tx_packet_, (uint8_t*)p_data, param_len);
    err = end_make_dxl_packet(&info_tx_packet_);

    return info_tx_packet_;
}

InfoToMakeDXLPacket_t 
Olbin::command_set_motor_speed(int motorIdx, MOTOR_DIRECTION direction, int32_t speed)
{
    if(direction == MOTOR_DIRECTION::CCW)
    {
        speed+= 1024;
    }

    uint16_t addr;
    if(motorIdx == 1)
    {
        addr = ADDRESS::MOTOR1;
    }
    else
    {
        addr = ADDRESS::MOTOR2;
    }

    return get_command(200, addr, (uint8_t*)&speed, 2);
}

오랜만에 써보는 C/C++이라서 그런지 여기까지 오는데에도 한참 걸렸습니다. (C# 만세!!!)

get_command 함수는 id(다이나믹셀 부품들의 고유 id), addr(write 할 주소), *p_data(setting 값), data_length를 받아서

Write의 Instruction packet을 retrun 해 줍니다.

command_set_motor_speed의 경우는 motorIdx (1번/2번 모터), direction(CW/CCW), speed를 받아

get_command 함수를 통해 모터를 움직일 수 있는 instruction packet을 받아 옵니다.

여기서 CM-50만을 생각한 관계로 ID 값은 200으로 고정하였습니다.

return type은 InfoToMakeDXLPacket_t라는 구조체인데,

p_packet_buf에 실제 byte가 저장이 되고 generated_packet_length에 instruction packet의 길이가 저장이 됩니다.

 

이 파일들을 아두이노 폴더 아래 libraries/Olbin_Protocol2.0 폴더아래 위치를 시켜주면

아두이노 IDE에서 해당 파일들을 참조할 수 있습니다.

그럼 이제 아두이노 IDE를 열어 간단한 펌웨어 코드를 만들어 주면 됩니다.

돈 받고 팔아될 코드는 아니어서 대충 돌아가기만 하면 되니 마음 편하게 작성을 합니다.

#include "SoftwareSerial.h"
#include "olbin.h"

SoftwareSerial mySerial(2,3);
int distance =0;

/* Control protocol
@[Target][Length][Data]#
- Target:
  M : Motor
  L : Led
  U : Ultra Sonic
- Data:
    Motor :
      MF : Move forward
      MB : Move backward
      TR : Turn Right
      TL : Turn Left
      ST : Stop
      SL : Set speed to slow (200)
      SM : Set speed to medium (256)
      SH : Set speed to fast (300)
    LED :
      BN : Blue LED On
      BF : Blue LED Off
      RN : Red LED On
      RF : Red LED Off
    Ultra Sonic :
      Data None.
      Return [distance in cm]
*/

// CM-50 Commands (Control bytes)
int32_t robot_speed;
Olbin olbin;
InfoToMakeDXLPacket_t _CM50_command;


void setup() {
  mySerial.begin(57600);
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for Native USB only
  }

  // Generate CM-50 Commands
  olbin = Olbin();
  change_speed(256);
  Serial.println("Start");

}

우선, 올빈봇만의 시리얼 프로토콜을 정의하여 명령어를 주고 받을 수 있게 합니다.

위 코드의 주석에서 보듯 간단하게 @로 시작해서 #으로 끝나면 명령어라고 인식을 하게 하고

첫번째 string은 Target으로 [M]otor, [U]ltrasonicSensor, [L]ed 등으로 정의 합니다.

다음 string은 숫자형식으로 다음에 올 data의 length를 적어주고

length만큼의 data를 붙여주는 것으로 간단히 정의 하였습니다.

 

setup에서는 mySerial(Software Serial)을 57600 bps로 시작하여 CM-50과 통신을 할 수 있게 하고

Serial을 115200 bps로 명령어를 주고 받을 수 있도록 하였습니다.

그리고 command를 생성하기 위한 Olbin 객체를 하나 생성해 주면 됩니다.

 

void loop() {

  // CM-50 Control code
  String recv_data=Serial.readStringUntil('#');
  if(recv_data.charAt(0) == '@')
  {
    char target = recv_data.charAt(1);
    int data_len = recv_data.charAt(2) - '0';
    String data = recv_data.substring(3, 3 + data_len);

    switch(target){
      case 'M': // Motor control
        if(data.equals("MF")) {
          moveForward();
        }
        else if(data.equals("MB")){
          moveBackward();
        }
        else if(data.equals("ST")){
          stopMoving();
        }
        else if(data.equals("TR")) {
          turnRight();
        }
        else if(data.equals("TL")) {
          turnLeft();
        }
        else if(data.equals("SL")) {
          change_speed(200);
        }
        else if(data.equals("SM")) {
          change_speed(256);
        }
        else if(data.equals("SH")) {
          change_speed(300);
        }
        break;

      case 'L':
        break;
      case 'U': // Ultrasonic sensor control
        distance = getDistance();
        Serial.println(distance);
        break;
    }
  }

}

loop에서는 Serial로 들어온 string을 읽어서 Serial.readStringUntil 을 이용하여 명령어의 마지막 char (#)를 확인하고

위에서 정의된 프로토콜에 따라 명령어를 분석하여, 명령어에 따라 알맞은 함수를 호출해 줍니다.

 

void write_command(InfoToMakeDXLPacket_t command)
{
  mySerial.write(command.p_packet_buf, command.generated_packet_length);
  delay(100);
}


/// CM-50 Motor

void moveForward()
{
  _CM50_command = olbin.command_set_motor_speed(1, MOTOR_DIRECTION::CCW, robot_speed);
  write_command(_CM50_command);
  _CM50_command = olbin.command_set_motor_speed(2, MOTOR_DIRECTION::CW, robot_speed);
  write_command(_CM50_command);
}

write_command 함수는 InfoToMakeDXLPacket_t 객체를 받아

p_packet_buf의 데이터를 generated_packet_length 만큼 software serial 포트에 write 해 줍니다.

 

moveForward 함수는 첫 번째 모터는 CCW, 두 번째 모터는 CW 방향으로 회전하게 하여

로봇을 앞으로 이동시키는 역할을 합니다.

위에서 설명한 command_set_motor_speed 함수를 사용하여 packet을 생성하였습니다.

 

이와 같은 식으로 펌웨어 작성을 마친 뒤, 아두이노 IDE에서 업로드를 하고 테스트를 해볼 수 있습니다.

IDE에서 업로드 후 시리얼 모니터를 열어 명령어를 실행해 보면

@M2MF#을 통해 로봇을 앞으로 이동 시킴

원하는 대로 바퀴가 움직이는 것을 확인 할 수 있습니다.

CM-50을 사용하시거나, 아님 다른 Dynamixel 을 사용하시는 분들도 아래 깃허브에 있는 코드들을 

마음껏 수정해 가시면서 사용할 수 있습니다.

https://github.com/reitn/OlbinBot

 

이제 펌웨어 준비까지 마쳤으니,

블록코딩으로 가서 로봇의 동작을 블록코딩 툴에서 제어를 할 수 있게 만들어 주면 됩니다.