User Tools

Site Tools


darwin_walk_toward_target

Program Darwin OP 2 to Walk toward Target

Author: Yu Hang He Email: hey6@unlv.nevada.edu
Date: Last modified on <11/19/17>
Keywords: Darwin OP 2, C Programming, Robotis OP 2

This tutorial will demonstrate how to program Darwin to walk towards a target. The tutorial will incorporate materials from previous two sections: How to Add a New Mode to Darwin OP 2 and How to Use Darwin OP 2's Computer Vision. The target is consisted of two 20cm by 20cm panels, colored green and blue, perpendicular to each other. This tutorial take approximately 3 hours to complete.

Motivation and Audience

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

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;
                 }       
             }

Demonstration

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

Final Words

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

darwin_walk_toward_target.txt · Last modified: 2017/11/20 16:12 by yuhanghe