CPhidgetAdvancedServoHandle InitializeServos(int n, int* channels, int grip_channel, double* AccelThrottle, double* VelLimThrottle, double grip_AccelThrottle, double grip_VelLimThrottle) { int result; const char *err; double minAccel, maxAccel, minVel, maxVel; /* If servo is already defined, return. */ if (servo != NULL) { return servo; } /* Create the advanced servo object */ CPhidgetAdvancedServo_create(&servo); /* Set the handlers to be run when the device is plugged in (opened) from software, unplugged (closed) from software, or generates an error. */ CPhidget_set_OnAttach_Handler((CPhidgetHandle)servo, AttachHandler, NULL); CPhidget_set_OnDetach_Handler((CPhidgetHandle)servo, DetachHandler, NULL); CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL); /* Open the device for connections */ CPhidget_open((CPhidgetHandle)servo, -1); LOG(logINFO) << "Waiting for servos to be attached..."; if ((result = CPhidget_waitForAttachment((CPhidgetHandle)servo, 5000))) { CPhidget_getErrorDescription(result, &err); LOG(logERROR) << "Problem waiting for attachment: "<< err; return 0; } /* Set the acceleration and velocity limits of each joint angle actuator */ for (int i = 0; i < n; i++) { CPhidgetAdvancedServo_getAccelerationMin(servo, channels[i], &minAccel); CPhidgetAdvancedServo_getAccelerationMax(servo, channels[i], &maxAccel); CPhidgetAdvancedServo_setAcceleration(servo, channels[i], AccelThrottle[i]*(maxAccel - minAccel) + minAccel ); CPhidgetAdvancedServo_getVelocityMin(servo, channels[i], &minVel); CPhidgetAdvancedServo_getVelocityMax(servo, channels[i], &maxVel); CPhidgetAdvancedServo_setVelocityLimit(servo, channels[i], VelLimThrottle[i]*(maxVel - minVel) + minVel ); } /* Set the acceleration and velocity limits of the end effector */ CPhidgetAdvancedServo_getAccelerationMin(servo, grip_channel, &minAccel); CPhidgetAdvancedServo_getAccelerationMax(servo, grip_channel, &maxAccel); CPhidgetAdvancedServo_setAcceleration(servo, grip_channel, grip_AccelThrottle*(maxAccel - minAccel) + minAccel ); CPhidgetAdvancedServo_getVelocityMin(servo, grip_channel, &minVel); CPhidgetAdvancedServo_getVelocityMax(servo, grip_channel, &maxVel); CPhidgetAdvancedServo_setVelocityLimit(servo, grip_channel, grip_VelLimThrottle*(maxVel - minVel) + minVel ); return (CPhidgetAdvancedServoHandle)servo; }
int main(int argc, char* argv[]) { int result; const char *err; double position; // Init ROS node overriding SIGINT (roslaunch, ctrl-c) // and XMLRPC shutdown (rosnode kill) // See also ticket // https://code.ros.org/trac/ros/ticket/3417 // as a unified solution might appear in future ROS versions ros::init( argc, argv, "servo_mast", ros::init_options::NoSigintHandler ); signal( SIGINT, mySigIntHandler ); // Override shutdown ros::XMLRPCManager::instance()->unbind( "shutdown" ); ros::XMLRPCManager::instance()->bind( "shutdown", shutdownCallback ); ros::NodeHandle nodeHandle; //create the advanced servo object CPhidgetAdvancedServo_create( &servo ); // Set the handlers to be run when the device is plugged in or opened from software, // unplugged or closed from software, or generates an error. CPhidget_set_OnAttach_Handler( (CPhidgetHandle)servo, AttachHandler, NULL ); CPhidget_set_OnDetach_Handler( (CPhidgetHandle)servo, DetachHandler, NULL ); CPhidget_set_OnError_Handler( (CPhidgetHandle)servo, ErrorHandler, NULL ); // Registers a callback that will run when the motor position is changed. // Args: // the handle for the Phidget // the function that will be called // arbitrary pointer that will be supplied to the callback function (may be NULL) CPhidgetAdvancedServo_set_OnPositionChange_Handler( servo, PositionChangeHandler, NULL ); //open the device for connections CPhidget_open( (CPhidgetHandle)servo, -1 ); //get the program to wait for an advanced servo device to be attached printf("Waiting for Phidget to be attached...."); result = CPhidget_waitForAttachment( (CPhidgetHandle)servo, 10000 ); if( result ) { CPhidget_getErrorDescription( result, &err ); printf( "Problem attaching Phidget: %s\n", err ); return 0; } else { } CPhidgetAdvancedServo_getAccelerationMin(servo, 0, &minAcc); CPhidgetAdvancedServo_getAccelerationMax(servo, 0, &maxAcc); CPhidgetAdvancedServo_getVelocityMin(servo, 0, &minVel); CPhidgetAdvancedServo_getVelocityMax(servo, 0, &maxVel); maxVel = maxVel/320; maxAcc = maxAcc/12800; CPhidgetAdvancedServo_setAcceleration( servo, 0, maxAcc); CPhidgetAdvancedServo_setVelocityLimit( servo, 0, maxVel); display_properties( servo ); // Defaults. If user only publishes float64 messages, these will be used //CPhidgetAdvancedServo_setAcceleration( servo, 0, minAcc*2/4 ); //CPhidgetAdvancedServo_setVelocityLimit( servo, 0, maxVel/8 ); publisher = nodeHandle.advertise<std_msgs::Float64>( "mast_float2", 100 ); publisher_2 = nodeHandle.advertise<servo_mast::mast_position>("mast_position", 100); ros::Subscriber subscriber1 = nodeHandle.subscribe( "mast_turn", 1000, turnCallback ); //ros::Subscriber subscriber2 = nodeHandle.subscribe( "mast_float", 1000, floatCallback ); ros::Subscriber subscriber2 = nodeHandle.subscribe( "mast_float", 100, floatCallback ); while( !g_request_shutdown ) { std_msgs::Float64 msg; msg.data=serv_pos; //publisher.publish(msg); ros::spinOnce(); //usleep(10000); //CPhidgetAdvancedServo_setEngaged( servo, 0, 1 ); //CPhidgetAdvancedServo_setPosition( servo, 0, 180 ); //printf("Motor: 0 > Current Position: %f\n", current_pos); //usleep(100000); usleep(3000); } printf( "Closing...\n" ); shutdownStuff(); return 0; }