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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
}
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);
}
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;
}
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;
}
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;
}
int Phidget::waitForAttachment(int timeout)
{
return CPhidget_waitForAttachment(handle_, timeout);
}
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;
}
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;
}
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();
}
