Exemple #1
0
void motor::update(DefaultGUIModel::update_flags_t flag) {
    switch (flag) {
    case INIT:
        velocity = 0;
        setParameter("velocity", QString::number(velocity)); //initially 0
        CPhidgetMotorControl_setVelocity(motoControl, 0, velocity); // should let you change the target velocity
        break;

    case MODIFY:
        velocity = getParameter("velocity").toDouble();
        CPhidgetMotorControl_setVelocity(motoControl, 0, velocity); // should let you change the target velocity
        break;

    case UNPAUSE:
        break;

    case PAUSE:
        break;

    case PERIOD:
        period = RT::System::getInstance()->getPeriod() * 1e-6; // ms
        break;

    default:
        break;
    }
}
Exemple #2
0
void Controller::stop()
{
    CPhidgetMotorControl_setAcceleration (motoControl, 0, 0);
    CPhidgetMotorControl_setVelocity (motoControl, 0, 0);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, 0);
    CPhidgetMotorControl_setVelocity (motoControl, 1, 0);
}
Exemple #3
0
void Controller::turnLeft()
{
    CPhidgetMotorControl_setAcceleration (motoControl, 0, 0);
    CPhidgetMotorControl_setVelocity (motoControl, 0, 0);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, accel * accelRightFactor);
    CPhidgetMotorControl_setVelocity (motoControl, 1, speed * speedRightFactor);
}
Exemple #4
0
void Controller::moveBackwardRight()
{
    CPhidgetMotorControl_setAcceleration (motoControl, 0, accel*backwardTurnFastFactor);
    CPhidgetMotorControl_setVelocity (motoControl, 0, speed*backwardTurnFastFactor);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, -accel * backwardTurnSlowFactor);
    CPhidgetMotorControl_setVelocity (motoControl, 1, -speed * backwardTurnSlowFactor);
}
Exemple #5
0
void Controller::moveBackward()
{
    CPhidgetMotorControl_setAcceleration (motoControl, 0, accel * accelLeftFactor);
    CPhidgetMotorControl_setVelocity (motoControl, 0, speed * speedLeftFactor);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, -accel * accelRightFactor);
    CPhidgetMotorControl_setVelocity (motoControl, 1, -speed * speedRightFactor);
}
Exemple #6
0
void Controller::rotateOnSpotRight()
{
    CPhidgetMotorControl_setAcceleration (motoControl, 0, rotationOnSpotSpeed);
    CPhidgetMotorControl_setVelocity (motoControl, 0, rotationOnSpotSpeed);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, rotationOnSpotSpeed);
    CPhidgetMotorControl_setVelocity (motoControl, 1, rotationOnSpotSpeed);
}
Exemple #7
0
void Controller::rotateOnSpot()
{
    int rotationSpeed = 100;
    CPhidgetMotorControl_setAcceleration (motoControl, 0, -rotationSpeed);
    CPhidgetMotorControl_setVelocity (motoControl, 0, -rotationSpeed);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, -rotationSpeed);
    CPhidgetMotorControl_setVelocity (motoControl, 1, -rotationSpeed);
}
Exemple #8
0
void Controller::turnRight()
{
    CPhidgetMotorControl_setAcceleration (motoControl, 0, -accel * accelLeftFactor);
    CPhidgetMotorControl_setVelocity (motoControl, 0, -speed * speedLeftFactor);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, 0);
    CPhidgetMotorControl_setVelocity (motoControl, 1, 0);
    /*usleep(500000);
      rotateOnSpotRight();
      usleep(50000);*/
}
Exemple #9
0
void All_Stop()
	/*--------------------
		mise a 0 des vitesses moteurs et debrayage
		Sert pour la sortie de manip
		---------------------*/
{
	CPhidgetMotorControl_setVelocity (motorControl, 0,0);
	CPhidgetMotorControl_setVelocity (motorControl, 1,0);
	Gere_Embrayage(DEBRAYE);
	Buzzer(0);
}
Exemple #10
0
int Gere_Embrayage_Motor(int percent_embrayage1,int percent_embrayage2)
/*------------------------
permet d'embrayer + ou - les embrayages moteur
percent_embrayage:

0: completement debraye
100: completement embraye

embrayage 1: left, 2 : right
------------------------*/
{
  //gauche
  CPhidgetMotorControl_setVelocity (EmbrayageControl, 0,percent_embrayage1);
  embrayage_status_left=percent_embrayage1;
  //droit
  CPhidgetMotorControl_setVelocity (EmbrayageControl,1,percent_embrayage2);
  embrayage_status_right=percent_embrayage2;
  return 0;
}
Exemple #11
0
void Controller::turnAt(double angle)
{
    int turnSpeed = 40;
    int turnDelay = 150000;
    if (-22 < angle && angle < 22)
    {
        moveForward();
        usleep(200000);
        return;
    }
    if (angle > 0) angle = std::min(angle, 45.0);
    else if (angle < 0) angle = std::max(angle, -45.0);
    int sign = (int) (angle / std::abs(angle));
    CPhidgetMotorControl_setAcceleration (motoControl, 0, sign * turnSpeed);
    CPhidgetMotorControl_setVelocity (motoControl, 0, sign * turnSpeed);

    CPhidgetMotorControl_setAcceleration (motoControl, 1, sign * turnSpeed);
    CPhidgetMotorControl_setVelocity (motoControl, 1, sign * turnSpeed);
    usleep((std::abs(angle) * turnDelay) / 45);
    stop();
}
Exemple #12
0
//--------------MOTEUR----------------
void Set_Speed_Motor(int motor,int value)
/*---------------
Met la vitesse moteur de motor (0 ou 1) en % entre -100 et 100
---------------*/
{
  int left1;

  left1=value;
  if(left1>100)left1=100;
  if(left1<-100)left1=-100;
  CPhidgetMotorControl_setVelocity (motorControl,motor,left1);
}
void motor_callback(const std_msgs::String::ConstPtr& msg){
	ROS_INFO("Motor Move Request:");
	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);

	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;
	}

	CPhidgetMotorControl_setAcceleration (motoControl, 0, 50.00);
	CPhidgetMotorControl_setVelocity (motoControl, 0, 100.00);
	
	//Wait for some time... 
	ros::Duration(1).sleep();
	
	CPhidgetMotorControl_setAcceleration (motoControl, 0,0);
	CPhidgetMotorControl_setVelocity (motoControl, 0, 0);

	printf("Closing...\n");
	CPhidget_close((CPhidgetHandle)motoControl);
	CPhidget_delete((CPhidgetHandle)motoControl);
}
Exemple #14
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;
}
Exemple #15
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;
}
/*!
 * \brief callback when a velocity command is received
 * \param ptr encoder parameters
 */
void velocityCommandCallback(const geometry_msgs::Twist::ConstPtr& ptr)
{

    ROS_WARN("I GOT ONE!");
    if (initialised) {
        geometry_msgs::Twist m = *ptr;
        // convert Twist to motor velocities
        float rotate,forward;
        double incr,dInput;

        float x = m.linear.x;
        float y = m.angular.z;

        if ((m.linear.x!=0) || (m.angular.z!=0)) {
            //ROS_DEBUG("cmd_vel %.3f angular %.3f",
            //          m.linear.x,m.angular.z);
            //ROS_DEBUG("actual  %.3f/%.3f angular %.3f",
            //          odom.twist.twist.linear.x,
            //          odom.twist.twist.linear.y,
            //          odom.twist.twist.angular.z);
        }
        else {
            //ROS_DEBUG("Zero speed");
        }

        if (x_forward) {
            rotate = y;
            forward = x;
        }
        else {
            rotate = x;
            forward = y;
        }
        float forward_speed = speed * forward;

        ros::Time current_time = ros::Time::now();
        if (odometry_active) {
            if (motors_active) {
                double time_elapsed =
                    (current_time - odom.header.stamp).toSec();
                if (time_elapsed > 0.001) {
                    double angular_velocity_error =
                        rotate - odom.twist.twist.angular.z;

                    // how fast are we actually moving?
                    double vx = odom.twist.twist.linear.x;
                    double vy = odom.twist.twist.linear.y;
                    double v = sqrt(vx*vx + vy*vy);
                    if (forward<0) v=-v;

                    // forward velocity error
                    double forward_velocity_error = forward - v;

                    double angular_accel =
                        angular_velocity_error / time_elapsed;
                    if (fabs(angular_accel) >
                            max_angular_accel) {
                        ROS_DEBUG("Angular acceleration " \
                                  "limit reached");
                        if (angular_accel > 0) {
                            angular_accel = max_angular_accel;
                        }
                        else {
                            angular_accel = -max_angular_accel;
                        }
                    }

                    if (fabs(angular_velocity_error) >
                            max_angular_error) {
                        ROS_WARN("Angular velocity " \
                                 "error %f degrees/sec = %f - %f",
                                 angular_velocity_error*180/
                                 3.1415927,
                                 rotate,
                                 odom.twist.twist.angular.z);
                    }
                    if ((rotate>0.1) &&
                            (odom.twist.twist.angular.z<-0.1)) {
                        ROS_WARN("Rotation command " \
                                 "positive and odometry " \
                                 "direction negative");
                    }
                    if ((rotate<-0.1) &&
                            (odom.twist.twist.angular.z>0.1)) {
                        ROS_WARN("Rotation command " \
                                 "negative and odometry " \
                                 "direction positive");
                    }
                    if (fabs(forward_velocity_error) >
                            max_velocity_error) {
                    }
                    double linear_accel =
                        forward_velocity_error / time_elapsed;
                    if (fabs(linear_accel) > max_linear_accel) {
                        ROS_WARN("Linear acceleration " \
                                 "limit %f/%f reached",
                                 linear_accel,max_linear_accel);
                        if (linear_accel > 0) {
                            linear_accel = max_linear_accel;
                        }
                        else {
                            linear_accel = -max_linear_accel;
                        }
                    }

                    // linear integral and derivative
                    ITerm[0] +=
                        linear_velocity_integral * linear_accel;
                    if (ITerm[0] > max_linear_accel) {
                        ITerm[0] = max_linear_accel;
                    }
                    else {
                        if (ITerm[0] <
                                -max_linear_accel) {
                            ITerm[0] = -max_linear_accel;
                        }
                    }

                    if (fabs(forward)>linear_deadband) {
                        incr =
                            (linear_velocity_proportional *
                             linear_accel) +
                            ITerm[0] -
                            (linear_velocity_derivative *
                             (v - last_v));
                        if (invert_forward) {
                            incr = -incr;
                        }
                        current_linear_velocity += incr;
                    }
                    else {
                        current_linear_velocity *= 0.8;
                    }
                    if (current_linear_velocity > speed) {
                        current_linear_velocity = speed;
                    }
                    if (current_linear_velocity <-speed) {
                        current_linear_velocity = -speed;
                    }

                    // angular integral and derivative
                    ITerm[1] +=
                        angular_velocity_integral *
                        angular_accel;
                    if (ITerm[1] > max_angular_accel) {
                        ITerm[1] = max_angular_accel;
                    }
                    else {
                        if (ITerm[1] < -max_angular_accel) {
                            ITerm[1] = -max_angular_accel;
                        }
                    }

                    if (fabs(rotate)>angular_deadband) {
                        incr =
                            (angular_velocity_proportional *
                             angular_accel) +
                            ITerm[1] -
                            (angular_velocity_derivative *
                             (odom.twist.twist.angular.z -
                              last_angv));
                        if (invert_rotation) {
                            incr = -incr;
                        }
                        current_angular_velocity += incr;
                    }
                    else {
                        current_angular_velocity *= 0.8;
                    }
                    if (current_angular_velocity >
                            max_angular_velocity) {
                        current_angular_velocity =
                            max_angular_velocity;
                    }
                    if (current_angular_velocity <
                            -max_angular_velocity) {
                        current_angular_velocity =
                            -max_angular_velocity;
                    }

                    CPhidgetMotorControl_setVelocity (phid,
                                                      1,
                                                      current_linear_velocity +
                                                      current_angular_velocity);
                    CPhidgetMotorControl_setVelocity (phid, 0,
                                                      -current_linear_velocity +
                                                      current_angular_velocity);
                    CPhidgetMotorControl_setAcceleration (phid,
                                                          0,
                                                          acceleration);
                    CPhidgetMotorControl_setAcceleration (phid,
                                                          1,
                                                          acceleration);

                    last_v = v;
                    last_angv = odom.twist.twist.angular.z;
                }
            }
        }
        else {
            CPhidgetMotorControl_setVelocity (phid, 0,
                                              -forward_speed -
                                              (rotate*speed));
            CPhidgetMotorControl_setVelocity (phid, 1,
                                              forward_speed -
                                              (rotate*speed));
            CPhidgetMotorControl_setAcceleration (phid, 0,
                                                  acceleration);
            CPhidgetMotorControl_setAcceleration (phid, 1,
                                                  acceleration);
        }
        last_velocity_command = current_time;
        motors_active = true;
    }
}
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();	

}
void stop_motors()
{
    CPhidgetMotorControl_setVelocity (phid, 0, 0);
    CPhidgetMotorControl_setVelocity (phid, 1, 0);
    motors_active = false;
}