Ejemplo n.º 1
0
int Create_KIT3()
	/*-----------------
		creation du handler pour le kit interface 2
		------------------*/
{
	int err1;
	const char *errStr;

	CPhidgetInterfaceKit_create(&IFK3);
	CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK3,
			IFK3_SensorChangeHandler,
			NULL);
	CPhidget_open((CPhidgetHandle)IFK3,kit_number3);
	//wait 5 seconds for attachment
	if((err1 = CPhidget_waitForAttachment((CPhidgetHandle)IFK3, 5000))
			!= EPHIDGET_OK)
	{
		CPhidget_getErrorDescription(err1, &errStr);
		printf("Error waiting for attachment IFK3: (%d): %s\n",err1,errStr);
		return 0;
	}
	return 1;
}
Ejemplo n.º 2
0
bool attach(
			CPhidgetLEDHandle &phid,
			int serial_number)
{
    int result;
    const char *err;

    // create the LED object
    CPhidgetLED_create(&phid);

    // 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)phid,
								  AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)phid,
								  DetachHandler, NULL);
    CPhidget_set_OnError_Handler((CPhidgetHandle)phid,
								 ErrorHandler, NULL);

    //open the LED for device connections
    CPhidget_open((CPhidgetHandle)phid, -1);

    //get the program to wait for an LED device to be attached
    printf("Waiting for LED to be attached....");
    if ((result =
		 CPhidget_waitForAttachment((CPhidgetHandle)phid,
									10000))) {
        CPhidget_getErrorDescription(result, &err);
        printf("Problem waiting for attachment: %s\n", err);
        return false;
    }
    else {
        return true;
    }
}
int LED_simple()
{
	int result, i;
	const char *err;

	//Declare an LED handle
	CPhidgetLEDHandle led = 0;

	//create the LED object
	CPhidgetLED_create(&led);

	//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)led, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)led, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)led, ErrorHandler, NULL);

	//open the LED for device connections
	CPhidget_open((CPhidgetHandle)led, -1);

	//get the program to wait for an LED device to be attached
	printf("Waiting for LED to be attached....");
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)led, 10000)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem waiting for attachment: %s\n", err);
		return 0;
	}

	//Display the properties of the attached LED device
	display_properties(led);

	printf("Press any key to continue\n");
	getchar();

	//turn on the leds one at a time.
	//This example assumes LED's plugged into locations 0-9
	for(i = 0; i < 10; i++)
	{
		CPhidgetLED_setBrightness(led, i, 100); //maximum brightness is 100, 0 is off.  Can set this value to anything including and inbetween these values.
	}

	printf("Press any key to continue\n");
	getchar();

	//turn off the LEDs one at a a time
	//This example assumes LED's plugged into locations 0-9
	for(i = 0; i < 10; i++)
	{
		CPhidgetLED_setBrightness(led, i, 0); //maximum brightness is 100, 0 is off.  Can set this value to anything including and inbetween these values.
	}

	printf("Press any key to end\n");
	getchar();

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)led);
	CPhidget_delete((CPhidgetHandle)led);

	//all done, exit
	return 0;
}
Ejemplo n.º 4
0
int textlcd_simple()
{
	int result;
	const char *err;

	//Declare an TextLCD handle
	CPhidgetTextLCDHandle txt_lcd = 0;

	//create the TextLCD object
	CPhidgetTextLCD_create(&txt_lcd);

	//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)txt_lcd, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)txt_lcd, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)txt_lcd, ErrorHandler, NULL);

	//open the TextLCD for device connections
	CPhidget_open((CPhidgetHandle)txt_lcd, -1);

	//get the program to wait for an TextLCD device to be attached
	printf("Waiting for TextLCD to be attached....\n");
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)txt_lcd, 10000)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem waiting for attachment: %s\n", err);
		return 0;
	}

	//Display the properties of the attached textlcd device
	display_properties(txt_lcd);

	//read TextLCD event data
	printf("Reading.....\n");

	//Begin simulation of capabilities

	//Step 1: Write a simple message to the first row
	printf("Writing to first row. Press any key to continue\n");
	getchar();

	CPhidgetTextLCD_setDisplayString (txt_lcd, 0, "Row 1");

	//Step 2: write a simple message to the second row
	printf("Writing to second row. Press any key to continue\n");
	getchar();

	CPhidgetTextLCD_setDisplayString (txt_lcd, 1, "Row 2");

	//Step 3: turn up, turn down, and set back to default the contrast
	printf("Adjusting contrast up. Press any key to continue\n");
	getchar();

	CPhidgetTextLCD_setContrast (txt_lcd, 255); //valid range is 0 - 255, default is 0 normal viewable seems to be around 100

	printf("Restoring default contrast. Press any key to continue\n");
	getchar();

	CPhidgetTextLCD_setContrast (txt_lcd, 110);

	//Step 4: Turn on the cursor
	printf("Turn on cursor. Press any key to continue\n");
	getchar();

	CPhidgetTextLCD_setCursorOn (txt_lcd, 1);

	//Step 5: turn on the cursor blink
	printf("Turn on cursor blink. Press any key to continue\n");
	getchar();

	CPhidgetTextLCD_setCursorOn (txt_lcd, 0);

	CPhidgetTextLCD_setCursorBlink (txt_lcd, 1);

	//End simulation
	printf("Press any key to end\n");
	getchar();

	CPhidgetTextLCD_setCursorBlink (txt_lcd, 0);
	CPhidgetTextLCD_setDisplayString (txt_lcd, 0, "");
	CPhidgetTextLCD_setDisplayString (txt_lcd, 1, "");

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)txt_lcd);
	CPhidget_delete((CPhidgetHandle)txt_lcd);

	//all done, exit
	return 0;
}
Ejemplo n.º 5
0
int interfacekit_simple()
{
	int result, numSensors, i;
	const char *err;

	//Declare an InterfaceKit handle
	CPhidgetInterfaceKitHandle ifKit = 0;

	//create the InterfaceKit object
	CPhidgetInterfaceKit_create(&ifKit);

	//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)ifKit, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);

	//Registers a callback that will run if an input changes.
	//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);

	//Registers a callback that will run if the sensor value changes by more than the OnSensorChange trig-ger.
	//Requires the handle for the IntefaceKit, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL);

	//Registers a callback that will run if an output changes.
	//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL);

	//open the interfacekit for device connections
	CPhidget_open((CPhidgetHandle)ifKit, -1);

	//get the program to wait for an interface kit device to be attached
	printf("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;
	}

	int count = 0;
	bool run = true;
	power_button_reset();
	while (run)
	  {
	    printf("Hello\n");
	    int lw = whleft.state();
	    int rw = whright.state();
	    int he = hall.state();
	    int sn = sonar.state();
	    if(power_button_get_value() >= 2){
	      power_button_reset();
	    }
	    if(he == 1 && motor.moveState() == 1){
	      count +=1;
	    }
	    if(he == 0){
	      count = 0;
	    }
	    if(power_button_get_value() < 1){
	      motor.stop();
	      count = 0;
	    }else if (sn == 1) {
	      motor.right();
	    }else if(lw == 1) {
	      motor.right();
	    }else if(rw == 1) {
	      motor.left();
	    }else if(he == 1 && motor.moveState() == 1 && count>5) {
	       motor.reverse();
	       sleep(5);
	       motor.right();
	       sleep(3);
	       count =0;
	    }else{
	      motor.forwards();
	    }

	    printf("Left Whisker: %d, Right Whisker: %d, Hall Effect: %d, Sonar: %d\n", lw, rw, he, sn);
	    sleep(1);

	    
	  }

	//Display the properties of the attached interface kit device
	display_properties(ifKit);

	//read interface kit event data
	/*printf("Reading.....\n");

	//keep displaying interface kit data until user input is read
	printf("Press any key to go to next step\n");
	getchar();

	printf("Modifying sensor sensitivity triggers....\n");

	//get the number of sensors available
	CPhidgetInterfaceKit_getSensorCount(ifKit, &numSensors);

	//Change the sensitivity trigger of the sensors
	for(i = 0; i < numSensors; i++)
	{
		CPhidgetInterfaceKit_setSensorChangeTrigger(ifKit, i, 100);  //we'll just use 10 for fun
	}

	//read interface kit event data
	printf("Reading.....\n");

	//keep displaying interface kit data until user input is read
	printf("Press any key to go to next step\n");
	getchar();

	printf("Toggling Ratiometric....\n");

	CPhidgetInterfaceKit_setRatiometric(ifKit, 0);

	//read interface kit event data
	printf("Reading.....\n");*/

	//keep displaying interface kit data until user input is read
	printf("Press any key to end\n");
	getchar();

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)ifKit);
	CPhidget_delete((CPhidgetHandle)ifKit);

	//all done, exit
	return 0;
}
Ejemplo n.º 6
0
int test_interfacekit()
{
	int i,j,kit,k1,k2,macX,macY,macZ,ierr,result;
	int speed_percent[2],light_value,GREEN,RED,YELLOW;
	double accX,accY,accZ,acc_calX,acc_calY,acc_calZ;
	double tiltX,tiltY,tilt_calX,tilt_calY;
	double amean[3];
	const char *err_str;

	//creation du handler pour le kit interface 1 puis 
	//ouverture du kit interface
	printf("Attaching Interface kit 1\n");
	if(Create_KIT1()!=1)goto exit;
	printf("Interface kit is attached\n");

	printf("Do you want to attach other kit ?\n");
	printf("Kit 2              1\n");
	printf("Kit 3              2\n");
	printf("Kit LCD            3\n");
	printf("All kits          10\n");
	scanf("%d",&kit);
	if(kit==1 ||kit==10)
	{
		printf("Attaching Interface kit 2\n");
		if(Create_KIT2()!=1)goto exit;
		printf("Interface kit 2 is attached\n");
	}
	if(kit==2 ||kit==10)
	{
		printf("Attaching Interface kit 3\n");
		if(Create_KIT3()!=1)goto exit;
		printf("Interface kit 3 is attached\n");

		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_rear_left, 1);
		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_rear_right, 1);
		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_front_left, 1);
		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_front_right, 1);
		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_side_2, 1);
		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_side_4, 1);
		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_side_8, 1);
		CPhidgetInterfaceKit_setOutputState(IFK3,sensor_side_10, 1);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_rear_left, 1);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_rear_right, 1);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_front_left, 0);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_front_right, 0);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_side_2, 0);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_side_4, 0);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_side_8, 0);
		CPhidgetInterfaceKit_setSensorChangeTrigger(
				(CPhidgetInterfaceKitHandle)IFK3,
				sensor_side_10, 0);
	}
	if(kit==3|| kit==10)
	{
		printf("Attaching Text LCD \n");
		if(Create_Text_LCD()!=1)goto exit;
		printf("Text LCD attached\n");
		printf("Attaching LCD kit\n");
		if(Create_KITLCD()!=1)goto exit;
		printf("LCD kit attached\n");
	}
	//creation du handler pour la carte de controle des embrayages
	if(embrayage_number!=-1)
	{
		CPhidgetMotorControl_create(&EmbrayageControl);
		CPhidget_open((CPhidgetHandle)EmbrayageControl,embrayage_number);
	}
	if(encoder1_number!=-1)
	{
		CPhidgetEncoder_create(&ENCODER1);
		CPhidgetEncoder_set_OnPositionChange_Handler (ENCODER1,
				ENCODER1_PositionChangeHandler
				, NULL);
		CPhidget_open((CPhidgetHandle)ENCODER1,encoder1_number);
	}
	if(encoder2_number!=-1)
	{
		CPhidgetEncoder_create(&ENCODER2);
		CPhidgetEncoder_set_OnPositionChange_Handler (ENCODER2,
				ENCODER2_PositionChangeHandler
				, NULL);
		CPhidget_open((CPhidgetHandle)ENCODER2,encoder2_number);
	}
	if(motorcontrol_number!=-1)
	{
		CPhidgetMotorControl_create(&motorControl);
		CPhidget_open((CPhidgetHandle)motorControl,motorcontrol_number);
	}
	if(ir_receiver1!=-1)
	{
		CPhidgetIR_create(&ir1);
		CPhidget_set_OnAttach_Handler((CPhidgetHandle)ir1, AttachHandlerIR,NULL);
		CPhidget_set_OnDetach_Handler((CPhidgetHandle)ir1, DetachHandlerIR,NULL);
		CPhidget_set_OnError_Handler((CPhidgetHandle)ir1, ErrorHandlerIR,NULL);
		CPhidgetIR_set_OnCode_Handler(ir1, CodeHandler, NULL);
		CPhidget_open((CPhidgetHandle)ir1, ir_receiver1);
		printf("Waiting for PhidgetIR to be attached.... \n");
		if((result = CPhidget_waitForAttachment((CPhidgetHandle)ir1, 10000)))
		{
			CPhidget_getErrorDescription(result, &err_str);
			printf("Problem waiting for attachment IR1: %s\n", err_str);
			return 0;
		}
	}
	if(ir_receiver2!=-1)
	{
		CPhidgetIR_create(&ir2);
		CPhidget_set_OnAttach_Handler((CPhidgetHandle)ir2, AttachHandlerIR,NULL);
		CPhidget_set_OnDetach_Handler((CPhidgetHandle)ir2, DetachHandlerIR,NULL);
		CPhidget_set_OnError_Handler((CPhidgetHandle)ir2, ErrorHandlerIR,NULL);
		CPhidgetIR_set_OnCode_Handler(ir2, CodeHandler, NULL);
		CPhidget_open((CPhidgetHandle)ir2, ir_receiver2);
		printf("Waiting for PhidgetIR to be attached.... \n");
		if((result = CPhidget_waitForAttachment((CPhidgetHandle)ir2, 10000)))
		{
			CPhidget_getErrorDescription(result, &err_str);
			printf("Problem waiting for attachment IR2: %s\n", err_str);
			return 0;
		}
	}
	if(ir_receiver3!=-1)
	{
		CPhidgetIR_create(&ir3);
		CPhidget_set_OnAttach_Handler((CPhidgetHandle)ir3, AttachHandlerIR,NULL);
		CPhidget_set_OnDetach_Handler((CPhidgetHandle)ir3, DetachHandlerIR,NULL);
		CPhidget_set_OnError_Handler((CPhidgetHandle)ir3, ErrorHandlerIR,NULL);
		CPhidgetIR_set_OnCode_Handler(ir3, CodeHandler, NULL);
		CPhidget_open((CPhidgetHandle)ir3, ir_receiver3);
		printf("Waiting for PhidgetIR to be attached.... \n");
		if((result = CPhidget_waitForAttachment((CPhidgetHandle)ir3, 10000)))
		{
			CPhidget_getErrorDescription(result, &err_str);
			printf("Problem waiting for attachment IR3: %s\n", err_str);
			return 0;
		}
	}

	while(1)
	{
		printf("exit:                    0\n");
		printf("Embrayage:               1\n");
		printf("Buzzer,stop,inhibit      2\n");
		printf("Encoder:                 3\n");
		printf("Moteur:                  4\n");
		printf("Capteur distance:        5\n");
		printf("Capteur de contact:      6\n");
		printf("Capteur de lumiere:      7\n");
		printf("Capteur de force poignet:8\n");
		printf("Radiocommande:           9\n");
		printf("Colonne lumineuse:       10\n");
		printf("Joystick:                11\n");
		printf("etat batterie,LCD        12\n");
		printf("accelerometre            13\n");
		printf("recepteur IR             14\n");

		scanf("%d",&i);
		if(i==0)goto exit;
		//embrayage
		if(i==1)
		{
			j=-1;
			while(j!=0)
			{
				printf("clutch motor 1:        1\n");
				printf("clutch motor 2:        2\n");
				printf("unclutch motor 1:      3\n");
				printf("unclutch motor 2:      4\n");
				printf("clutch motor 1 and 2:  5\n");
				printf("unclutch motor 1 and 2:6\n");
				printf("exit                  :0\n");
				scanf("%d",&j);
				if(j==0)continue;
				if(j==1)
				{
					printf("%% de clutch entre 0 et 100 \n");
					scanf("%d",&k1);
					CPhidgetMotorControl_setVelocity (EmbrayageControl, 0,k1);
				}
				if(j==3)
				{
					CPhidgetMotorControl_setVelocity (EmbrayageControl, 0,0);
				}
				if(j==2)
				{
					printf("%% de clutch entre 0 et 100 \n");
					scanf("%d",&k1);
					CPhidgetMotorControl_setVelocity (EmbrayageControl,1,k1);
				}
				if(j==4)
				{
					CPhidgetMotorControl_setVelocity (EmbrayageControl, 1,0);
				}
				if(j==5)
				{
					printf("%% de clutch entre 0 et 100 pour embrayage 1 et 2\n");
					scanf("%d",&k1);
					scanf("%d",&k2);
					CPhidgetMotorControl_setVelocity (EmbrayageControl,0,k1);
					CPhidgetMotorControl_setVelocity (EmbrayageControl,0,k2);
				}
				if(j==6)
				{
					Embraye_Debraye(0);
				}
			}
		}
		//buzzer, stop, inhibit
		if(i==2)
		{
			j=-1;
			while(j!=0)
			{
				printf("exit                  :0\n");
				printf("run buzzer:           1\n");
				printf("stop buzzer:          2\n");
				printf("etat bouton stop:     3\n");
				printf("etat bouton inhibit:  4\n");
				scanf("%d",&j);
				if(j==0){Mesure_Stop=-1;continue;}
				if(j==1)
					CPhidgetInterfaceKit_setOutputState(IFK,buzzer_number, 1);
				if(j==2)
					CPhidgetInterfaceKit_setOutputState(IFK,buzzer_number, 0);
				if(j==3)
				{
					CPhidgetInterfaceKit_getInputState(IFK,inhibit2_number,&k1);
					printf("Bouton stop: %d\n",k1);
					Mesure_Stop=1;
				}
				if(j==4)
				{
					CPhidgetInterfaceKit_getInputState(IFK,inhibit1_number,&k1);
					printf("Bouton inhibit: %d\n",k1);
					Mesure_Stop=1;
				}
			}
		}
		//codeur
		if(i==3)
		{
			j=-1;
			while(j!=0)
			{
				printf("exit                 0\n");
				printf("get encoder 1:       1\n");
				printf("get encoder 2:       2\n");
				printf("get encoder 1,2:     3\n");
				scanf("%d",&j);
				if(j==0)continue;
				if(j==1)
				{
					wheelPos[0]=8*atan2(1.,1.)*10*encoderPos[0]/
						(encoder1_reduction*encoder1_pulse_perturn);
					printf("position encoder 1: %d roue:%f\n",encoderPos[0],
							wheelPos[0]);
				}
				if(j==2)
				{
					wheelPos[1]=8*atan2(1.,1.)*10*encoderPos[1]/
						(encoder2_reduction*encoder2_pulse_perturn);
					printf("position encoder 2: %d roue:%f\n",encoderPos[1],
							wheelPos[1]);
				}
				if(j==3)
				{
					wheelPos[0]=8*atan2(1.,1.)*10*encoderPos[0]/
						(encoder1_reduction*encoder1_pulse_perturn);
					wheelPos[1]=8*atan2(1.,1.)*10*encoderPos[1]/
						(encoder2_reduction*encoder2_pulse_perturn);
					printf("position encoder 1: %d roue:%f\n",encoderPos[0],
							wheelPos[0]);
					printf("position encoder 2: %d roue:%f\n",encoderPos[1],
							wheelPos[1]);
				}
			}
		}
		//moteur
		if(i==4)
		{
			j=-1;
			while(j!=0)
			{
				printf("exit                0\n");
				printf("control motor 1:    1\n");
				printf("control motor 2:    2\n");
				printf("control motor 1,2:  3\n");
				scanf("%d",&j);
				if(j==0)continue;
				if(j==1)
				{
					CPhidgetMotorControl_setAcceleration (motorControl, 0, 50.00);
					speed_percent[0]=-1;
					while(speed_percent[0]<0 || speed_percent[0]>100)
					{
						printf("vitesse en %%, entier ?\n");
						scanf("%d",&speed_percent[0]);
						if(speed_percent[0]<0 || speed_percent[0]>100)
						{
							printf("incorrect speed\n");
							continue;
						}
					}
					CPhidgetMotorControl_setVelocity (motorControl, 0,
							speed_percent[0]);
				}
				if(j==2)
				{
					CPhidgetMotorControl_setAcceleration (motorControl, 1, 50.00);
					speed_percent[1]=-1;
					while(speed_percent[1]<0 || speed_percent[1]>100)
					{
						printf("vitesse en %%, entier ?\n");
						scanf("%d",&speed_percent[1]);
						if(speed_percent[1]<0 || speed_percent[1]>100)
						{
							printf("incorrect speed\n");
							continue;
						}
					}
					CPhidgetMotorControl_setVelocity (motorControl, 1,
							speed_percent[1]);
				}
				if(j==3)
				{
					CPhidgetMotorControl_setAcceleration (motorControl, 0, 50.00);
					CPhidgetMotorControl_setAcceleration (motorControl, 1, 50.00);
					speed_percent[0]=-1;
					while(speed_percent[0]<0 || speed_percent[0]>100)
					{
						printf("vitesse 1 en %%, entier ?\n");
						scanf("%d",&speed_percent[0]);
						if(speed_percent[0]<0 || speed_percent[0]>100)
						{
							printf("incorrect speed\n");
							continue;
						}
					}
					speed_percent[1]=-1;
					while(speed_percent[1]<0 || speed_percent[1]>100)
					{
						printf("vitesse 2 en %%, entier ?\n");
						scanf("%d",&speed_percent[1]);
						if(speed_percent[1]<0 || speed_percent[1]>100)
						{
							printf("incorrect speed\n");
							continue;
						}
					}
					CPhidgetMotorControl_setVelocity (motorControl, 0,
							speed_percent[0]);
					CPhidgetMotorControl_setVelocity (motorControl, 1,
							speed_percent[1]);
				}
			}
		}
		//capteur de distance
		if(i==5)
		{

			j=-1;
			while(j!=0)
			{
remesure:
				for(k1=0;k1<10;k1++)
				{
					amin[k1]=10000.;
					amax[k1]=-10000.;
				}
				for(k1=0;k1<2;k1++)
				{
					amin_IR[k1]=10000.;
					amax_IR[k1]=-10000.;
				}
				printf("exit                       0\n");
				printf("IR                         1\n");
				printf("US                         2\n");
				printf("Tous                       3\n");
				printf("duree mesure (defaut:%d s) 4\n",Time_Mesure);
				printf("IR rear left              10\n");
				printf("IR rear right             11\n");
				printf("US front right             12\n");
				printf("US front left             13\n");
				printf("US side right front       14\n");
				printf("US side right rear        15\n");
				printf("US side left rear         16\n");
				printf("US side left front        17\n");
				printf("IR up left            20\n");
				printf("IR up left            21\n");
				scanf("%d",&j);
				if(j==0){Mesure_Capteur=-1;continue;}
				if(j==1){Mesure_Capteur=0;sleep(Time_Mesure);Mesure_Capteur=-1;}
				if(j==2){Mesure_Capteur=1;sleep(Time_Mesure);Mesure_Capteur=-1;}
				if(j==3){Mesure_Capteur=2;sleep(Time_Mesure);Mesure_Capteur=-1;}
				if(j>=10){Mesure_Capteur=j;sleep(Time_Mesure);Mesure_Capteur=-1;}
				if(j==4)
				{
					printf("duree entiere de mesure?\n");
					scanf("%d",&Time_Mesure);
					goto remesure;
				}
			}
		}
		//capteur de contact
		if(i==6)
		{
			j=-1;
			Kontact1_old=Kontact2_old=-1;
			CPhidgetInterfaceKit_getInputState(IFK,contact_front_left,&Kontact1);
			CPhidgetInterfaceKit_getInputState(IFK,contact_front_right,&Kontact2);
			if(Kontact1!=Kontact1_old||Kontact2!=Kontact2_old)
			{
				if(Kontact1==0 && contact_front_left_type==1)
					printf("contact front, left: NO CONTACT\n");
				if(Kontact1==1 && contact_front_left_type==1)
					printf("contact front, left: CONTACT\n");
				if(Kontact1==0 && contact_front_left_type==0)
					printf("contact front, left: CONTACT\n");
				if(Kontact1==1 && contact_front_left_type==0)
					printf("contact front, left: NO CONTACT\n");

				if(Kontact2==0 && contact_front_right_type==1)
					printf("contact front, right: NO CONTACT\n");
				if(Kontact2==1 && contact_front_right_type==1)
					printf("contact front, right: CONTACT\n");
				if(Kontact2==0 && contact_front_right_type==0)
					printf("contact front, right: CONTACT\n");
				if(Kontact2==1 && contact_front_right_type==0)
					printf("contact front, right: NO CONTACT\n");
				Kontact1_old=Kontact1;
				Kontact2_old=Kontact2;
			}
			printf("exit                       0\n");
			while(j!=0)
			{
				scanf("%d",&j);
			}
		}
		//lumiere
		if(i==7)
		{
			j=-1;


			while(j!=0)
			{
remesure_light:
				printf("exit                       0\n");
				printf("mesure                     1\n");
				printf("duree mesure (defaut:%d s) 2\n",Time_Mesure);
				scanf("%d",&j);
				if(j==0){Mesure_Light=-1;continue;}
				if(j==1)
				{
					CPhidgetInterfaceKit_getSensorValue(IFK,light_port,
							&light_value);
					printf("light: %d\n",light_value);
					Mesure_Light=1;
					sleep(Time_Mesure);
					Mesure_Light=-1;
				}
				if(j==2)
				{
					printf("duree entiere de mesure?\n");
					scanf("%d",&Time_Mesure);
					goto remesure_light;
				}
			}
		}
		//force
		if(i==8)
		{
			j=-1;
			while(j!=0)
			{
remesure_force:
				printf("exit                       0\n");
				printf("mesure                     1\n");
				printf("duree mesure (defaut:%d s) 2\n",Time_Mesure);
				scanf("%d",&j);
				if(j==0){Mesure_Force=-1;continue;}
				if(j==1)
				{
					CPhidgetInterfaceKit_getSensorValue(IFK,forceL,
							&light_value);
					printf("force left: %f",light_value*5/1000.);
					CPhidgetInterfaceKit_getSensorValue(IFK,forceR,
							&light_value);
					printf("force right: %f\n",light_value*5/1000.);
					Mesure_Force=1;
					sleep(Time_Mesure);
					Mesure_Force=-1;
				}
				if(j==2)
				{
					printf("duree entiere de mesure?\n");
					scanf("%d",&Time_Mesure);
					goto remesure_force;
				}
			}
		}
		//radiocommande
		if(i==9)
		{
			printf("Allumez l'emetteur puis le recepteur\n");
			printf("Marche avant,arriere,gauche,droit par manette gauche\n");
			printf("Ramasse cle par manette droite\n");
			printf("tapez 1 quand pret\n");
			scanf("%d",&j);
			for(k1=1;k1<4;k1++)
			{
				FORWARD_RC[k1]=forward_rc[k1];
				TURN_RC[k1]=turn_rc[k1];
			}
			while(j!=0)
			{
remesure_radio:
				printf("exit                       0\n");
				printf("test commande              1\n");
				printf("duree test (defaut:%d s) 2\n",Time_Mesure_Radio);
				scanf("%d",&j);
				if(j==0){Mesure_Radio=-1;continue;}
				if(j==1)
				{
					Mesure_Radio=1;
					sleep(Time_Mesure);
					Mesure_Radio=-1;
				}
				if(j==2)
				{
					printf("duree entiere de mesure?\n");
					scanf("%d",&Time_Mesure);
					goto remesure_radio;
				}
			}
		}
		//colone lumineuse
		if(i==10)
		{
			j=-1;
			//	  CPhidgetInterfaceKit_getOutputState(IFK2,port_24V_number,&k1);
			//	  printf("output %d:%d\n",port_24V_number,k1);
			//mise en route du 24V
			Init_Colonne_24V();

			GREEN=colonne_lumineuse[2];
			YELLOW=colonne_lumineuse[3];
			RED=colonne_lumineuse[4];
			/*
				 CPhidgetInterfaceKit_getOutputState(IFK2,port_24V_number,&k1);
				 printf("output %d:%d\n",port_24V_number,k1);
				 while(k1==0)
				 {
				 CPhidgetInterfaceKit_setOutputState(IFK2,port_24V_number,0);
				 sleep(1);
				 CPhidgetInterfaceKit_setOutputState(IFK2,port_24V_number,1);
				 sleep(1);
				 CPhidgetInterfaceKit_getOutputState(IFK2,port_24V_number,&k1);
				 printf("output %d:%d\n",port_24V_number,k1);
				 }
				 */
			while(j!=0)
			{
				printf("exit                       0\n");
				printf("allumage vert         : 1\n");
				printf("extinction vert       : 2\n");
				printf("allumage jaune        : 3\n");
				printf("extinction jaune      : 4\n");
				printf("allumage rouge        : 5\n");
				printf("extinction rouge      : 6\n");
				scanf("%d",&j);
				if(j==0)
				{
					CPhidgetInterfaceKit_setOutputState(IFK2,GREEN,0);
					CPhidgetInterfaceKit_setOutputState(IFK2,RED,0);
					CPhidgetInterfaceKit_setOutputState(IFK2,YELLOW,0);
					Stop_Colonne_24V();
					continue;
				}
				if(j==1)CPhidgetInterfaceKit_setOutputState(IFK2,GREEN,1);
				if(j==2)CPhidgetInterfaceKit_setOutputState(IFK2,GREEN,0);
				if(j==3)CPhidgetInterfaceKit_setOutputState(IFK2,YELLOW,1);
				if(j==4)CPhidgetInterfaceKit_setOutputState(IFK2,YELLOW,0);
				if(j==5)CPhidgetInterfaceKit_setOutputState(IFK2,RED,1);
				if(j==6)CPhidgetInterfaceKit_setOutputState(IFK2,RED,0);
			}
		}
		//joystick
		if(i==11)
		{
			j=-1;
			for(k1=0;k1<3;k1++)
			{
				mid_joystick[k1]=(value_joystick[k1][0]+value_joystick[k1][1])/2.;
				width_joystick[k1]=value_joystick[k1][1]-value_joystick[k1][0];
			}
			while(j!=0)
			{
				printf("exit                       0\n");
				printf("mesure joystick            1\n");
				scanf("%d",&j);
				if(j==0){Mesure_Joystick=-1;continue;}
				if(j==1){Mesure_Joystick=1;}
			}
		}
		//batterie
		if(i==12)
		{
			j=-1;
			while(j!=0)
			{
				CPhidgetTextLCD_setContrast (txt_lcd, 110);
				CPhidgetTextLCD_setDisplayString (txt_lcd, 0, "Welcome to ANG");
				printf("Le LCD doit afficher le message 'Welcome to ANG'\n");
				printf("exit                       0\n");
				printf("mesure batterie            1\n");
				scanf("%d",&j);
				if(j==0){Mesure_Batterie=-1;continue;}
				if(j==1)
				{
					CPhidgetInterfaceKit_getSensorValue(IFK_LCD,volt_sensor,
							&light_value);
					VOLT=light_value*VOLT_PER_UNIT;
					CPhidgetInterfaceKit_getSensorValue(IFK_LCD,amp_sensor,
							&light_value);
					CURRENT=(light_value/13.2)-37.8787;
					CONSOM_WATT=VOLT*CURRENT;
					printf("Voltage: %f Current: %f %f Watt\n",VOLT,CURRENT,
							CONSOM_WATT);
					clock_gettime(CLOCK_MONOTONIC, &now);
					temps_batterie=temps1_batterie=now.tv_sec+1.e-9*now.tv_nsec;
					temps2_batterie=temps_batterie;
					Mesure_Batterie=1;
				}
			}
		}
		//accelerometre
		if(i==13)
		{

			j=-1;
			while(j!=0)
			{
remesure_acc:
				for(k1=0;k1<3;k1++)
				{
					amin_acc[k1]=10000.;
					amax_acc[k1]=-10000.;
				}
				printf("exit                       0\n");
				printf("mesure individuelle        1\n");
				printf("calibration                2\n");
				printf("duree mesure (defaut:%d s) 3\n",Time_Mesure);
				scanf("%d",&j);
				if(j==0){Mesure_Accelero=-1;continue;}
				if(j==2)
				{
					printf("laissez le deambulateur au repose pour 10s\n");
					amean[0]=amean[1]=amean[2]=0;
					for(k1=0;k1<10;k1++)
					{
						CPhidgetInterfaceKit_getSensorValue(IFK,accelero_X,
								&light_value);
						amean[0]+=light_value/10.;
						CPhidgetInterfaceKit_getSensorValue(IFK,accelero_Y,
								&light_value);
						amean[1]+=light_value/10.;
						CPhidgetInterfaceKit_getSensorValue(IFK,accelero_Z,
								&light_value);
						amean[2]+=light_value/10.;
						sleep(1);
					}
					printf("Mesure moyenne %f %f %f\n",amean[0],amean[1],amean[2]);
					goto remesure_acc;
				}
				if(j==1)
				{
					CPhidgetInterfaceKit_getSensorValue(IFK,accelero_X,
							&macX);
					CPhidgetInterfaceKit_getSensorValue(IFK,accelero_Y,
							&macY);
					CPhidgetInterfaceKit_getSensorValue(IFK,accelero_Z,
							&macZ);
					Get_Acceleration(&accX,&accY,&accZ,&tiltX,&tiltY,
							&acc_calX,&acc_calY,&acc_calZ,&tilt_calX,
							&tilt_calY,macX,macY,macZ);
					printf("Acceleration non calibree: %f %f %f\n",accX,accY,accZ);
					printf("Tilt x: %f  y: %f\n",tiltX,tiltY);
					if(has_accelero_calibration==1)
					{
						printf("Acceleration calibree: %f %f %f\n",acc_calX,
								acc_calY,acc_calZ);
						printf("Tilt calibre x: %f  y: %f\n",tilt_calX,tilt_calY);
					}
					Mesure_Accelero=1;sleep(Time_Mesure);Mesure_Accelero=-1;
				}
				if(j==3)
				{
					printf("duree entiere de mesure?\n");
					scanf("%d",&Time_Mesure);
					goto remesure_acc;
				}
			}
		}
		//recepteur IR
		if(i==14)
		{
			if(nb_touche==0)
			{
				printf("donnez le nom du fichier touche\n");
				scanf("%s",file_touche);
				j=Read_IR_File(file_touche,CODE_IR,&nb_touche);
				if(j==-1)
				{
					printf("Pas de fichier touche\n");
					continue;
				}
				if(j==-2)
				{
					printf("Erreur lecture fichier touche\n");
					continue;
				}
			}
			j=-1;
			while(j!=0)
			{
				printf("exit                       0\n");
				printf("nouveau fichier touche     1\n");
				printf("attente action             2\n");
				scanf("%d",&j);
				if(j==0){Mesure_IRR=-1;continue;}
				if(j==1)
				{
					printf("donnez le nom du fichier touche\n");
					scanf("%s",file_touche);
					ierr=Read_IR_File(file_touche,CODE_IR,&nb_touche);
					if(ierr==-1)
					{
						printf("Pas de fichier touche\n");
						continue;
					}
					if(ierr==-2)
					{
						printf("Erreur lecture fichier touche\n");
						continue;
					}
				}
				if(j==2)
				{
					Mesure_IRR=1;
				}
			}
		}
	}

exit:
	CPhidget_close((CPhidgetHandle)IFK);
	CPhidget_delete((CPhidgetHandle)IFK);

	return 0;
}
int interfacekit_simple()
{
	int result, numSensors, i;
	const char *err;

	//Declare an InterfaceKit handle
	CPhidgetInterfaceKitHandle ifKit = 0;

	//create the InterfaceKit object
	CPhidgetInterfaceKit_create(&ifKit);

	//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)ifKit, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);

	//Registers a callback that will run if an input changes.
	//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);

	//Registers a callback that will run if the sensor value changes by more than the OnSensorChange trig-ger.
	//Requires the handle for the IntefaceKit, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL);

	//Registers a callback that will run if an output changes.
	//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL);

	//open the interfacekit for device connections
	CPhidget_open((CPhidgetHandle)ifKit, -1);

	//get the program to wait for an interface kit device to be attached
	printf("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
	display_properties(ifKit);

	//read interface kit event data
	printf("Reading.....\n");

	//keep displaying interface kit data until user input is read
	printf("Press any key to go to next step\n");
	getchar();

	printf("Modifying sensor sensitivity triggers....\n");

	//get the number of sensors available
	CPhidgetInterfaceKit_getSensorCount(ifKit, &numSensors);

	//Change the sensitivity trigger of the sensors
	for(i = 0; i < numSensors; i++)
	{
		CPhidgetInterfaceKit_setSensorChangeTrigger(ifKit, i, 100);  //we'll just use 10 for fun
	}

	//read interface kit event data
	printf("Reading.....\n");

	//keep displaying interface kit data until user input is read
	printf("Press any key to go to next step\n");
	getchar();

	printf("Toggling Ratiometric....\n");

	CPhidgetInterfaceKit_setRatiometric(ifKit, 0);

	//read interface kit event data
	printf("Reading.....\n");

	//keep displaying interface kit data until user input is read
	printf("Press any key to end\n");
	getchar();

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)ifKit);
	CPhidget_delete((CPhidgetHandle)ifKit);

	//all done, exit
	return 0;
}
Ejemplo n.º 8
0
int motorcontrol_simple()
{
	int result;
	const char *err;

	//Declare a motor control handle
	CPhidgetMotorControlHandle motoControl = 0;
	//CPhidgetMotorControlHandle motoControl2 = 1;

	//create the motor control object
	CPhidgetMotorControl_create(&motoControl);
	//CPhidgetMotorControl_create(&motoControl2);

	//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)motoControl, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl, ErrorHandler, NULL);
	/*	CPhidget_set_OnAttach_Handler((CPhidgetHandle)motoControl2, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl2, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl2, ErrorHandler, NULL);*/

	//Registers a callback that will run if an input changes.
	//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, InputChangeHandler, NULL);
	//CPhidgetMotorControl_set_OnInputChange_Handler (motoControl2, InputChangeHandler, NULL);

	//Registers a callback that will run if a motor changes.
	//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl, VelocityChangeHandler, NULL);
	//CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl2, VelocityChangeHandler, NULL);

	//Registers a callback that will run if the current draw changes.
	//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl, CurrentChangeHandler, NULL);
	//CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl2, CurrentChangeHandler, NULL);

	//open the motor control for device connections
	CPhidget_open((CPhidgetHandle)motoControl, -1);
	//CPhidget_open((CPhidgetHandle)motoControl2, -1);

	//get the program to wait for a motor control device to be attached
	printf("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;
	}
	/*if((result = CPhidget_waitForAttachment((CPhidgetHandle)motoControl2, 10000)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem waiting for attachment 2: %s\n", err);
		return 0;
		}*/

	//Display the properties of the attached motor control device
	display_properties(motoControl);
	//display_properties(motoControl2);

	//read motor control event data
	printf("Reading.....\n");

	//keep displaying motor control event data until user input is read
	printf("Press any key to continue\n");
	getchar();

	//Control the motor a bit.
	//Step 1: increase acceleration to 50, set target sped at 100
	/*CPhidgetMotorControl_setAcceleration (motoControl, 0, -50.00);
	CPhidgetMotorControl_setVelocity (motoControl, 0, -100.00);
	CPhidgetMotorControl_setAcceleration (motoControl, 1, 50.00);
	CPhidgetMotorControl_setVelocity (motoControl, 1, 100.00);

	printf("Press any key to continue\n");
	getchar();*/

	//Step 2: Set acceleration to 100, decrease target speed to 75
	CPhidgetMotorControl_setAcceleration (motoControl, 0, -100.00);
	CPhidgetMotorControl_setVelocity (motoControl, 0, -100.00);
	CPhidgetMotorControl_setAcceleration (motoControl, 1, 100.00);
	CPhidgetMotorControl_setVelocity (motoControl, 1, 70.00);

	printf("Press any key to continue\n");
	getchar();

	//Step 3: Stop the motor by decreasing speed to 0;
	CPhidgetMotorControl_setVelocity (motoControl, 0, 0.00);
	CPhidgetMotorControl_setAcceleration (motoControl, 0, 0.00);
	CPhidgetMotorControl_setVelocity (motoControl, 1, 0.00);
	CPhidgetMotorControl_setAcceleration (motoControl, 1, 0.00);

	printf("Press any key to end\n");
	getchar();

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)motoControl);
	CPhidget_delete((CPhidgetHandle)motoControl);
	/*CPhidget_close((CPhidgetHandle)motoControl2);
	  CPhidget_delete((CPhidgetHandle)motoControl2);*/

	//all done, exit
	return 0;
}
bool attach(
    CPhidgetMotorControlHandle &phid,
    int serial_number)
{
    // create the object
    CPhidgetMotorControl_create(&phid);

    // 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)phid,
                                  AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)phid,
                                  DetachHandler, NULL);
    CPhidget_set_OnError_Handler((CPhidgetHandle)phid,
                                 ErrorHandler, NULL);

    // Registers a callback that will run if an input changes.
    // Requires the handle for the Phidget, the function
    // that will be called, and a arbitrary pointer that
    // will be supplied to the callback function (may be NULL).
    CPhidgetMotorControl_set_OnInputChange_Handler (phid,
            InputChangeHandler,
            NULL);

    // Registers a callback that will run if a motor changes.
    // Requires the handle for the Phidget, the function
    // that will be called, and a arbitrary pointer that
    // will be supplied to the callback function (may be NULL).
    CPhidgetMotorControl_set_OnVelocityChange_Handler (phid,
            VelocityChangeHandler,
            NULL);

    // Registers a callback that will run if the current
    // draw changes.
    // Requires the handle for the Phidget, the function
    // that will be called, and a arbitrary pointer that
    // will be supplied to the callback function (may be NULL).
    CPhidgetMotorControl_set_OnCurrentChange_Handler (phid,
            CurrentChangeHandler,
            NULL);

    //open the device for connections
    CPhidget_open((CPhidgetHandle)phid, serial_number);

    // get the program to wait for an motor control
    // device to be attached
    if (serial_number == -1) {
        ROS_INFO("Waiting for Motor Control HC Phidget " \
                 "to be attached....");
    }
    else {
        ROS_INFO("Waiting for Motor Control HC Phidget " \
                 "%d to be attached....", serial_number);
    }
    int result;
    if((result =
                CPhidget_waitForAttachment((CPhidgetHandle)phid,
                                           10000)))
    {
        const char *err;
        CPhidget_getErrorDescription(result, &err);
        ROS_ERROR("Problem waiting for motor " \
                  "attachment: %s", err);
        return false;
    }
    else return true;
}
Ejemplo n.º 10
0
int IR_Simple()
{
	CPhidgetIRHandle ir = 0;
	int result, i, dataLength, bitCount;
	const char *err;
	unsigned char data[16];
	
	//Apple volume up
	unsigned char code[4] = {0x77,0xe1,0xd0,0xf0};

	//Apple volume up
    int rawData[67] = {
       9040,   4590,    540,    630,    550,   1740,    550,   1750,    550,   1740,
        550,    620,    550,   1750,    550,   1740,    550,   1750,    550,   1740,
        550,   1740,    560,   1740,    540,    630,    550,    620,    550,    620,
        540,    630,    550,   1750,    550,   1740,    560,   1740,    550,    620,
        550,   1740,    550,    620,    550,    620,    560,    610,    550,    620,
        550,   1750,    550,   1740,    550,    620,    550,   1740,    550,   1750,
        550,    620,    550,    620,    550,    620,    540};

	//Apple uses standard NEC code
	CPhidgetIR_CodeInfo codeInfo = {0}; //this sets eveything to 0 - important if we're NOT going to explicitely fill everything in
	codeInfo.bitCount = 32;
	codeInfo.encoding = PHIDGET_IR_ENCODING_SPACE;
	codeInfo.gap = 110000;
	codeInfo.trail = 560;
	codeInfo.header[0] = 9000, codeInfo.header[1] = 4500;
	codeInfo.one[0] = 560, codeInfo.one[1] = 1700;
	codeInfo.zero[0] = 560, codeInfo.zero[1] = 560;
	codeInfo.repeat[0] = 9000, codeInfo.repeat[1] = 2250, codeInfo.repeat[2] = 560;

	//The rest of these parameters don't need to be filled in, as we're going to use the defaults
	//but this is how they could be filled in:

	//codeInfo.length = PHIDGET_IR_LENGTH_CONSTANT;
	//codeInfo.min_repeat = 1;
	////toggle mask should be bit-length long
	//codeInfo.toggle_mask[0] = 0x00, codeInfo.toggle_mask[1] = 0x00, codeInfo.toggle_mask[2] = 0x00, codeInfo.toggle_mask[3] = 0x00;
	//codeInfo.carrierFrequency = 38000;
	//codeInfo.dutyCycle = 33;


	CPhidgetIR_create(&ir);

	CPhidget_set_OnAttach_Handler((CPhidgetHandle)ir, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)ir, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)ir, ErrorHandler, NULL);

	CPhidgetIR_set_OnCode_Handler(ir, CodeHandler, NULL);
	CPhidgetIR_set_OnLearn_Handler(ir, LearnHandler, NULL);
	CPhidgetIR_set_OnRawData_Handler(ir, RawDataHandler, NULL);

	CPhidget_open((CPhidgetHandle)ir, -1);

	printf("Waiting for PhidgetIR to be attached.... \n");
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)ir, 10000)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem waiting for attachment: %s\n", err);
		return 0;
	}

	//Display the properties of the attached accelerometer device
	display_properties((CPhidgetHandle)ir);

	printf("Reading.....\n");

	printf("Press any key to Transmit a code...\n");
	getchar();

	if(result = CPhidgetIR_Transmit(ir, code, &codeInfo))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Error: %s\n", err);
	}

	printf("Press any key to Transmit some raw data...\n");
	getchar();

	CPhidgetIR_TransmitRaw(ir, rawData, 67, 38000, 33, 110000);

	printf("Press any key to get the last code read...\n");
	getchar();


	dataLength = 16;
	
	if((result = CPhidgetIR_getLastCode(ir, data, &dataLength, &bitCount)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Error: %s\n", err);
	}
	else
	{
		printf("Last Code: ");
		for(i = 0; i < dataLength; i++)
		{
			printf("%02x", data[i]); 
		}
		printf("\n");
	}

	printf("Press any key to end\n");
	getchar();

	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)ir);
	CPhidget_delete((CPhidgetHandle)ir);

	return 0;
}
Ejemplo n.º 11
0
int interfacekit_simple()
// initialize the phidget interface kit board and phidget spatial (imu)
{
	int result, num_analog_inputs, num_digital_inputs;
	const char *err;
	ros::NodeHandle n;

	//create the InterfaceKit object
	CPhidgetInterfaceKit_create(&ifKit);

	//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_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
	CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, DigitalInputHandler, NULL);
	CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, AnalogInputHandler, NULL);


	//open the interfacekit and spatial for device connections
	CPhidget_open((CPhidgetHandle)ifKit, -1);
	

	CPhidgetInterfaceKit_getInputCount(ifKit, &num_digital_inputs);
	CPhidgetInterfaceKit_getSensorCount(ifKit, &num_analog_inputs);


	printf("Waiting for interface kit to be attached....");
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 1000)))
	{
		CPhidget_getErrorDescription(result, &err);
		ROS_ERROR("Phidget IK: Problem waiting for attachment: %s\n", err);
		interfaceKitError = 1;
	}
	else
	{
        	irData_pub = n.advertise<corobot_msgs::SensorMsg>("infrared_data", 100);
        	powerdata_pub = n.advertise<corobot_msgs::PowerMsg>("power_data", 100);
        	bumper_pub = n.advertise<corobot_msgs::SensorMsg>("bumper_data", 100);
		// sensors connected to the phidget interface kit other than bumpers, voltage sensor, ir sensor and sonars. 
        	other_pub = n.advertise<corobot_msgs::SensorMsg>("sensor_data", 100); 
	}	
	

	//Initialize the phidget spatial board, if any
	if (imu)
	{
		CPhidgetSpatial_create(&spatial);
		CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL);
		CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL);
		CPhidget_open((CPhidgetHandle)spatial, -1);

		// attach the devices
		printf("Waiting for spatial to be attached.... \n");
		if((result = CPhidget_waitForAttachment((CPhidgetHandle)spatial, 1000)))
		{
			CPhidget_getErrorDescription(result, &err);
			ROS_ERROR("Phidget Spatial: Problem waiting for attachment: %s\n", err);
			spatialError = 1;
		}
		else
		{
			imu_pub = n.advertise<sensor_msgs::Imu>("imu_data",100);
			mag_pub = n.advertise<sensor_msgs::MagneticField>("magnetic_data",100);
      calibrate_gyroscope_service = n.advertiseService("calibrate_gyroscope",calibrate_gyroscope);
		}

		CPhidgetSpatial_setDataRate(spatial, 4);
	}

	CPhidgetInterfaceKit_setRatiometric(ifKit, 0);

	//Initialize the sonars, if any are present
	if(sonarsPresent)
	{
		CPhidgetInterfaceKit_setOutputState(ifKit, bwOutput, 1);
		CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
		// sleep for 250ms
		ros::Duration(0.250).sleep(); 
		CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 1);
		// sleep for 2ms
		ros::Duration(0.002).sleep(); 
		CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
		// sleep for 150ms
		ros::Duration(0.150).sleep(); 

		sonar_pub = n.advertise<corobot_msgs::SensorMsg>("sonar_data", 100);
	}
	return 0;
}
Ejemplo n.º 12
0
bool attach(
			CPhidgetAccelerometerHandle &phid,
			int serial_number,
			double sensitivity)
{
    // create the object
    CPhidgetAccelerometer_create(&phid);

    // 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)phid,
								  AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)phid,
								  DetachHandler, NULL);
    CPhidget_set_OnError_Handler((CPhidgetHandle)phid,
								 ErrorHandler, NULL);

    // Registers a callback that will run if the
	// acceleration changes by more than the Acceleration
	// trigger.
    // Requires the handle for the Accelerometer, the
	// function that will be called, 
    // and an arbitrary pointer that will be supplied to
	// the callback function (may be NULL)
    CPhidgetAccelerometer_set_OnAccelerationChange_Handler(phid,
														   accel_AccelChangeHandler,
														   NULL);

    //open the device for connections
    CPhidget_open((CPhidgetHandle)phid, serial_number);

    // get the program to wait for an accelerometer
	// device to be attached
    if (serial_number == -1) {
        ROS_INFO("Waiting for Accelerometer Phidget to " \
				 "be attached....");
    }
    else {
        ROS_INFO("Waiting for Accelerometer Phidget %d " \
				 "to be attached....", serial_number);
    }
    int result;
    if((result =
		CPhidget_waitForAttachment((CPhidgetHandle)phid,
								   10000)))	{
			const char *err;
			CPhidget_getErrorDescription(result, &err);
			ROS_ERROR("Problem waiting for attachment: %s",
					  err);
			return false;
		}
    else {
		// get the number of available axes on the
		// attached accelerometer
        int numAxes=2;
		CPhidgetAccelerometer_getAxisCount(phid, &numAxes);

        // set the sensitivity
		for (int i = 0; i < numAxes; i++) {
			CPhidgetAccelerometer_setAccelerationChangeTrigger(phid,
															   i,
															   sensitivity);
        }
        return true;
    }
}
Ejemplo n.º 13
0
    Controller::Controller()
    : motoControl(0)
    , speed(40)
    , speedLeftFactor(1.5)
    , speedRightFactor(1.5)
    , accelLeftFactor(1.5)
    , accelRightFactor(1.5)
    , rotationOnSpotSpeed(100)
    , accel(15)
    , backwardTurnFastFactor(1.5)
    , backwardTurnSlowFactor(-1.5)
    , servo(0)
    , servoOpen(35.00)
      , servoClosed(150.00)
      /*, speed(100)
        , speedLeftFactor(1.0)
        , speedRightFactor(1.0)
        , accelLeftFactor(1.0)
        , accelRightFactor(1.0)
        , rotationOnSpotSpeed(40)
        , accel(100)
        , backwardTurnFastFactor(1)
        , backwardTurnSlowFactor(-1)*/
{
    //create the motor control object
    CPhidgetMotorControl_create(&motoControl);

    //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)motoControl, AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)motoControl, DetachHandler, NULL);
    CPhidget_set_OnError_Handler((CPhidgetHandle)motoControl, ErrorHandler, NULL);



    ifKit = 0;
        
        //create the InterfaceKit object
        CPhidgetInterfaceKit_create(&ifKit);
        
        //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)ifKit, AttachHandler, NULL);
        CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
        CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);

    CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);

    //open the interfacekit for device connections
    CPhidget_open((CPhidgetHandle)ifKit, -1);

    CPhidgetAdvancedServo_create(&servo);
    CPhidget_open((CPhidgetHandle)servo, -1);

    int result;
    const char *err;
    if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000)))
    {
        CPhidget_getErrorDescription(result, &err);
    }

    //Registers a callback that will run if an input changes.
    //Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
    CPhidgetMotorControl_set_OnInputChange_Handler (motoControl, InputChangeHandler, NULL);

    //Registers a callback that will run if a motor changes.
    //Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
    CPhidgetMotorControl_set_OnVelocityChange_Handler (motoControl, VelocityChangeHandler, NULL);

    //Registers a callback that will run if the current draw changes.
    //Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
    CPhidgetMotorControl_set_OnCurrentChange_Handler (motoControl, CurrentChangeHandler, NULL);

    //open the motor control for device connections
    CPhidget_open((CPhidgetHandle)motoControl, -1);

    //get the program to wait for a motor control device to be attached
    CPhidget_waitForAttachment((CPhidgetHandle)motoControl, 10000);

    CPhidgetAdvancedServo_setEngaged(servo, 0, 1);
    CPhidgetAdvancedServo_setPosition (servo, 0, 40.00);
}
Ejemplo n.º 14
0
int interfacekit_simple()
{
	int result, num_analog_inputs, num_digital_inputs;
	const char *err;

	//create the InterfaceKit object
	CPhidgetInterfaceKit_create(&ifKit);

	//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_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);
           
	CPhidgetInterfaceKit_set_OnInputChange_Handler (ifKit, InputChangeHandler, NULL);

	CPhidgetInterfaceKit_set_OnSensorChange_Handler (ifKit, SensorChangeHandler, NULL);

	CPhidgetInterfaceKit_set_OnOutputChange_Handler (ifKit, OutputChangeHandler, NULL);

	//For phidget spatial
	//CPhidgetSpatialHandle spatial = 0;
	CPhidgetSpatial_create(&spatial);
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)spatial, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)spatial, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL);
	CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL);


	//open the interfacekit for device connections
	CPhidget_open((CPhidgetHandle)ifKit, -1);


	CPhidget_open((CPhidgetHandle)spatial, -1);


	CPhidgetInterfaceKit_getInputCount(ifKit, &num_digital_inputs);
	CPhidgetInterfaceKit_getSensorCount(ifKit, &num_analog_inputs);


	printf("Waiting for spatial to be attached.... \n");
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)spatial, 1000)))
	{
		CPhidget_getErrorDescription(result, &err);
		ROS_ERROR("Phidget Spatial: Problem waiting for attachment: %s\n", err);
		spatialError = 1;
	}

	printf("Waiting for interface kit to be attached....");
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 1000)))
	{
		CPhidget_getErrorDescription(result, &err);
		ROS_ERROR("Phidget IK: Problem waiting for attachment: %s\n", err);
		phidget888_connected = false;
		interfaceKitError = 1;
	}

	phidget888_connected = true;
	
	CPhidgetInterfaceKit_setRatiometric(ifKit, 0);//

	CPhidgetSpatial_setDataRate(spatial, 16);	

	 CPhidgetEncoder_create(&m_leftEncoder);
         CPhidget_set_OnAttach_Handler((CPhidgetHandle) m_leftEncoder,LeftEncoderAttach, NULL);
         CPhidget_open((CPhidgetHandle) m_leftEncoder, -1);
        
	if (m_encoder1Seen && m_encoder2Seen)
	    phidget_encoder_connected = true;
	else phidget_encoder_connected = false;

	//Initialize the sonars, if any are present
	if(sonarsPresent)
	{
		CPhidgetInterfaceKit_setOutputState(ifKit, bwOutput, 1);
		CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
		ros::Duration(0.250).sleep(); // sleep for 250ms
		CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 1);
		ros::Duration(0.002).sleep(); // sleep for 2ms
		CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
		ros::Duration(0.150).sleep(); // sleep for 150ms
	}
	return 0;
}
Ejemplo n.º 15
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;
}
Ejemplo n.º 16
0
int test_interfacekit()
{
	int numInputs, numOutputs, numSensors;
	int err;
	CPhidgetInterfaceKitHandle IFK = 0;

	CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, NULL);
	
	CPhidgetInterfaceKit_create(&IFK);

	CPhidgetInterfaceKit_set_OnInputChange_Handler(IFK, IFK_InputChangeHandler, NULL);
	CPhidgetInterfaceKit_set_OnOutputChange_Handler(IFK, IFK_OutputChangeHandler, NULL);
	CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK, IFK_SensorChangeHandler, NULL);
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)IFK, IFK_AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)IFK, IFK_DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)IFK, IFK_ErrorHandler, NULL);
	
	CPhidget_open((CPhidgetHandle)IFK, -1);

	//wait 5 seconds for attachment
	if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 0)) != EPHIDGET_OK )
	{
		const char *errStr;
		CPhidget_getErrorDescription(err, &errStr);
		printf("Error waiting for attachment: (%d): %s\n",err,errStr);
		goto exit;
	}
	
	display_generic_properties((CPhidgetHandle)IFK);
	CPhidgetInterfaceKit_getOutputCount((CPhidgetInterfaceKitHandle)IFK, &numOutputs);
	CPhidgetInterfaceKit_getInputCount((CPhidgetInterfaceKitHandle)IFK, &numInputs);
	CPhidgetInterfaceKit_getSensorCount((CPhidgetInterfaceKitHandle)IFK, &numSensors);
	
	CPhidgetInterfaceKit_setOutputState((CPhidgetInterfaceKitHandle)IFK, 0, 1);

	printf("Sensors:%d Inputs:%d Outputs:%d\n", numSensors, numInputs, numOutputs);
	
	//err = CPhidget_setDeviceLabel((CPhidgetHandle)IFK, "test");
	
	while(1)
	{
		sleep(1);
	}
	
	while(1)
	{
		CPhidgetInterfaceKit_setOutputState(IFK, 7, 1);
		CPhidgetInterfaceKit_setOutputState(IFK, 7, 0);
	}

	CPhidgetInterfaceKit_setOutputState(IFK, 0, 1);
	sleep(1);
	CPhidgetInterfaceKit_setOutputState(IFK, 0, 0);
	sleep(1);
	CPhidgetInterfaceKit_setOutputState(IFK, 0, 1);
	sleep(1);
	CPhidgetInterfaceKit_setOutputState(IFK, 0, 0);

	sleep(5);

exit:
	CPhidget_close((CPhidgetHandle)IFK);
	CPhidget_delete((CPhidgetHandle)IFK);

	return 0;
}
Ejemplo n.º 17
0
int  Phidget::waitForAttachment(int timeout)
{
  return CPhidget_waitForAttachment(handle_, timeout);
}
Ejemplo n.º 18
0
int rfid_simple()
{
	int result;
	const char *err;

	//Declare an RFID handle
	CPhidgetRFIDHandle rfid = 0;

	printf("RFIDHandler\n");

	//create the RFID object
	CPhidgetRFID_create(&rfid);

	printf("Create\n");

	//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)rfid, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)rfid, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)rfid, ErrorHandler, NULL);

	printf("device EVENT Handlers\n");

	//Registers a callback that will run if an output changes.
	//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetRFID_set_OnOutputChange_Handler(rfid, OutputChangeHandler, NULL);

	//Registers a callback that will run when a Tag is read.
	//Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetRFID_set_OnTag_Handler(rfid, TagHandler, NULL);

	//Registers a callback that will run when a Tag is lost (removed from antenna read range).
	//Requires the handle for the PhidgetRFID, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetRFID_set_OnTagLost_Handler(rfid, TagLostHandler, NULL);

	printf("RFID EVENT Handlers\n");

	//open the RFID for device connections
	CPhidget_open((CPhidgetHandle)rfid, -1);

	printf("OPENED\n");

	sleep(1);

	//get the program to wait for an RFID device to be attached
	printf("Waiting for RFID to be attached....\n");


	if((result = CPhidget_waitForAttachment((CPhidgetHandle)rfid, 10000)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem waiting for attachment: %s\n", err);
		return 0;
	}

	//Display the properties of the attached RFID device
	display_properties(rfid);

	CPhidgetRFID_setAntennaOn(rfid, 1);

	//read RFID event data
	printf("Reading.....\n");

	//keep displaying RFID event data until user input is read
	printf("Press any key to continue\n");
	getchar();

	//toggle the digital output (when making this example I had an LED plugged into the digital output index 0
	CPhidgetRFID_setOutputState(rfid, 0, 1);

	//keep displaying RFID event data until user input is read
	printf("Press any key to continue\n");
	getchar();

	//toggle the digital output (when making this example I had an LED plugged into the digital output index 0
	CPhidgetRFID_setOutputState(rfid, 0, 0);

	printf("Press any key to end\n");
	getchar();

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)rfid);
	CPhidget_delete((CPhidgetHandle)rfid);

	//all done, exit
	return 0;
}
int accelerometer_simple()
{
	int result, numAxes;
	const char *err;

	//Declare an accelerometer handle
	CPhidgetAccelerometerHandle accel = 0;

	//create the accelerometer object
	CPhidgetAccelerometer_create(&accel);

	//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)accel, AttachHandler, NULL);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)accel, DetachHandler, NULL);
	CPhidget_set_OnError_Handler((CPhidgetHandle)accel, ErrorHandler, NULL);

	//Registers a callback that will run if the acceleration changes by more than the Acceleration trigger.
	//Requires the handle for the Accelerometer, the function that will be called, 
	//and an arbitrary pointer that will be supplied to the callback function (may be NULL)
	CPhidgetAccelerometer_set_OnAccelerationChange_Handler(accel, accel_AccelChangeHandler, NULL);

	//open the acclerometer for device connections
	CPhidget_open((CPhidgetHandle)accel, -1);

	//get the program to wait for an accelerometer device to be attached
	printf("Waiting for accelerometer to be attached.... \n");
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)accel, 10000)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem waiting for attachment: %s\n", err);
		return 0;
	}

	//Display the properties of the attached accelerometer device
	display_properties((CPhidgetHandle)accel);

	//read accelerometer event data
	printf("Reading.....\n");

	//get the number of available axes on the attached accelerometer
	CPhidgetAccelerometer_getAxisCount(accel, &numAxes);

	//Most accelerometers have 2 axes so we'll pre-set their sensitivity to make the data more readable
	CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 0, 0.500);
	CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 1, 0.500);

	//If the accelerometer attached is a 3-axis, we'll set the sensitivity of the 3rd axis
	if(numAxes > 2)
	{
		CPhidgetAccelerometer_setAccelerationChangeTrigger(accel, 2, 0.500);
	}

	//wait until user input is read
	printf("Press any key to end\n");
	getchar();

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)accel);
	CPhidget_delete((CPhidgetHandle)accel);

	//all done, exit
	return 0;
}
Ejemplo n.º 20
0
int servo_simple()
{
	int result;
	double curr_pos;
	const char *err;

	//Declare an servo handle
	CPhidgetServoHandle servo = 0;

	//create the servo object
	CPhidgetServo_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.
	//Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
	CPhidgetServo_set_OnPositionChange_Handler(servo, PositionChangeHandler, NULL);

	//open the servo for device connections
	CPhidget_open((CPhidgetHandle)servo, -1);

	//get the program to wait for an servo device to be attached
	printf("Waiting for Servo controller 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 servo device
	display_properties(servo);

	//read servo event data
	printf("Reading.....\n");

	//This example assumes servo motor is attached to index 0

	//display current motor position
	CPhidgetServo_getPosition(servo, 0, &curr_pos);
	printf("Motor: 0 > Current Position: %f\n", curr_pos);

	//keep displaying servo event data until user input is read
	printf("Press any key to continue\n");
	getchar();

	//change the motor position
	//valid range is -22 to 232
	//we'll set it to a few random positions to move it around

	//Step 1: Position 10.00
	printf("Move to position 10.00. Press any key to Continue\n");
	getchar();

	CPhidgetServo_setPosition (servo, 0, 10.00);

	//Step 2: Position 50.00
	printf("Move to position 50.00. Press any key to Continue\n");
	getchar();

	CPhidgetServo_setPosition (servo, 0, 50.00);

	//Step 3: Position 100.00
	printf("Move to position 100.00. Press any key to Continue\n");
	getchar();

	CPhidgetServo_setPosition (servo, 0, 100.00);

	//Step 4: Position 150.00
	printf("Move to position 150.00. Press any key to Continue\n");
	getchar();

	CPhidgetServo_setPosition (servo, 0, 150.00);

	//Step 5: Position 200.00
	printf("Move to position 200.00. Press any key to Continue\n");
	getchar();

	CPhidgetServo_setPosition (servo, 0, 200.00);

	//Step 6: Position 20.00
	printf("Move to position 20.00. Press any key to Continue\n");
	getchar();

	CPhidgetServo_setPosition (servo, 0, 20.00);

	//Step 7: Diseangage
	printf("Disengage. Press any key to Continue\n");
	getchar();

	CPhidgetServo_setEngaged (servo, 0, 0);

	printf("Press any key to end\n");
	getchar();

	//since user input has been read, this is a signal to terminate the program so we will close the phidget and delete the object we created
	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)servo);
	CPhidget_delete((CPhidgetHandle)servo);

	//all done, exit
	return 0;
}
Ejemplo n.º 21
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;
}
int main(int argc, char** argv){
	ros::init(argc, argv, "pan");
	ros::NodeHandle n;
	ROS_INFO("Is the correct motor node running?");
	//connect to Hypervisor and com nodes
	ros::Subscriber hv_sub = n.subscribe("Hypervisor_Output", 10, hvCallback);	
	ros::Subscriber com_sub = n.subscribe("Com_Commands", 10, comCallback);
	
	//Open connection for Log node to connect to
	ros::Publisher pub = n.advertise<std_msgs::String>("Motor_Movement",10);
	
	//Connect to CV service
	ros::ServiceClient client = n.serviceClient<ICT_Viper::CvService>("cv_service");
	ICT_Viper::CvService srv;
	
	//Connect to motor

	int x_offset = 0;
	int result;
	const char *err;

	//Declare a motor control handle
	CPhidgetMotorControlHandle motoControl = 0;
	
	//create the motor control object
	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);
	CPhidget_open((CPhidgetHandle)motoControl, -1);

	ROS_INFO("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 -1;
	}
	ROS_INFO("Motor connection established!");

	while (ros::ok())
	{
	//	ROS_INFO("making service request...\n");
		srv.request.A = 0;
		if (client.call(srv))
		{
			ROS_INFO("x offset = %d\n", (int) srv.response.Coords);
			x_offset = srv.response.Coords;
			if (x_offset >= 320)
			{
				CPhidgetMotorControl_setAcceleration(motoControl, 0, 50.0);
				CPhidgetMotorControl_setVelocity(motoControl, 0, 100.0);
			}
			else
			{
				CPhidgetMotorControl_setAcceleration(motoControl, 0, -50.0);
				CPhidgetMotorControl_setVelocity(motoControl, 0, -100.0);
			}
		}
		else
		{
			ROS_ERROR("cv service call failed");
		}
	}

	//time to exit
	CPhidgetMotorControl_setAcceleration (motoControl, 0,0);
	CPhidgetMotorControl_setVelocity (motoControl, 0, 0);

	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)motoControl);
	
	//ros::spin();	

}