[RIS] 2/5 Motor Encoder 값 수신 완료

• MD200T의 Source Code 분석 후 Encoder값을 ROS Message로 Publish하는 코드 추가

 

1. main.cpp

// Tick Value Publisher
ros::Publisher rightPub;
ros::Publisher leftPub;
std_msgs::Int32 right_ticks;
std_msgs::Int32 left_ticks;

• main문 전에 각 바퀴의 Encoder값을 Publish할 Publisher와 실제 Encoder 값을 저장할 Message 선언
• 다른 파일들에서 해당 변수들을 사용하기 위해 전역변수로 선언

 

// Declare left and right motor`s tick pulishers
    rightPub = nh.advertise<std_msgs::Int32>("right_ticks", 10);
    leftPub = nh.advertise<std_msgs::Int32>("left_ticks", 10);

• main문의 robot_pose_pub Publisher 선언문 아래에 위의 2 바퀴의 Encoder 값을 Publish하는 rightPub, leftPub 선언문 작성
• 각 바퀴의 Encoder 값은 Integer 32bits type으로, "right_ticks", "left_ticks" 이름의 Topic으로 선언
• std_msgs Package의 Int32 Message File 사용

 

// Publish Tick Data
void PublishTicks(void)
{
    rightPub.publish(right_ticks);
    leftPub.publish(left_ticks);
//if 0
    //ROS_INFO("\r\n");
    //ROS_INFO("mtr ticks: %d : %d\n", left_ticks, right_ticks);
//endif
}

• 소스코드의 맨 아랫 단에 Encoder 값을 실제로 Publish하는 함수 정의

 

 

2. robot.cpp

// Bring meesages from main.cpp to use to tick variable
extern std_msgs::Int32 right_ticks;
extern std_msgs::Int32 left_ticks;

• ResetOdom 함수 바로 위에서 main.cpp에서 선언한 Message right_ticks와 left_ticks를 extern을 통해 불러옴

 

// Calculate Tick Values
void CalTicks(PID_PNT_MAIN_DATA_t *pData)
{
    int32_t ticks_left;
    int32_t ticks_right;

    int32_t encoder_minimum = -2147483648;
    int32_t encoder_maximum = 2147483647;
    
    ticks_left = (pData->mtr_pos_id1)/64;
    ticks_right = (pData->mtr_pos_id2)/64;

    right_ticks.data = ticks_right; 
    left_ticks.data = ticks_left;
    
#if 0
    ROS_INFO("\r\n");
    ROS_INFO("mtr ticks: %d : %d", ticks_left, ticks_right);
#endif
}

• 소스코드 밑단에서 Encoder 값을 계산하는 함수 작성
• 32bits 환경에서 저장할 수 있는 범위인 - 2147483648 ~ 2147483647 로 Encoder값 범위 지정
• 아래의 com.hpp 파일과 Datasheet 확인 결과 PID_MAIN_DATA_t Structure의 member 중 mtr_pos_id1, 2가 각각 왼쪽, 오른쪽 바퀴의 Encoder 값을 저장함을 확인
• 이후 해당 값들을 right_ticks / left_ticks Message의 data member에 대입

 

• __attribute__((aligned(1), packed)) PID_PNT_MAIN_DATA_t
  • __attribute__ : Linux의 GCC Compiler에 존재하는 메커니즘으로, Compiler가 Program에서 사용될 함수들을 더 주의해서 최적화하고 검사하도록 함
  • 즉 함수 선언 시 Compiler에게 특성을 줌
  • 뒤의 소괄호 안에 속성 인자를 작성
  • aligned(alignment) 는 함수에 대한 최소 정렬을 Byte 단위로 나타낸다
  • 즉 Structure의 Memory를 몇 Byte 단위로 끊어 위치시킬 것인지 나타냄
• 위의 경우 Structure에 대한 Memory 주소값이 1Byte씩 나눠짐
• 따라서 총 32bytes 크기의 주소값 할당
• 관련 글

https://youngseong.tistory.com/125

[RIS] 1/5 (__attribute, stdint.h, ros::Timer)

 

 

3. com.cpp

// Import Functions from main.cpp and robot.cpp
extern int CalTicks(PID_PNT_MAIN_DATA_t *pData);    // From robot.cpp
extern void PublishTicks(void);                     // From main.cpp

 

• robot.cpp와 main.cpp 파일에서 CalTicks, PublishTicks 함수를 불러옴

 

int MdReceiveProc(void) //save the identified serial data to defined variable according to PID NUMBER data
{
	... ...
	... ...
    	... ...

	// Import Functions from 
	CalTicks(&curr_pid_pnt_main_data);  // From robot.cpp
	PublishTicks();                     // From main.cpp
                
	... ...
    	... ...
    	... ...
}

• MdReceiveProc 함수의 PubRobotPose 함수 바로 직후에 위에서 불러온 두 함수 호출

 

• 위 과정을 통해 모터 엔코더 값 수신 완료

 

실행 결과