int setup()
{
init_debugging();
int result, numSensors, i;
const char *err;
//handles *Handles;
//Declare an InterfaceKit handle
// Setup the IFKit
CPhidgetInterfaceKit_create(&ifKit);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifKit, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, IKInputChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, IKSensorChangeHandler, NULL);
CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, IKOutputChangeHandler, NULL);
CPhidget_open((CPhidgetHandle)ifKit, -1);
//get the program to wait for an interface kit device to be attached
SetupLog("Waiting for interface kit to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached interface kit device
IKDisplayProperties(ifKit);
// Setup motoControl
CPhidgetMotorControl_create(&motoControl);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl, ErrorHandler, NULL);
CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, MCInputChangeHandler, NULL);
CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl, MCVelocityChangeHandler, NULL);
CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl, MCCurrentChangeHandler, NULL);
CPhidget_open((CPhidgetHandle)motoControl, -1);
SetupLog("Waiting for MotorControl to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)motoControl, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
MCDisplayProperties(motoControl);
CPhidgetMotorControl_setAcceleration (motoControl, 0, 50.00);
CPhidgetMotorControl_setAcceleration (motoControl, 1, 50.00);
// Setup AdvancedServo
CPhidgetAdvancedServo_create(&servo);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)servo, AttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)servo, DetachHandler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL);
CPhidgetAdvancedServo_set_OnPositionChange_Handler(servo, ASPositionChangeHandler, NULL);
CPhidget_open((CPhidgetHandle)servo, -1);
SetupLog("Waiting for Phidget to be attached....");
if((result = CPhidget_waitForAttachment((CPhidgetHandle)servo, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
//Display the properties of the attached device
ASDisplayProperties(servo);
CPhidgetAdvancedServo_setEngaged(servo, 0, 1);
state.ServoPosition = 0;
sensor.RightWhisker = 0;
sensor.LeftWhisker = 0;
sensor.FrontFacingIR = 0;
sensor.TopIR = 0;
state.AverageBaseLight = (float)10000;
sensor.TopLeftLight = 0;
sensor.TopRightLight = 0;
state.flashWasOn = 0;
state.wasOnBlackInLastIteration = 0;
sensor.SpinSensor = 10.0;
state.expectedMovement = None;
state.expectedFor = 0;
state.exitTrialCounter = 0;
state.stuckCounter = 0;
state.previousState = 2;
gettimeofday(&state.lastFlashSighted, NULL);
//#ifdef FREQUENCY
//state.frequency = FREQUENCY;
//#endif
#ifndef NO_POWERLIB
power_button_reset();
while(power_button_get_value()==0)
{
sleep(1);
}
#endif
return 0;
}
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;
}