Table of Contents
How to Program Darwin OP 2 to Walk Backward
Author: Yu Hang He Email: hey6@unlv.nevada.edu
Date: Last modified on <11/20/17>
Keywords: Darwin OP 2, C Programming, Robotis OP 2
This tutorial will demonstrate how to program Darwin OP 2 to walk backward. The walking algorithm for backward motion is included in Darwin OP 2's source code by Robotis. However, there is no on-line, or barely any, documentation of this process. This tutorial will provide some insight into the process and it will take approximately 1 hour to complete.
Motivation and Audience
This tutorial's motivation is to demonstrate and document how to program Darwin OP 2 to walk backward. This tutorial assumes the reader has the following background and interests:
- Familiar with handling Darwin OP 2
- Familiar with Cplusplus/C programming language
- Familiar with Cplusplus/C codes on Darwin OP 2
The rest of this tutorial is presented as follows:
Programming
Darwin OP 2's Walking class calculates motor positions for walking algorithm internally based on parameters. Most of these parameters can be edited through walking_tuner program. Here is a detailed tutorial from Robotis on how to use walking_tuner.
There are 3 parameter in walking_tuner that cannot be saved: Step forward/back (mm), Step right/left (mm), and Step Direction (degree)
. Instead, these variables should be updated constantly by your program during Darwin OP 2's walking process.
Below is a sample program that demonstrates Darwin OP 2 walking backward. Place both main.cpp
and Makefile
inside a new folder in directory robotisop2/Linux/project/tutorial/
. Then, type make
command to compile executable and type ./walk_backward
to execute program.
- | main.cpp
#include <stdio.h> #include <unistd.h> #include <string.h> #include <libgen.h> #include "LinuxDARwIn.h" #define INI_FILE_PATH "../../../../Data/config.ini" #define U2D_DEV_NAME "/dev/ttyUSB0" using namespace Robot; void change_current_dir() { char exepath[1024] = {0}; if(readlink("/proc/self/exe", exepath, sizeof(exepath)) != -1) chdir(dirname(exepath)); } int main(void) { printf( "\n===== Walk Backward Tutorial for DARwIn =====\n\n"); change_current_dir(); minIni* ini = new minIni(INI_FILE_PATH); //////////////////// Framework Initialize //////////////////////////// LinuxCM730 linux_cm730(U2D_DEV_NAME); CM730 cm730(&linux_cm730); if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } //Port initialization and opening, dynamixel power on MotionManager::GetInstance()->LoadINISettings(ini); Walking::GetInstance()->LoadINISettings(ini); //Load default settings for MotionManager and Walking module MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance()); motion_timer->Start(); //Create MotionManager object and registers head and walking modules, then timers are initialized. ///////////////////////////////////////////////////////////////////// /////////////////////////Capture Motor Position////////////////////// int n = 0; int param[JointData::NUMBER_OF_JOINTS * 5]; int wGoalPosition, wStartPosition, wDistance; for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++) { wStartPosition = MotionStatus::m_CurrentJoints.GetValue(id); wGoalPosition = Walking::GetInstance()->m_Joint.GetValue(id); if( wStartPosition > wGoalPosition ) wDistance = wStartPosition - wGoalPosition; else wDistance = wGoalPosition - wStartPosition; wDistance >>= 2; if( wDistance < 8 ) wDistance = 8; param[n++] = id; param[n++] = CM730::GetLowByte(wGoalPosition); param[n++] = CM730::GetHighByte(wGoalPosition); param[n++] = CM730::GetLowByte(wDistance); param[n++] = CM730::GetHighByte(wDistance); } cm730.SyncWrite(MX28::P_GOAL_POSITION_L, 5, JointData::NUMBER_OF_JOINTS - 1, param); //Capture initial position of Darwin OP 2's motor position printf("Press the ENTER key to begin!\n"); getchar(); Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true); MotionManager::GetInstance()->SetEnable(true); //Walking and MotionManager enable motors while(1) { if(Action::GetInstance()->IsRunning() == 0) { Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true); if(Walking::GetInstance()->IsRunning() == false){ Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0; //X_MOVE_AMPLITUDE is a variable in Walking class that controls Darwin OP 2's walking step length, //each unit corresponds to approximately 1 mm. Set X_MOVE_AMPLITUDE to negative numbers to program //Darwin OP 2 to walk backwards. The Walking class calculates corresponding motor positions //internally Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0; //A_MOVE_AMPLITUDE is a variable in Walking class that controls Darwin OP 2's yaw movement. Each //unit correspond approximately to 1 degree. Positive degrees for Darwin OP 2 to turn //counterclockwise, negative degree for clockwise. Walking::GetInstance()->Start(); //Start walking algorithm } Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0; Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0; } } return 0; }
- | Makefile
TARGET = walk_backward INCLUDE_DIRS = -I../../../include -I../../../../Framework/include CXX = g++ CXXFLAGS += -O2 -DLINUX -Wall $(INCLUDE_DIRS) #CXXFLAGS += -O2 -DDEBUG -DLINUX -Wall $(INCLUDE_DIRS) LFLAGS += -lpthread -ljpeg -lrt OBJECTS = main.o all: $(TARGET) clean: rm -f *.a *.o $(TARGET) core *~ *.so *.lo libclean: make -C ../../../build clean distclean: clean libclean darwin.a: make -C ../../../build $(TARGET): darwin.a $(OBJECTS) $(CXX) $(CFLAGS) $(OBJECTS) ../../../lib/darwin.a -o $(TARGET) $(LFLAGS) chmod 755 $(TARGET) # useful to make a backup "make tgz" tgz: clean mkdir -p backups tar czvf ./backups/ball_following_`date +"%Y_%m_%d_%H.%M.%S"`.tgz --exclude backups *
Demonstration
In this demonstration, I successfully compile and execute the above programs
Final Words
This tutorial's objective was to demonstrate how to program Darwin OP 2 to walk backward.
For questions, clarifications, etc, Email: hey6@unlv.nevada.edu