[ROS] OpenCR Segway (수정중)

[한창호]

Turtlebot3으로 Segway를 할 수 있다. 아두이노IDE에서 OpenCR 보드를 연결하고 소스를 컴파일하고 업로드하여 실행해보자.

세그웨이 참고: https://emanual.robotis.com/docs/en/platform/turtlebot3/locomotion/#friendslocomotion

실행동영상: https://www.youtube.com/watch?v=VAY-0xBOE2g

Type: Segway robot Features: Balancing with only two DYNAMIXEL’s by applying PID controller. Components Two DYNAMIXEL X 430 Series  (All for balancing) OpenCR1.0 Board TurtleBot3 Chassis and Battery 3D printed chassis XM430-W210

* 우분투 20.04에서 아두이노 설치

구분	설명
아두이노 설치	* Ubuntu에 아두이노 설치  https://www.arduino.cc/en/software  $ wget https://downloads.arduino.cc/arduino-ide/arduino-ide_2.2.1_Linux_64bit.zip $ unzip arduino-ide_2.2.1_Linux_64bit.zip $ sudo ln -s /home/user/arduino-ide_2.2.1_Linux_64bit/arduino-ide /usr/bin/arduino   # 심볼릭링크 $ arduino * 아두이노 설정 1) File -> Preferences -> [Additional Boards Manager URLs] 내에 다음을 복사해서 넣는다. https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json  2) Tools → Board → Boards Manager 에서 검색창에 [OpenCR]이라고 치고 OpenCR by ROBOTIS 라이브러리를 설치한다.
Filters Library 설치	세그웨이를 실행하려면 다음 library가 필요하다. - A Realtime Digital Signal Processing (DSP) Library for Arduino. - Filters library have to be downloaded and includes it arduino IDE. Github Link
OpenCR 소스	아두이노 메뉴 [File] -> [Examples ] -> [Turblebot3] -> [turtlebot3_friends] 에 세그웨이가 있다. 직접 다운 받으려면 다음 주소로 받을 수 있다. - 다운로드 사이트: https://github.com/ROBOTIS-GIT/OpenCR/tree/master $ git clone -b noetic-devel https://github.com/ROBOTIS-GIT/OpenCR.git $ cd OpenCR/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_friends/turtlebot3_segway 다운로드하고 나면 위 폴더에 segway 소스가 보인다.

 * OpenCR에 segway 소스를 굽는다.

* OpenCR - turtlebot3_segway 소스코드

OpenCR/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_friends/turtlebot3_segway/

turtlebot3_segway.h
turtlebot3_segway.ino
turtlebot3_segway_motor_driver.h
turtlebot3_segway_motor_driver.cpp

파일	코드
turtlebot3_segway.h	#ifndef TURTLEBOT3_EXAMPLE06_SEGWAY_CONFIG_H #define TURTLEBOT3_EXAMPLE06_SEGWAY_CONFIG_H #include <IMU.h> #include <Filters.h> #include "turtlebot3_segway_motor_driver.h" #define DEBUG #define PWM_LIMIT                       885 #define CONTOL_PERIOD                   7000     // in microseconds void startDynamixelControlInterrupt(); void imuInit(); void getAngle(float angle[3]); void controlSegway(); #endif // TURTLEBOT3_EXAMPLE06_SEGWAY_CONFIG_H
turtlebot3_segway.ino	/******************************************************************************* * Copyright 2016 ROBOTIS CO., LTD. *******************************************************************************/ /* Authors: Yoonseok Pyo, Leon Jung, Darby Lim, HanCheol Cho */ #include "turtlebot3_segway.h" /******************************************************************************* * Declaration for Hardware Timer (Interrupt control) *******************************************************************************/ HardwareTimer Timer(TIMER_CH1); /******************************************************************************* * Declaration for IMU *******************************************************************************/ cIMU IMU; /******************************************************************************* * Declaration for Filter *******************************************************************************/ float filterFrequency = 1.0;    // Hz FilterOnePole lowpassFilter(LOWPASS, filterFrequency); /******************************************************************************* * Declaration for motor *******************************************************************************/ Turtlebot3MotorDriver motor_driver; float angle[3] = {0.0,0.0,0.0}; //roll, pitch, yaw // initial angle offset float angle_offset = 0.0; // keyboard input char keyboard; // Set PID gain float p_gain = 4000.0; float i_gain = 2.0; float d_gain = 78.0; float control_output = 0.0; void setup() {   motor_driver.init();      // Setting for Dynamixel motors   imuInit();                  // Initialization IMU } void loop() {   if (IMU.update() > 0)  getAngle(angle); #ifdef DEBUG   if (Serial.available())   {     keyboard = Serial.read();     if (keyboard == 'u')    {      p_gain = p_gain + 1.0;    }     else if (keyboard == 'j')    {      p_gain = p_gain - 1.0;    }     else if (keyboard == 'o')    {      d_gain = d_gain + 0.1;    }     else if (keyboard == 'l')    {      d_gain = d_gain - 0.1;    }     else if (keyboard == 'i')    {      i_gain = i_gain + 0.1;    }     else if (keyboard == 'k')    {      i_gain = i_gain - 0.1;    }     else if (keyboard == 'a')    {      angle_offset = angle_offset + 0.01;    }     else if (keyboard == 's')    {      angle_offset = angle_offset - 0.01;    }   }   Serial.print(" P : ");  Serial.print(p_gain);   Serial.print(" I : ");  Serial.print(i_gain);   Serial.print(" D : ");  Serial.print(d_gain);   Serial.print(" offset : ");  Serial.print(angle_offset);   Serial.print(" output : ");  Serial.print(control_output);   Serial.print(" angle : ");  Serial.println(angle[0]); #endif } /******************************************************************************* * Get angle from IMU *******************************************************************************/ void getAngle(float angle[3]) {   float roll, pitch, yaw;   roll  = IMU.rpy[0];   pitch = IMU.rpy[1];   yaw   = IMU.rpy[2];   angle[0] = lowpassFilter.input(pitch) + angle_offset;   angle[1] = pitch + angle_offset; } void startDynamixelControlInterrupt() {   Timer.pause();   Timer.setPeriod(CONTOL_PERIOD);           // in microseconds   Timer.attachInterrupt(controlSegway);   Timer.refresh();   Timer.resume(); } /******************************************************************************* * Initialization of IMU *******************************************************************************/ void imuInit() {   IMU.begin();   IMU.SEN.acc_cali_start();   while( IMU.SEN.acc_cali_get_done() == false )   {     IMU.update();   }   // Start Dynamixel Control Interrupt   startDynamixelControlInterrupt(); } /******************************************************************************* * Control segway PWM *******************************************************************************/ void controlSegway(void) {   bool dxl_comm_result = false;   static float control_input = 0.0;   static float cur_error = 0.0, pre_error = 0.0, integral = 0.0, derivative = 0.0;   static float diff_time = 0.007;   static int16_t cnt = 0;           // timer counter   cur_error  = control_input - angle[0];   integral   = integral + (cur_error * diff_time);   derivative = (cur_error - pre_error) / diff_time;   if (cnt > 500)   {     integral = 0.0;     cnt = 0;   }   else   {     cnt++;   }   control_output = p_gain * cur_error + i_gain * integral  + d_gain * derivative;   if (control_output >= PWM_LIMIT) {  control_output = PWM_LIMIT;  }   else if (control_output <= (-1) * PWM_LIMIT)  {  control_output = (-1) * PWM_LIMIT;  }   pre_error = cur_error;   dxl_comm_result = motor_driver.controlMotor((int64_t)control_output, (int64_t)control_output);   if (dxl_comm_result == false) return; }
turtlebot3_segway_motor_driver.h	/******************************************************************************* * Copyright 2016 ROBOTIS CO., LTD. *******************************************************************************/ /* Authors: Yoonseok Pyo, Leon Jung, Darby Lim, HanCheol Cho */ #ifndef TURTLEBOT3_SEGWAY_MOTOR_DRIVER_H_ #define TURTLEBOT3_SEGWAY_MOTOR_DRIVER_H_ #include <DynamixelSDK.h> // Control table address (Dynamixel X-series) #define ADDR_X_TORQUE_ENABLE            64 #define ADDR_X_GOAL_PWM                 100 #define ADDR_X_GOAL_VELOCITY            104 #define ADDR_X_PROFILE_ACCELERATION     108 #define ADDR_X_PROFILE_VELOCITY         112 #define ADDR_X_GOAL_POSITION            116 #define ADDR_X_REALTIME_TICK            120 #define ADDR_X_PRESENT_VELOCITY         128 #define ADDR_X_PRESENT_POSITION         132 // Limit values (XM430-W210-T) #define LIMIT_X_MAX_VELOCITY            240 // Data Byte Length #define LEN_X_TORQUE_ENABLE             1 #define LEN_X_GOAL_PWM                  2 #define LEN_X_GOAL_VELOCITY             4 #define LEN_X_GOAL_POSITION             4 #define LEN_X_REALTIME_TICK             2 #define LEN_X_PRESENT_VELOCITY          4 #define LEN_X_PRESENT_POSITION          4 #define PROTOCOL_VERSION                2.0     // Dynamixel protocol version 2.0 #define DXL_LEFT_ID                     1       // ID of left rear motor #define DXL_RIGHT_ID                    2       // ID of right rear motor #define BAUDRATE                        1000000 // baud rate of Dynamixel #define DEVICENAME                      ""      // no need setting on OpenCR #define TORQUE_ENABLE                   1       // Value for enabling the torque #define TORQUE_DISABLE                  0       // Value for disabling the torque class Turtlebot3MotorDriver {  public:   Turtlebot3MotorDriver();   ~Turtlebot3MotorDriver();   bool init(void);   void closeDynamixel(void);   bool setTorque(uint8_t id, bool onoff);   bool controlMotor(int64_t left_wheel_value, int64_t right_wheel_value);  private:   uint32_t baudrate_;   float  protocol_version_;   uint8_t left_wheel_id_, right_wheel_id_;   dynamixel::PortHandler *portHandler_;   dynamixel::PacketHandler *packetHandler_;   dynamixel::GroupSyncWrite *groupSyncWritePWM_; }; #endif // TURTLEBOT3_SEGWAY_MOTOR_DRIVER_H_
turtlebot3_segway_motor_driver.cpp	/******************************************************************************* * Copyright 2016 ROBOTIS CO., LTD. *******************************************************************************/ /* Authors: Yoonseok Pyo, Leon Jung, Darby Lim, HanCheol Cho */ #include "turtlebot3_segway_motor_driver.h" Turtlebot3MotorDriver::Turtlebot3MotorDriver() : baudrate_(BAUDRATE),   protocol_version_(PROTOCOL_VERSION),   left_wheel_id_(DXL_LEFT_ID),   right_wheel_id_(DXL_RIGHT_ID) { } Turtlebot3MotorDriver::~Turtlebot3MotorDriver() {   closeDynamixel(); } bool Turtlebot3MotorDriver::init(void) {   portHandler_   = dynamixel::PortHandler::getPortHandler(DEVICENAME);   packetHandler_ = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);   // Open port   if (portHandler_->openPort())  {    ERROR_PRINT("Port is opened");  }   else  {    ERROR_PRINT("Port couldn't be opened");    return false;  }   // Set port baudrate   if (portHandler_->setBaudRate(baudrate_))  {    ERROR_PRINT("Baudrate is set");  }   else  {    ERROR_PRINT("Baudrate couldn't be set");    return false;  }   // Enable Dynamixel Torque   setTorque(left_wheel_id_, true);   setTorque(right_wheel_id_, true);   groupSyncWritePWM_ = new dynamixel::GroupSyncWrite(portHandler_, packetHandler_, ADDR_X_GOAL_PWM, LEN_X_GOAL_PWM);   return true; } bool Turtlebot3MotorDriver::setTorque(uint8_t id, bool onoff) {   uint8_t dxl_error = 0;   int dxl_comm_result = COMM_TX_FAIL;   dxl_comm_result = packetHandler_->write1ByteTxRx(portHandler_, id, ADDR_X_TORQUE_ENABLE, onoff, &dxl_error);   if(dxl_comm_result != COMM_SUCCESS)  {     packetHandler_->getTxRxResult(dxl_comm_result);   }   else if(dxl_error != 0)  {     packetHandler_->getRxPacketError(dxl_error);   } } void Turtlebot3MotorDriver::closeDynamixel(void) {   // Disable Dynamixel Torque   setTorque(left_wheel_id_, false);   setTorque(right_wheel_id_, false);   // Close port   portHandler_->closePort(); } bool Turtlebot3MotorDriver::controlMotor(int64_t left_wheel_value, int64_t right_wheel_value) {   bool dxl_addparam_result_;   int8_t dxl_comm_result_;   dxl_addparam_result_ = groupSyncWritePWM_->addParam(left_wheel_id_, (uint8_t*)&left_wheel_value);   if (dxl_addparam_result_ != true)  return false;   dxl_addparam_result_ = groupSyncWritePWM_->addParam(right_wheel_id_, (uint8_t*)&right_wheel_value);   if (dxl_addparam_result_ != true)  return false;   dxl_comm_result_ = groupSyncWritePWM_->txPacket();   if (dxl_comm_result_ != COMM_SUCCESS)  {     packetHandler_->getTxRxResult(dxl_comm_result_);     return false;   }   groupSyncWritePWM_->clearParam();   return true; }