[simspark-cvs] simspark/contrib/agentspark soccerbotbehavior.cpp, 1.1.2.1, 1.1.2.2 soccerbotbehavior.h, 1.1.2.1, 1.1.2.2

[Markus R. <rol...@us...>]

Update of /cvsroot/simspark/simspark/contrib/agentspark
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv28887

Modified Files:
      Tag: WIN32
	soccerbotbehavior.cpp soccerbotbehavior.h 
Log Message:
- fixed soccerbot movement. It does now wave its hand continuously

Index: soccerbotbehavior.h
===================================================================
RCS file: /cvsroot/simspark/simspark/contrib/agentspark/Attic/soccerbotbehavior.h,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -d -r1.1.2.1 -r1.1.2.2
*** soccerbotbehavior.h	25 Feb 2007 15:53:51 -0000	1.1.2.1
--- soccerbotbehavior.h	25 Feb 2007 17:58:25 -0000	1.1.2.2
***************
*** 83,87 ****
          ARM_ROTATE = 1,
          ARM_WAVE_1 = 2,
!         ARM_WAVE_2 = 3
      };
  
--- 83,88 ----
          ARM_ROTATE = 1,
          ARM_WAVE_1 = 2,
!         ARM_WAVE_2 = 3,
!         ARM_DOWN = 4
      };
  
***************
*** 97,100 ****
--- 98,103 ----
      void ParseUniversalJointInfo(const oxygen::Predicate& predicate);
  
+     float ClampAngle(float angle);
+ 
  protected:
      zeitgeist::Zeitgeist mZG;

Index: soccerbotbehavior.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/contrib/agentspark/Attic/soccerbotbehavior.cpp,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -d -r1.1.2.1 -r1.1.2.2
*** soccerbotbehavior.cpp	25 Feb 2007 15:53:51 -0000	1.1.2.1
--- soccerbotbehavior.cpp	25 Feb 2007 17:58:25 -0000	1.1.2.2
***************
*** 68,71 ****
--- 68,72 ----
  
      SetupJointIDMap();
+ 
      // use the scene effector to build the agent and beam to a
      // position near the center of the playing field
***************
*** 133,137 ****
  
      // try to read axis1 angle
!     if (! predicate.GetValue(iter,"ax1", sense.angle1))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint angle1!" << endl;
--- 134,138 ----
  
      // try to read axis1 angle
!     if (! predicate.GetValue(iter,"axis1", sense.angle1))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint angle1!" << endl;
***************
*** 139,143 ****
      }
      // try to read axis1 rate
!     if (! predicate.GetValue(iter,"rt1", sense.rate1))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint rate1!" << endl;
--- 140,144 ----
      }
      // try to read axis1 rate
!     if (! predicate.GetValue(iter,"rate1", sense.rate1))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint rate1!" << endl;
***************
*** 145,149 ****
      }   
      // try to read axis2 angle
!     if (! predicate.GetValue(iter,"ax2", sense.angle2))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint angle2!" << endl;
--- 146,150 ----
      }   
      // try to read axis2 angle
!     if (! predicate.GetValue(iter,"axis2", sense.angle2))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint angle2!" << endl;
***************
*** 151,155 ****
      }   
      // try to read axis2 rate
!     if (! predicate.GetValue(iter,"rt2", sense.rate2))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint rate2!" << endl;
--- 152,156 ----
      }   
      // try to read axis2 rate
!     if (! predicate.GetValue(iter,"rate2", sense.rate2))
      {
          cerr << "(SoccerbotBehavior) could not parse universal joint rate2!" << endl;
***************
*** 164,176 ****
  }
  
  
! 
! string SoccerbotBehavior::Think(const std::string& message)
  {
!     //sleep(1);
  
!     static const float gain = 0.1f;
!     static BehaviorState state = ARM_UP;
  
      // parse message and extract joint angles
      //cout << "(SoccerbotBehavior) received message " << message << endl;
--- 165,192 ----
  }
  
+ static const float gain = 0.1f;
+ static SoccerbotBehavior::BehaviorState state = SoccerbotBehavior::ARM_UP;
  
! float SoccerbotBehavior::ClampAngle(float angle)
  {
!     if (angle >= 0.0) 
!     {
!         if (angle < gain)
!         {
!             return gain;
!         }
!     } else
!     {
!         if (angle > - gain)
!         {
!             return -gain;
!         }
!     }
  
!     return angle;
! }
  
+ string SoccerbotBehavior::Think(const std::string& message)
+ {
      // parse message and extract joint angles
      //cout << "(SoccerbotBehavior) received message " << message << endl;
***************
*** 212,224 ****
      stringstream ss("");
  
- //    curAngle = mUniversalJointSenseMap[JID_LLEG_5_6].angle1;
- 
- //    if (curAngle < 40.0)
- //    {
- //        newAngle = gain * (40.0 - curAngle);
- //        ss << "(lleg_eff_5_6 0.0 " << newAngle << ")";
- //    }
- 
- #if 0
      switch(state)
      {
--- 228,231 ----
***************
*** 227,231 ****
              if (curAngle < 90.0)
              {
!                 newAngle = gain * (90.0 - curAngle);
                  ss << "(rarm_eff_1_2 0.0 " << newAngle << ")";
              }
--- 234,238 ----
              if (curAngle < 90.0)
              {
!                 newAngle = ClampAngle(gain * (90.0f - curAngle));
                  ss << "(rarm_eff_1_2 0.0 " << newAngle << ")";
              }
***************
*** 235,243 ****
              }
              break;
          case ARM_ROTATE:
              curAngle = mHingeJointSenseMap[JID_RARM_3].angle;
!             if (curAngle < 90.0)
              {
!                 newAngle = gain * (90.0 - curAngle);
                  ss << "(rarm_eff_3 " << newAngle << ")";
              }
--- 242,251 ----
              }
              break;
+ 
          case ARM_ROTATE:
              curAngle = mHingeJointSenseMap[JID_RARM_3].angle;
!             if (curAngle > -90.0f)
              {
!                 newAngle = ClampAngle(gain * (-90.0f - curAngle));
                  ss << "(rarm_eff_3 " << newAngle << ")";
              }
***************
*** 247,255 ****
              }
              break;
          case ARM_WAVE_1:
              curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
              if (curAngle < 90.0)
              {
!                 newAngle = gain * (90.0 - curAngle);
                  ss << "(rarm_eff_4 " << newAngle << ")"; 
              }
--- 255,264 ----
              }
              break;
+ 
          case ARM_WAVE_1:
              curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
              if (curAngle < 90.0)
              {
!                 newAngle = ClampAngle(gain * (90.0f - curAngle));
                  ss << "(rarm_eff_4 " << newAngle << ")"; 
              }
***************
*** 259,288 ****
              }
              break;
          case ARM_WAVE_2:
              curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
              if (curAngle > 60.0 || curAngle <= 59.5)
              {
!                 newAngle = gain * (60.0 - curAngle);
                  ss << "(rarm_eff_4 " << newAngle << ")";
              }
              else
              {
!                 state = ARM_WAVE_1;
              }
              break;
          default:   
              break;
      }
  
! #endif
! 
! //    cout << "+++" << endl;
! //    cout << "current angle: " << curAngle  << endl;
! //    cout << "desired angle: " << newAngle  << endl;
! //    cout << "(Behavior) sending string " << ss.str() << " to server" << endl;
! //    cout << "State is " << state << endl;
! //    cout << "---" << endl;
  
!     //return ss.str();
!     return string("");
  }
--- 268,309 ----
              }
              break;
+ 
          case ARM_WAVE_2:
              curAngle = mHingeJointSenseMap[JID_RARM_4].angle;
              if (curAngle > 60.0 || curAngle <= 59.5)
              {
!                 newAngle = ClampAngle(gain * (60.0f - curAngle));
                  ss << "(rarm_eff_4 " << newAngle << ")";
              }
              else
              {
!                 state = ARM_DOWN;
!             }
!             break;
! 
!         case ARM_DOWN:
!            curAngle = mUniversalJointSenseMap[JID_RARM_1_2].angle2;
!             if (curAngle > 0)
!             {
!                 newAngle = ClampAngle(-gain * 20.0f);
!                 ss << "(rarm_eff_1_2 0.0 " << newAngle << ")";
!             }
!             else
!             {
!                 state = ARM_UP;
              }
              break;
+ 
          default:   
              break;
      }
  
!     //    cout << "+++" << endl;
!     //    cout << "current angle: " << curAngle  << endl;
!     //    cout << "desired angle: " << newAngle  << endl;
!     //    cout << "(Behavior) sending string " << ss.str() << " to server" << endl;
!     //    cout << "State is " << state << endl;
!     //    cout << "---" << endl;
  
!     return ss.str();
  }