DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== 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
\\ 
{{ ::yuhang:darwin_sprint:darwin_target.jpg?700 |}}
\\ 
This tutorial will demonstrate how to program Darwin to walk towards a target. The tutorial will incorporate materials from previous two sections: [[new_mode_demonstration| How to Add a New Mode to Darwin OP 2]] and [[darwin_color_detection| 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:
  * [[darwin_walk_toward_target#Programming | Programming]]
  * [[darwin_walk_toward_target#Demonstration | Demonstration]]
  * [[darwin_walk_toward_target#Final_Words | Final Words]]


===== Programming =====

Following a target can be accomplished through BallFollower Class. There is a [[http://support.robotis.com/en/product/darwin-op/development/tutorials/linux/walking_control.htm| 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.

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

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

Modified ColorFinder class
\\
Directory ''**Framework/src/vision/ColorFinder.cpp**''
<code 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;
}
</code>

Modifed its respective header file
\\ 
Directory ''**Framework/include/ColorFinder.h**''
<code cpp | ColorFinder.h>
//Add declaration for new function
under public:

Point2D& GetPosition(Image* hsv_img, int &count);
</code>

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

===== Demonstration =====

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

{{ youtube>HaZladeEsJo?large }} 

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