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;
}
void turnCallback( const servo_mast::mast_turn::ConstPtr& msg )
{
//change the motor position
//valid range is -23 to 232, but for most motors ~30-210
double pos = msg->position;
double acc = msg->acceleration;
double vel = msg->velocity;
bool engage = true;
if( (pos > 30.0) && (pos < 210.0) ) {
CPhidgetAdvancedServo_setPosition( servo, 0, pos );
}
else { engage = false; }
if( engage && (acc > minAcc) && (acc < maxAcc) ) {
CPhidgetAdvancedServo_setAcceleration( servo, 0, acc );
}
else { engage = false; }
if( engage && (vel > minVel) && (vel < maxVel) ) {
CPhidgetAdvancedServo_setVelocityLimit( servo, 0, vel );
}
else { engage = false; }
if( engage ) {
CPhidgetAdvancedServo_setEngaged( servo, 0, 1 );
}
}
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;
}