int Create_KIT1()
/*-----------------
creation du handler pour le kit interface 1
------------------*/
{
int err;
const char *errStr;
CPhidgetInterfaceKit_create(&IFK);
CPhidgetInterfaceKit_set_OnInputChange_Handler(IFK, IFK_InputChangeHandler,
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);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(IFK,IFK_SensorChangeHandler,
NULL);
//ouverture du kit interface
CPhidget_open((CPhidgetHandle)IFK,kit_number);
//wait 5 seconds for attachment
if((err = CPhidget_waitForAttachment((CPhidgetHandle)IFK, 5000))
!= EPHIDGET_OK)
{
CPhidget_getErrorDescription(err, &errStr);
printf("Error waiting for attachment IFK1: (%d): %s\n",err,errStr);
return 0;
}
//pour faire marcher les boutons stop et inhibit
CPhidgetInterfaceKit_setOutputState(IFK,true_inhibit_number, 1);
return 1;
}
VALUE interfacekit_close(VALUE self) {
PhidgetInfo *info = device_info(self);
ensure(CPhidgetInterfaceKit_set_OnInputChange_Handler((CPhidgetInterfaceKitHandle) info->handle, NULL, NULL));
ensure(CPhidgetInterfaceKit_set_OnSensorChange_Handler((CPhidgetInterfaceKitHandle) info->handle, NULL, NULL));
return rb_call_super(0,NULL);
}
void unsetOnChangeHandler(CPhidgetInterfaceKitHandle ifk, onChangeType t) {
switch (t) {
case inputChanged:
CPhidgetInterfaceKit_set_OnInputChange_Handler(ifk, NULL, NULL);
case outputChanged:
CPhidgetInterfaceKit_set_OnOutputChange_Handler(ifk, NULL, NULL);
case sensorChanged:
CPhidgetInterfaceKit_set_OnSensorChange_Handler(ifk, NULL, NULL);
}
}
int setOnChangeHandler(CPhidgetInterfaceKitHandle ifk, handler *h, onChangeType t) {
switch (t) {
case inputChanged:
return CPhidgetInterfaceKit_set_OnInputChange_Handler(ifk, &onChangeHandler, h);
case outputChanged:
return CPhidgetInterfaceKit_set_OnOutputChange_Handler(ifk, &onChangeHandler, h);
case sensorChanged:
return CPhidgetInterfaceKit_set_OnSensorChange_Handler(ifk, &onChangeHandler, h);
}
}
bool attach(
CPhidgetInterfaceKitHandle &phid,
int serial_number)
{
CPhidget_enableLogging(PHIDGET_LOG_VERBOSE, "/home/motters/Desktop/phidgets.log");
//create the object
CPhidgetInterfaceKit_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 an arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetInterfaceKit_set_OnInputChange_Handler (phid, 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 (phid, 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 (phid, OutputChangeHandler, NULL);
//open the device for connections
CPhidget_open((CPhidgetHandle)phid, serial_number);
//get the program to wait for an interface kit device to be attached
if (serial_number == -1) {
ROS_INFO("Waiting for Interface Kit Phidget to be attached....");
}
else {
ROS_INFO("Waiting for Interface Kit 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 return true;
}
VALUE interfacekit_initialize(VALUE self, VALUE serial) {
PhidgetInfo *info = device_info(self);
InterfaceKitInfo *ifkit_info = ALLOC(InterfaceKitInfo);
memset(ifkit_info, 0, sizeof(InterfaceKitInfo));
ifkit_info->is_data_rates_known = false;
ifkit_info->is_dual_ratiometric_mode = false;
CPhidgetInterfaceKitHandle interfacekit = 0;
ensure(CPhidgetInterfaceKit_create(&interfacekit));
info->handle = (CPhidgetHandle)interfacekit;
info->on_type_attach = interfacekit_on_attach;
info->on_type_detach = interfacekit_on_detach;
info->on_type_free = interfacekit_on_free;
info->type_info = ifkit_info;
ensure(CPhidgetInterfaceKit_set_OnInputChange_Handler(interfacekit, interfacekit_on_digital_change, info));
ensure(CPhidgetInterfaceKit_set_OnSensorChange_Handler(interfacekit, interfacekit_on_analog_change, info));
return rb_call_super(1, &serial);
}
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 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 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 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 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;
}
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;
}