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.
====== How to Program Darwin OP 2 to Walk Backward with Computer Vision======


**Author:** Yu Hang He **Email:** <hey6@unlv.nevada.edu> 
\\
**Date:** Last modified on <11/27/17>
\\
**Keywords:** Darwin OP 2, C Programming, Robotis OP 2, Computer Vision
\\ 
{{ ::yuhang:darwin_sprint:darwin_target.jpg?700 |}}
\\ 
This tutorial will demonstrate how to program Darwin OP 2 to walk backward while guided by computer vision. In previous tutorial, [[Darwin_walk_toward_target|tutorial for walking toward target]], we used BallFollower class to target the marker. However, the BallFollower class will only work with forward motion. This tutorial will provide some insight into the process of program Darwin OP 2 to walk backward with computer vision and it will take approximately 1 hour to complete. 
\\ 

===== Motivation and Audience =====

This tutorial's motivation is to demonstrate and document how to program Darwin OP 2 to walk backward. 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 =====

The BallFollower class use an algorithm that is similar to proportional controller to follow a target. Even though BallFollower class will not directly work with walking backward, the basic algorithm used in that class is applicable to backward motion. 
\\ \\ 
In the algorithm, Darwin OP 2's head pan angle is set as process variable(PV). The head pan position at 0, when Darwin OP 2 is facing directly forward, is set as setpoint(SP). The percentage difference between PV and SP is set as error. Finally, A_MOVE_AMPLITUDE is control variable(CV), output of algorithm.
\\ \\ 
Below is a demonstration of the algorithm combining with backward motion.

<code cpp | main.cpp>
int m_FollowMaxRLTurn = 35.0, m_UnitRLTurn = 1.0, m_GoalRLTurn = 0, m_RLTurn = 0;
//initialize 4 variables to be used in the algorithm. m_FollowMaxRLTurn is used to set the maximum right or 
//left turn amplitude. m_UnitRLTurn is used to set how much turn amplitude change at each iteration. 
//m_GoalRLTurn is calculated based on error. m_RLTurn is the output.

_marker_found = marker_tracker.SearchAndTracking(center);
//The algorithm must work with BallTracker class since it depends on head's pan angle, which is used as PV.

            if(Action::GetInstance()->IsRunning() == 0)
            {
		Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
                Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);

                if(Walking::GetInstance()->IsRunning() == false){
                Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0;
                Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;
                Walking::GetInstance()->Start();
                }
                //Enable motors and initialize backward motion
                
                
                if(_marker_found == 1)
                {
                    double pan = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN);
                    //receiving pan angle of Darwin OP 2's head as process variable
                    
                    double pan_range = Head::GetInstance()->GetLeftLimitAngle();
                    double pan_percent = pan / pan_range;
                    //Calculate the error as a percentage 

                    m_GoalRLTurn = m_FollowMaxRLTurn * pan_percent;
                    //Convert error percentage to changes in control variable
                    
                    if(m_RLTurn < m_GoalRLTurn)
              		m_RLTurn += m_UnitRLTurn;
              	    else if(m_RLTurn > m_GoalRLTurn)
              		m_RLTurn -= m_UnitRLTurn;
                    //Instead of directly setting control variable as output, the algorithm use incremental changes 
                    //to provide smoother transition while walking
                    
              	    Walking::GetInstance()->A_MOVE_AMPLITUDE = m_RLTurn;
                    //A_MOVE_AMPLITUDE is set to the output of algorithm
                    
                    Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0;

                }
                else
                {
                    Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0;
                    Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;
                }
             }

</code>

===== Demonstration =====

In this demonstration, I successfully compile and execute the above programs

{{ youtube>v8hwcmquTac?large }} 


{{ youtube>SHYVUATcH5I?large }} 
===== Final Words =====

This tutorial's objective was to demonstrate how to program Darwin OP 2 to walk backward with CV. 
\\
\\
For questions, clarifications, etc, Email: <hey6@unlv.nevada.edu>