Menu
▾
▴
[Robotflow-cvs] RobotFlow/Behaviors/src Goto.cc,1.1,1.2
|
From: Carle C. <car...@us...> - 2005-02-06 14:52:38
|
Update of /cvsroot/robotflow/RobotFlow/Behaviors/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv32244 Modified Files: Goto.cc Log Message: Modified global functionnality Index: Goto.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Behaviors/src/Goto.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** Goto.cc 4 Feb 2005 14:46:37 -0000 1.1 --- Goto.cc 6 Feb 2005 14:52:26 -0000 1.2 *************** *** 19,22 **** --- 19,23 ---- #include "Behavior.h" + #include <sstream> using namespace std; *************** *** 33,43 **** * @description No description available * ! * @input_name X ! * @input_type int ! * @input_description absolute target X position (mm) ! * ! * @input_name Y ! * @input_type int ! * @input_description absolute target Y position (mm) * * @input_name HEADING_POS --- 34,40 ---- * @description No description available * ! * @input_name PARAMS ! * @input_type String ! * @input_description absolute target X and Y position (mm) in String format * * @input_name HEADING_POS *************** *** 57,68 **** * @input_description Behavior activation * - * @output_name ROTATION - * @output_type int - * @output_description Robot Rotation value (degrees/s) - * * @output_name VELOCITY * @output_type int * @output_description Robot Velocity (mm/s) * * @output_name EXPLOITATION * @output_description Behavior exploitation --- 54,65 ---- * @input_description Behavior activation * * @output_name VELOCITY * @output_type int * @output_description Robot Velocity (mm/s) * + * @output_name ROTATION + * @output_type int + * @output_description Robot Rotation value (degrees/s) + * * @output_name EXPLOITATION * @output_description Behavior exploitation *************** *** 92,95 **** --- 89,97 ---- * @parameter_type int * @parameter_value 3 + * + * @parameter_name ACTIVATE_VELOCITY_ANGLE + * @parameter_description Angle range around heading that activates velocity (degrees) + * @parameter_type int + * @parameter_value 15 END*/ *************** *** 99,104 **** //inputs ! int xID; ! int yID; int headingID; int xPosID; --- 101,105 ---- //inputs ! int paramsID; int headingID; int xPosID; *************** *** 117,120 **** --- 118,122 ---- int m_goal_tol; int m_heading_tol; + int m_activate_velocity_angle; public: *************** *** 124,129 **** //inputs ! xID = addInput("X"); ! yID = addInput("Y"); headingID = addInput("HEADING_POS"); xPosID = addInput("X_POS"); --- 126,130 ---- //inputs ! paramsID = addInput("PARAMS"); headingID = addInput("HEADING_POS"); xPosID = addInput("X_POS"); *************** *** 141,144 **** --- 142,146 ---- m_goal_tol = dereference_cast<int> (parameters.get("GOAL_TOLERANCE")); m_heading_tol = dereference_cast<int> (parameters.get("HEADING_TOLERANCE")); + m_activate_velocity_angle = dereference_cast<int> (parameters.get("ACTIVATE_VELOCITY_ANGLE")); } *************** *** 147,152 **** { //cerr<<"goto activated"<<endl; ! ObjectRef xValue = getInput(xID,count); ! ObjectRef yValue = getInput(yID,count); ObjectRef headingValue = getInput(headingID,count); ObjectRef xPosValue = getInput(xPosID,count); --- 149,153 ---- { //cerr<<"goto activated"<<endl; ! ObjectRef paramsValue = getInput(paramsID,count); ObjectRef headingValue = getInput(headingID,count); ObjectRef xPosValue = getInput(xPosID,count); *************** *** 154,173 **** ! if (!xValue->isNil() && !yValue->isNil() && !headingValue->isNil() && !xPosValue->isNil() && !yPosValue->isNil()) { ! int x = dereference_cast<int>(xValue); ! int y = dereference_cast<int>(yValue); ! int heading = dereference_cast<int>(headingValue); ! int xpos = dereference_cast<int>(xPosValue); ! int ypos = dereference_cast<int>(yPosValue); if(xpos < x - m_goal_tol || xpos > x + m_goal_tol || ypos < y - m_goal_tol || ypos > y + m_goal_tol) { ! double phi = atan((double)((ypos-y)/(xpos-x))); ! double newHeading = heading; ! std::cout << "heading: " << heading << std::endl; ! if(xpos > x) { --- 155,189 ---- ! if (!paramsValue->isNil() && !headingValue->isNil() && !xPosValue->isNil() && !yPosValue->isNil()) { + RCPtr<String> params = paramsValue; + std::stringstream sstr(params->val()); ! double x; ! double y; ! ! sstr >> x; ! sstr >> y; ! ! double heading = (double)(dereference_cast<int>(headingValue)); ! ! if(heading < 0) ! heading = 360.0 + heading; ! double xpos = (double)(dereference_cast<int>(xPosValue)); ! double ypos = (double)(dereference_cast<int>(yPosValue)); ! ! std::cout << "x: " << x << std::endl; ! std::cout << "y: " << y << std::endl; ! std::cout << "xpos: " << xpos << std::endl; ! std::cout << "ypos: " << ypos << std::endl; ! std::cout << "heading: " << heading << std::endl; if(xpos < x - m_goal_tol || xpos > x + m_goal_tol || ypos < y - m_goal_tol || ypos > y + m_goal_tol) { ! double phi = abs(atan((double)((ypos-y)/(xpos-x)))); ! std::cout << "phi: " << phi << std::endl; ! double newHeading = 0; ! if(xpos > x) { *************** *** 193,200 **** } ! newHeading = ((180 * newHeading) / M_PI) + m_heading_offset; double diffHeading = heading - newHeading; std::cout << "newHeading: " << newHeading << std::endl; std::cout << "diffHeading: " << diffHeading << std::endl; --- 209,219 ---- } ! newHeading = ((180.0 * newHeading) / M_PI) + (double)(m_heading_offset); ! if(newHeading < 0) ! newHeading = 360.0 + newHeading; double diffHeading = heading - newHeading; + std::cout << "newHeading: " << newHeading << std::endl; std::cout << "diffHeading: " << diffHeading << std::endl; *************** *** 207,218 **** else { ! if(diffHeading > 0 || diffHeading > 180) { (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(-m_max_rotation)); - (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0)); } else { (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(m_max_rotation)); (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0)); } --- 226,244 ---- else { ! if((diffHeading > 0 && diffHeading < 180) || diffHeading < -180) { (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(-m_max_rotation)); } else { (*outputs[rotationID].buffer)[count] = ObjectRef(Int::alloc(m_max_rotation)); + } + + if(abs(diffHeading) < m_activate_velocity_angle) + { + (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(m_max_velocity/5)); + } + else + { (*outputs[velocityID].buffer)[count] = ObjectRef(Int::alloc(0)); } *************** *** 232,235 **** --- 258,263 ---- std::cout << std::endl; } + + }//object valid else |
