Statistics: Posted by KurtE — Sat Jul 14, 2012 1:44 pm

Code:

// Calculate servo speeds to achieve desired pose timing
// We make the following assumptions:
// AX-12 speed is 59rpm @ 12V which corresponds to 0.170s/60deg
// The AX-12 manual states this as the 'no load speed' at 12V
// The Moving Speed control table entry states that 0x3FF = 114rpm
// and according to Robotis this means 0x212 = 59rpm and anything greater 0x212 is also 59rpm
void calculatePoseServoSpeeds(uint16 time)
{
    int i;
   uint16 travel[NUM_AX12_SERVOS], temp_goal;
   uint32 factor;

   // read the current pose only if we are not walking (no time)
   if( walk_getWalkState() == 0 ) {
      readCurrentPose();      // takes 6ms
   }   
   
   // TEST: printf("\nCalculate Pose Speeds. Time = %i \n", time);
   // determine travel for each servo
   for (i=0; i<NUM_AX12_SERVOS; i++)
   {
      // TEST: printf("\nDXL%i Current, Goal, Travel, Speed:", i+1);
      
      // process the joint offset values bearing in mind the different variable types
      temp_goal = (int16) goal_pose[i] + joint_offset[i];
      if ( temp_goal < 0 ) {
         goal_pose[i] = 0;      // can't go below 0
      }
      else if ( temp_goal > 1023 ) {
         goal_pose[i] = 1023;   // or above 1023
      }
      else {
         goal_pose[i] = (uint16) temp_goal;
      }
      
      // find the amount of travel for each servo
      if( goal_pose[i] > current_pose[i]) {
         travel[i] = goal_pose[i] - current_pose[i];
      } else {
         travel[i] = current_pose[i] - goal_pose[i];
      }
      
      // if we are walking we simply set the current pose as the goal pose to save time
      if( walk_getWalkState() != 0 ) {
         current_pose[i] = goal_pose[i];   
      }      
   
      // now we can calculate the desired moving speed
      // for 59pm the factor is 847.46 which we round to 848
      // we need to use a temporary 32bit integer to prevent overflow
      factor = (uint32) 848 * travel[i];
      goal_speed[i] = (uint16) ( factor / time );
      // if the desired speed exceeds the maximum, we need to adjust
      if (goal_speed[i] > 1023) goal_speed[i] = 1023;
      // we also use a minimum speed of 26 (5% of 530 the max value for 59RPM)
      if (goal_speed[i] < 26) goal_speed[i] = 26;
      
      // TEST: printf(" %u, %u, %u, %u", current_pose[i], goal_pose[i], travel[i], goal_speed[i]);
   }
   
}


Statistics: Posted by planius — Sun Jul 08, 2012 1:54 am

Statistics: Posted by KurtE — Mon May 21, 2012 4:24 pm