/** @brief Function that will use use the sonar to later acquire their value
*/
int sendSonarResult()
{
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 1);
// sleep for 2ms
ros::Duration(0.002).sleep();
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
}
int main(int argc, char* argv[]) {
// Set up a SIGINT handler.
signal(SIGINT, catch_sigint);
// Create the InterfaceKit object.
CPhidgetInterfaceKitHandle ifKit = 0;
CPhidgetInterfaceKit_create(&ifKit);
// Register device handlers.
CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifKit, attach_handler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, detach_handler, NULL);
CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, error_handler, NULL);
// Open the interfacekit for device connections.
CPhidget_open((CPhidgetHandle)ifKit, -1);
// Wait for a device attachment.
fprintf(stderr, "waiting for interface kit to be attached....\n");
int result;
const char *err;
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000))) {
CPhidget_getErrorDescription(result, &err);
fprintf(stderr, "problem waiting for attachment: %s\n", err);
return 0;
}
// Check some properties of the device.
const char *device_type;
int num_outputs;
CPhidget_getDeviceType((CPhidgetHandle)ifKit, &device_type);
CPhidgetInterfaceKit_getOutputCount(ifKit, &num_outputs);
if ((strcmp(device_type, "PhidgetInterfaceKit") != 0)) {
fprintf(stderr, "unexpected device type: %s\n", device_type);
return 1;
}
if (num_outputs != 4) {
fprintf(stderr, "unexpected number of device outputs: %d\n", num_outputs);
return 1;
}
while (1) {
double time = get_time_in_seconds();
double factor = 1;
int l0_state = fmod(time * factor, 20) < 10;
int l1_state = fmod(time * factor, 20) >= 10;
CPhidgetInterfaceKit_setOutputState(ifKit, 0, l0_state);
CPhidgetInterfaceKit_setOutputState(ifKit, 2, l1_state);
usleep(100000);
}
fprintf(stderr, "shutting down...\n");
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
return 0;
}
//-------------BUZZER---------------
int Buzzer(int in)
/*--------------------
mise en route ou arret des buzzer par 1 ou 0 dans in
--------------------*/
{
if(in==1)CPhidgetInterfaceKit_setOutputState(IFK,buzzer_number, 1);
else
CPhidgetInterfaceKit_setOutputState(IFK,buzzer_number, 0);
return 0;
}
int main(int argc, char* argv[]) {
int result, numOutputs = 3, state = 0;
const char *err, *ptr;
CPhidgetInterfaceKitHandle phid = 0;
LocalErrorCatcher(CPhidgetInterfaceKit_create(&phid));
LocalErrorCatcher(CPhidget_set_OnAttach_Handler((CPhidgetHandle) phid, AttachHandler, NULL));
LocalErrorCatcher(CPhidget_set_OnDetach_Handler((CPhidgetHandle) phid, DetachHandler, NULL));
LocalErrorCatcher(CPhidget_set_OnError_Handler((CPhidgetHandle) phid, ErrorHandler, NULL));
// printf("Phidget Simple Playground (plug and unplug devices)\n");
// printf("Opening...\n");
LocalErrorCatcher(CPhidget_open((CPhidgetHandle) phid, -1));
LocalErrorCatcher(CPhidget_waitForAttachment((CPhidgetHandle) phid, 100000));
CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
// printf("%s\n", ptr);
printf("Turning power on\n");
CPhidgetInterfaceKit_setOutputState(phid, 0, 1);
printf("done\n");
LocalErrorCatcher(CPhidget_close((CPhidgetHandle) phid));
LocalErrorCatcher(CPhidget_delete((CPhidgetHandle) phid));
return 0;
}
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_output_set(VALUE self, VALUE index, VALUE is_on) {
PhidgetInfo *info = device_info(self);
InterfaceKitInfo *ifkit_info = info->type_info;
if (TYPE(index) != T_FIXNUM)
rb_raise(rb_eTypeError, MSG_OUTPUT_INDEX_MUST_BE_FIXNUM);
int index_int = FIX2INT(index);
if ((ifkit_info->digital_output_count == 0) || (index_int > (ifkit_info->digital_output_count-1)))
rb_raise(rb_eTypeError, MSG_OUTPUT_INDEX_TOO_HIGH);
int phidget_bool;
if (TYPE(is_on) == T_TRUE)
phidget_bool = PTRUE;
else if (TYPE(is_on) == T_FALSE)
phidget_bool = PFALSE;
else
rb_raise(rb_eTypeError, MSG_OUTPUT_VALUE_MUST_BE_BOOL);
ensure(CPhidgetInterfaceKit_setOutputState(
(CPhidgetInterfaceKitHandle)info->handle, index_int, phidget_bool));
ifkit_info->digital_output_states[index_int] = phidget_bool;
return is_on;
}
int Stop_Colonne_24V()
{
int GREEN,YELLOW,RED;
if(port_24V_number==-1)
{
printf("24V port has not been initialized\n");
return -1;
}
status_24V=-1;
GREEN=colonne_lumineuse[2];
YELLOW=colonne_lumineuse[3];
RED=colonne_lumineuse[4];
CPhidgetInterfaceKit_setOutputState(IFK2,port_24V_number,0);
CPhidgetInterfaceKit_setOutputState(IFK2,GREEN,0);
CPhidgetInterfaceKit_setOutputState(IFK2,YELLOW,0);
CPhidgetInterfaceKit_setOutputState(IFK2,RED,0);
return 1;
}
/** @brief Function that will manage the sonars and acquire their data
* this function is called every around 50ms
*/
int sendSonarResult()
{
corobot_msgs::RangeSensor data;
data.type = data.ULTRASOUND;
data.numberSensors = lastSonarInput + 1 - firstSonarInput;
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 1);
ros::Duration(0.002).sleep(); // sleep for 2ms
CPhidgetInterfaceKit_setOutputState(ifKit, strobeOutput, 0);
//Acquire the data and transform them into values in meters
for(int i = firstSonarInput; i<= lastSonarInput;i++)
{
data.range.push_back(sonarVoltageToMeters(analog_inputs_value[i]));
}
sonar_pub.publish(data);
return 0;
}
void setState(bool state) {
if (state != mState) {
mState = state;
if (mIfk && mType == QPDigitalIO::Output) {
CPhidgetInterfaceKit_setOutputState(mIfk, mIndex, state);
}
emit self->stateChanged(state);
}
}
//----------BOUTON STOP ET INHIBIT---------
int Init_Bouton_Stop()
/*-------------
Initialisation pour utiliser les boutons
-------------*/
{
if(true_inhibit_number==-1)return -1;
CPhidgetInterfaceKit_setOutputState(IFK,true_inhibit_number, 1);
return 1;
}
void WidgetsInterfaceKit::OnWtOutputStateChanged(Wt::WCheckBox* checkbox)
{
int i;
for (i=0; i<m_output_checkbox_array_length; i++)
{
if (checkbox == m_output_checkbox_array[i])
{
bool check = checkbox->isChecked();
CPhidgetInterfaceKit_setOutputState(m_phidget->GetNativeHandle(), i, check ? PTRUE : PFALSE);
::GetApplicationManager()->OnWtDigitalOutputChanged(GetApplication(), GetSerial(), i, check);
break;
}
}
}
int Set_Colonne_Lumineuse_GYR(int green1,int yellow1,int red1)
/*-----------------
allumage et extinction de la colonne lumineuse (1=allumage,0=extinction)
Doit etre precede par Init_Colonne_24V()
----------------*/
{
int GREEN,YELLOW,RED;
int green,yellow,red;
if(status_24V!=1)
{
printf("Cannot use light column as 24V is not on\n");
return -1;
}
if(red1!=0)red=1; else red=0;
if(green1!=0)green=1; else green=0;
if(yellow1!=0)yellow=1; else yellow=0;
GREEN=colonne_lumineuse[2];
YELLOW=colonne_lumineuse[3];
RED=colonne_lumineuse[4];
CPhidgetInterfaceKit_setOutputState(IFK2,GREEN,green);
CPhidgetInterfaceKit_setOutputState(IFK2,YELLOW,yellow);
CPhidgetInterfaceKit_setOutputState(IFK2,RED,red);
return 1;
}
//---COLONNE LUMINEUSE---------------
int Init_Colonne_24V()
{
int i,k1;
if(port_24V_number==-1)
{
printf("24V port has not been initialized\n");
return -1;
}
CPhidgetInterfaceKit_setOutputState(IFK2,port_24V_number,1);
sleep(1);
//verification que le output est bien passe a 1. Parfois il est necessaire
//de faire plusieurs passage 0-1 pour s'en assurer
CPhidgetInterfaceKit_getOutputState(IFK2,port_24V_number,&k1);
if(k1==0)
{
for(i=0;i<4;i++)
{
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);
if(k1==1)break;
}
if(i==4)
{
printf("Cannot initiate 24V for light column\n");
status_24V=-1;
return -1;
}
}
status_24V=1;
return 1;
}
/*!
* \brief set digital output state or sensor trigger level
* \param req requested parameters
* \param res returned parameters
*/
bool set_values(
phidgets::interface_kit::Request &req,
phidgets::interface_kit::Response &res)
{
switch(req.value_type)
{
case 1: { // set digital output
CPhidgetInterfaceKit_setOutputState (phid, req.index, req.value);
ROS_INFO("Output %d State %d", req.index, req.value);
break;
}
case 2: { // set sensor trigger level
CPhidgetInterfaceKit_setSensorChangeTrigger(phid, req.index, req.value);
ROS_INFO("Sensor %d Trigger level %d", req.index, req.value);
break;
}
}
res.ack = 1;
return(true);
}
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 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 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()
// 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;
}
