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 void get_arm_state() { carmen_arm_direct_get_state(arm_state.joint_angles, arm_state.joint_currents, arm_state.joint_angular_vels, NULL); arm_state.timestamp = carmen_get_time(); }