====== 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
\\ 
{{ ::yuhang:darwin_sprint:darwin_target.jpg?700 |}}
\\ 
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:
  * [[darwin_walk_toward_target#Programming | Programming]]
  * [[darwin_walk_toward_target#Demonstration | Demonstration]]
  * [[darwin_walk_toward_target#Final_Words | Final Words]]


===== 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 [[http://support.robotis.com/en/product/darwin-op/development/tools/walking_tuner.htm | 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.


<code cpp | 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;
}
</code>

<code make | 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 *
</code>
===== Demonstration =====

In this demonstration, I successfully compile and execute the above programs

{{ youtube>GQ8AyZpQ9NU?large }} 

===== 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>