void moveto(int moveto_ID, double moveto_TARGET, double moveto_SPEED)
{
int TARGET = 511 + floor(moveto_TARGET*195.6+0.5);;//511 is position 0 of dynamixel for the angle computation
int SPEED = min(1023, floor(moveto_SPEED*85.693+0.5));//1023 is the dynamixel maximum speed
dxl_write_word( moveto_ID, 32, SPEED );
dxl_write_word( moveto_ID, 30, TARGET );
}
int main()
{
int CommStatus;
double position[3] = {188.5, 36, -115};//spider leg tip position from CM5 center
double angle[3] = {0, 0, 0};//corresponding values of the 3 dynamixel
double x = 64.5;//internal parameter (horizontal distance between the second dynamixel axis and the leg tip)
char c = 0;
// Open device
if( dxl_initialize(DEFAULT_PORTNUM, DEFAULT_BAUDNUM) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press any key to terminate...\n" );
getch();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
while(1)
{
printf( "Press any key to continue!(press ESC to quit)\n" );
c = getch();
//printf( "%d\n", c);
if(c == 27)
break;
switch (c)
{
case 122:
position[0] = position[0]+10;
break;
case 115:
position[0] = position[0]-10;
break;
case 113:
position[1] = position[1]+10;
break;
case 100:
position[1] = position[1]-10;
break;
case 97:
position[2] = position[2]+10;
break;
case 101:
position[2] = position[2]-10;
break;
}
// Compute angles
if (position[0]>73.5)//the first dynamixel needs a specific case if the tip goes "behind" the axis
angle[0] = atan2(position[1]-36, position[0]-73.5);
else
angle[0] = atan2(36-position[1], 73.5-position[0]);
x = ( position[0]-73.5)/ cos( angle[0])-44;
angle[2] = asin(( x*x+position[2]*position[2]-12902)/12061);
angle[1] = -atan2(position[2], x)+ atan2( -93.5*cos (angle[2]), 64.5+93.5*sin (angle[2]));
//printf( "%f %f %f %f\n", x, angle[0], angle[1], angle[2]);
moveto ( 1, angle[0], 0.5);
moveto ( 2, angle[1]-0.356, 0.5);//the offsets are the set angles between the dynamixel 0 positions and the kinematic lines
moveto ( 3, angle[2]-0.471, 0.5);
CommStatus = dxl_get_result();
if( CommStatus != COMM_RXSUCCESS )
{
PrintCommStatus(CommStatus);
break;
}
}
maybe it's obvious from this, but I'm kinda new from C++ so I have a few questions:
how do I start a new project? to do this I just copied the example and modified it. I tried to create a new project, but I missed something and it never worked.
is there an easy way to control the speed instead of the position, for example the leg tip moving left continuously if I keep q pressed?
do you know a good resource to learn C++ applied to robotics, book or website? books dedicated to robotics seem to focus mainly on building the robot, and I couldn't find C++ tutorials on robotics.
thanks! this is a great forum!Statistics: Posted by animaniac — Sat Oct 08, 2011 9:34 pm
]]>