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; }