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