Sorry if this question has been asked before. It has been a long time since I have been up here (back in my RoboNova days). But recently I purchased a PhantomX robot up at Trossen and thought it would be fun to develop my own software for it. (Actually adapt the Arduino Phoenix code...)

I have been doing some experiments with the AX-12 servos and was wondering if anyone has come up with some algorithms or better yet functions that given a servos position (and maybe speed), and a desired new location and how much time to get there, that it can set the appropriate parameters of the servos to do that.

I know that the Arbotix software does this, but it does it by constantly sending new position commands out to each of the servos. I was wondering if you could offload most of this work to the servos themselves.

I have seen a few posts about the S Curves and the like, which I am still studying.

Thanks

Kurt

I have been doing some experiments with the AX-12 servos and was wondering if anyone has come up with some algorithms or better yet functions that given a servos position (and maybe speed), and a desired new location and how much time to get there, that it can set the appropriate parameters of the servos to do that.

I know that the Arbotix software does this, but it does it by constantly sending new position commands out to each of the servos. I was wondering if you could offload most of this work to the servos themselves.

I have seen a few posts about the S Curves and the like, which I am still studying.

Thanks

Kurt

- Code: Select all
`// 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]);

}

}