int main( int argn, char **argv ){
int i;
// so the program does not complain
argn = argn;
argv = argv;
fprintf( stderr, " started program... \n" );
// create the model
carmen_arm_model_t arm_model;
arm_model.dev = "/dev/ttyUSB0";
arm_model.num_joints = 3;
// initialize
carmen_arm_direct_initialize( &arm_model );
// set desired angles : order is always end -> base (elbow, shoulder, hip )
double joint_angles[3] = { 0.0 , 0.0 , 2.0 };
for( i = 0; i < 200; ++i ){
fprintf( stderr, " inside control loop \n" );
carmen_arm_direct_update_joints( joint_angles );
double curr_joint_angles[3];
double curr_joint_currents[3];
double curr_joint_angular_vels[3];
int curr_gripper_closed[1];
carmen_arm_direct_get_state(curr_joint_angles, curr_joint_currents,
curr_joint_angular_vels,
curr_gripper_closed );
printf(" on iter %d \n", i );
printf("\nJoint Angles: %f %f %f\n", curr_joint_angles[0],curr_joint_angles[1],curr_joint_angles[2]);
printf("Joint Currents: %f %f %f\n", curr_joint_currents[0],curr_joint_currents[1],curr_joint_currents[2]);
printf("Joint Angular Vels: %f %f %f\n", curr_joint_angular_vels[0],curr_joint_angular_vels[1],curr_joint_angular_vels[2]);
}
// clean up
carmen_arm_direct_shutdown();
}
static int initialize_arm(void)
{
int result;
result = carmen_arm_direct_initialize(&arm_model);
if (result < 0)
return -1;
reset_time = carmen_get_time();
if (goal_joint_angles == NULL) {
goal_joint_angles = (double *) calloc(arm_model.num_joints, sizeof(double));
carmen_test_alloc(goal_joint_angles);
}
return 0;
}
