====== Program Darwin OP 2 to Walk toward Target ====== **Author:** Yu Hang He **Email:** \\ **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. //Line 16 enum { INITIAL, READY, SOCCER, MOTION, VISION, SPRINT, //Add a new mode called "SPRINT" to an enumerated list of modes MAX_MODE }; //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**'' //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**'' //Add declaration for new function under public: Point2D& GetPosition(Image* hsv_img, int &count); //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. {{ 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: