Exemplo n.º 1
0
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;
}
Exemplo n.º 2
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;
}