DASL Wiki

User Tools

Site Tools

Trace:
darwin_walk_toward_target

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
darwin_walk_toward_target [2017/11/19 17:26] – [Demonstration] yuhanghedarwin_walk_toward_target [2017/11/20 16:12] (current) – [Motivation and Audience] yuhanghe
Line 18: Line 18:
  
   * Familiar with handling Darwin OP 2   * Familiar with handling Darwin OP 2
-  * Familiar with C++/C programming language +  * Familiar with Cplusplus/C programming language 
-  * Familiar with C++/C codes on Darwin OP 2+  * Familiar with Cplusplus/C codes on Darwin OP 2
  
 The rest of this tutorial is presented as follows: The rest of this tutorial is presented as follows:
-  * [[new_mode_demonstration#Programming | Programming]] +  * [[darwin_walk_toward_target#Programming | Programming]] 
-  * [[new_mode_demonstration#Demonstration | Demonstration]] +  * [[darwin_walk_toward_target#Demonstration | Demonstration]] 
-  * [[new_mode_demonstration#Final_Words | Final Words]]+  * [[darwin_walk_toward_target#Final_Words | Final Words]]
  
  
 ===== Programming ===== ===== Programming =====
  
-This tutorial will only demonstrate parts of program that needs modification to include a new mode.+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>  <code cpp | StatusCheck.h> 
Line 39: Line 41:
         MOTION,         MOTION,
         VISION,         VISION,
-        SPRINT, //Add a new mode called "NEW_MODE" to an enumerated list of modes+        SPRINT, //Add a new mode called "SPRINT" to an enumerated list of modes
         MAX_MODE         MAX_MODE
     };     };
Line 47: Line 49:
 //At the end of if statement Line 141 //At the end of if statement Line 141
 if(m_old_btn & BTN_MODE) if(m_old_btn & BTN_MODE)
-        /////////////////////////// +        ////////////////////////////// 
-        ////////////////////////// +        ///Skipping Unmodified Codes// 
-        //////////////////////////+        //////////////////////////////
         else if(m_cur_mode == SPRINT) //adding LED and play mp3 for new mode         else if(m_cur_mode == SPRINT) //adding LED and play mp3 for new mode
         {         {
             cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x04, NULL);             cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x04, NULL);
             //Signaling Darwin OP 2 which LED to turn on             //Signaling Darwin OP 2 which LED to turn on
-            LinuxActionScript::PlayMP3("../../../Data/mp3/New_Mode mode.mp3");+            LinuxActionScript::PlayMP3("../../../Data/mp3/Sprint mode.mp3");
             //play an mp3 file (you can use any mp3 file but make sure file name matches)             //play an mp3 file (you can use any mp3 file but make sure file name matches)
         }         }
     }     }
-    +        ////////////////////////////// 
 +        ///Skipping Unmodified Codes// 
 +        //////////////////////////////
 // Line 267 Add // Line 267 Add
   else if(m_cur_mode == SPRINT)   else if(m_cur_mode == SPRINT)
Line 66: Line 70:
       //initialize and enable MotionManager class       //initialize and enable MotionManager class
       m_is_started = 1;       m_is_started = 1;
-      LinuxActionScript::PlayMP3("../../../Data/mp3/Start New_Mode demonstration.mp3");+      LinuxActionScript::PlayMP3("../../../Data/mp3/Start sprint demonstration.mp3");
       //play an mp3 file (you can use any mp3 file but make sure file name matches)       //play an mp3 file (you can use any mp3 file but make sure file name matches)
  
Line 95: Line 99:
   //copied status check condition from soccer mode to new mode   //copied status check condition from soccer mode to new mode
   //These codes initialize Darwin OP 2 for movement and walking   //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>
  
 <code cpp | main.cpp> <code cpp | main.cpp>
-//L317 Add+//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:   case SPRINT:
   //implement new codes here to execute after pressing start button   //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> </code>
  
Line 111: Line 256:
 ===== Final Words ===== ===== Final Words =====
  
-This tutorial's objective was to demonstrate how to add a new mode to Darwin OP 2's demo program. Follow this tutorial to quickly implement and test new codes on Darwin OP 2.+This tutorial's objective was to demonstrate how to program Darwin OP 2 to walk toward target. 
 \\ \\
 \\ \\
darwin_walk_toward_target.1511141175.txt.gz · Last modified: by yuhanghe