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