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 take approximately 1 hour to complete.

This tutorial's motivation is to demonstrate how to program Darwin OP 2 to walk toward a target. 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
• Demonstration
• Final Words

Following a target can be accomplished through BallFollower Class. There is a tutorial on Robotis' online manual that may provide further detail.
This tutorial will only demonstrate parts of program that needs modification to include program Darwin OP 2 to walk toward a target.

| StatusCheck.h

//Line 16
enum {
        INITIAL,
        READY,
        SOCCER,
        MOTION,
        VISION,
        SPRINT, //Add a new mode called "SPRINT" to an enumerated list of modes
        MAX_MODE
    };

| StatusCheck.cpp

//At the end of if statement Line 141
if(m_old_btn & BTN_MODE)
        //////////////////////////////
        ///Skipping Unmodified Codes//
        //////////////////////////////
        else if(m_cur_mode == SPRINT) //adding LED and play mp3 for new mode
        {
            cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x04, NULL);
            //Signaling Darwin OP 2 which LED to turn on
            LinuxActionScript::PlayMP3("../../../Data/mp3/Sprint mode.mp3");
            //play an mp3 file (you can use any mp3 file but make sure file name matches)
        }
    }
        //////////////////////////////
        ///Skipping Unmodified Codes//
        //////////////////////////////
// Line 267 Add
  else if(m_cur_mode == SPRINT)
  {
      MotionManager::GetInstance()->Reinitialize();
      MotionManager::GetInstance()->SetEnable(true);
      //initialize and enable MotionManager class
      m_is_started = 1;
      LinuxActionScript::PlayMP3("../../../Data/mp3/Start sprint demonstration.mp3");
      //play an mp3 file (you can use any mp3 file but make sure file name matches)
 
      // Joint Enable...
      Action::GetInstance()->m_Joint.SetEnableBody(true, true);
 
      Action::GetInstance()->Start(9);
      while(Action::GetInstance()->IsRunning() == true) usleep(8000);
 
      Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
      Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
      //Reset Gyro sensor
      MotionManager::GetInstance()->ResetGyroCalibration();
      while(1)
      {
          if(MotionManager::GetInstance()->GetCalibrationStatus() == 1)
          {
              LinuxActionScript::PlayMP3("../../../Data/mp3/Sensor calibration complete.mp3");
              break;
          }
          else if(MotionManager::GetInstance()->GetCalibrationStatus() == -1)
          {
              LinuxActionScript::PlayMP3Wait("../../../Data/mp3/Sensor calibration fail.mp3");
              MotionManager::GetInstance()->ResetGyroCalibration();
          }
          usleep(8000);
      }
  //copied status check condition from soccer mode to new mode
  //These codes initialize Darwin OP 2 for movement and walking

Modified ColorFinder class
Directory Framework/src/vision/ColorFinder.cpp

| ColorFinder.cpp

//Creted a new function that take image pointer and int reference as parameter and return center coordinates and pixel count
Point2D& ColorFinder::GetPosition(Image* hsv_img, int &count)
{
    int sum_x = 0, sum_y = 0, count = 0;
 
    Filtering(hsv_img);
 
    ImgProcess::Erosion(m_result);
    ImgProcess::Dilation(m_result);
 
    for(int y = 0; y < m_result->m_Height; y++)
    {
        for(int x = 0; x < m_result->m_Width; x++)
        {
            if(m_result->m_ImageData[m_result->m_Width * y + x] > 0)
            {
                sum_x += x;
                sum_y += y;
                count++;
            }
        }
    }
 
    if(count <= (hsv_img->m_NumberOfPixels * m_min_percent / 100) || count > (hsv_img->m_NumberOfPixels * m_max_percent / 100))
    {
        m_center_point.X = -1.0;
        m_center_point.Y = -1.0;
    }
    else
    {
        m_center_point.X = (int)((double)sum_x / (double)count);
        m_center_point.Y = (int)((double)sum_y / (double)count);
    }
 
    return m_center_point;
}

Modifed its respective header file
Directory Framework/include/ColorFinder.h

| ColorFinder.h

//Add declaration for new function
under public:
 
Point2D& GetPosition(Image* hsv_img, int &count);

| main.cpp

//inside main loop
int main() {
        //////////////////////////////
        ///Skipping Unmodified Codes//
        //////////////////////////////
//Around Line 80
 
    ColorFinder* green_finder = new ColorFinder(120, 45, 35, 0, 0.3, 40.0);
    ColorFinder* blue_finder = new ColorFinder(225, 15, 45, 0, 0.3, 50.0);
    //created 2 new ColorFinder objects to find color green and blue
 
    BallTracker marker_tracker = BallTracker();
    //created a new BallTracker object to use camera tracking
    BallFollower marker_follower = BallFollower();
    //created a new BallFollower object to follow target/marker
    int _marker_found = 0;
    //created a flag for marker
        //////////////////////////////
        ///Skipping Unmodified Codes//
        //////////////////////////////
//inside while loop
while(1) {
 
    int greenCount = 0, blueCount = 0;
    //created 2 int to store filtered pixel counts
    Point2D green, blue, center;
    //created 3 objects of Point2D class
 
else if(StatusCheck::m_cur_mode == SPRINT) //under a new mode called Sprint
    {
 
    green = green_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame, greenCount);
    //using the modified GetPosition function that also return filtered pixel counts
    blue = blue_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame, blueCount);
    //store position of green and blue color
 
    if(green.X < 0 || blue.X < 0)
        center.X = -1;
    else
        center.X = (green.X + blue.X)/2;
    if(green.Y < 0 || blue.Y < 0)
	center.Y = -1;
    else 
        center.Y = (green.Y + blue.Y)/2;
    //calculate center of blue and green    
 
    _marker_found = marker_tracker.SearchAndTracking(center);
    //use camera tracking and raise the flag if marker is found
 
    }
        //////////////////////////////
        ///Skipping Unmodified Codes//
        //////////////////////////////
//around Line 350 Add
  case SPRINT:
  //implement new codes here to execute after pressing start button
            if(Action::GetInstance()->IsRunning() == 0)
            {
                Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
                Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
                //Initialize walking module and head joints
 
                if(Walking::GetInstance()->IsRunning() == false && _ball_found != 1){
            	Walking::GetInstance()->X_MOVE_AMPLITUDE = 30.0;
                //Set forward and back step length to 30cm
 
            	Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;
                //Set yaw step length to 0
 
            	Walking::GetInstance()->Start();
            	}
 
                if(_marker_found == 1)
                {
                marker_follower.Process(marker_tracker.ball_position);
                //use BallFollower object to start following marker
 
                        if(greenCount >= 10500 || blueCount >= 10500)
                        //pixel count can be used to approximate distance, but in this case, 
                        //this stop condition is sufficient                                 
                        {
                        Walking::GetInstance()->Stop();
			fprintf(stderr, "stop\n");
			while(Walking::GetInstance()->IsRunning() == true) usleep(1000000);
                        }
                 }
                 else
                 {
                 Walking::GetInstance()->X_MOVE_AMPLITUDE = 30.0;
                 Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;
                 }       
             }

In this demonstration, I created a new mode called Sprint mode following the previous step and successfully execute it on Darwin OP 2.

This tutorial's objective was to demonstrate how to program Darwin OP 2 to walk toward target.

For questions, clarifications, etc, Email: hey6@unlv.nevada.edu